Added Async Forward/Inverse Kinematics (#43)

* [WIP] Add async service call for FK/IK analogous to planning

* Compute FK returns a list of post_stampeds

* Pre-commit formatting

* Added FK example

* Comment changes to orientation path constraint examples

* Added IK example

* Update examples/ex_fk.py

Co-authored-by: Andrej Orsula <[email protected]>

* Update examples/ex_ik.py

Co-authored-by: Andrej Orsula <[email protected]>

* Update moveit2.py

---------

Co-authored-by: Ethan Gordon <[email protected]>
Co-authored-by: Andrej Orsula <[email protected]>

CMakeLists.txt

@@ -13,7 +13,9 @@ set(EXAMPLES_DIR examples)
 install(PROGRAMS
     ${EXAMPLES_DIR}/ex_collision_mesh.py
     ${EXAMPLES_DIR}/ex_collision_primitive.py
+    ${EXAMPLES_DIR}/ex_fk.py
     ${EXAMPLES_DIR}/ex_gripper.py
+    ${EXAMPLES_DIR}/ex_ik.py
     ${EXAMPLES_DIR}/ex_joint_goal.py
     ${EXAMPLES_DIR}/ex_orientation_path_constraint.py
     ${EXAMPLES_DIR}/ex_pose_goal.py

examples/ex_fk.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+"""
+Example of computing Forward Kinematics.
+- ros2 run pymoveit2 ex_fk.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
+- ros2 run pymoveit2 ex_fk.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False
+"""
+
+from threading import Thread
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+from pymoveit2 import MoveIt2, MoveIt2State
+from pymoveit2.robots import panda
+
+
+def main():
+    rclpy.init()
+
+    # Create node for this example
+    node = Node("ex_fk")
+
+    # Declare parameter for joint positions
+    node.declare_parameter(
+        "joint_positions",
+        [
+            0.0,
+            0.0,
+            0.0,
+            -0.7853981633974483,
+            0.0,
+            1.5707963267948966,
+            0.7853981633974483,
+        ],
+    )
+    node.declare_parameter("synchronous", True)
+
+    # Create callback group that allows execution of callbacks in parallel without restrictions
+    callback_group = ReentrantCallbackGroup()
+
+    # Create MoveIt 2 interface
+    moveit2 = MoveIt2(
+        node=node,
+        joint_names=panda.joint_names(),
+        base_link_name=panda.base_link_name(),
+        end_effector_name=panda.end_effector_name(),
+        group_name=panda.MOVE_GROUP_ARM,
+        callback_group=callback_group,
+    )
+
+    # Spin the node in background thread(s) and wait a bit for initialization
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(node)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+    node.create_rate(1.0).sleep()
+
+    # Get parameters
+    joint_positions = (
+        node.get_parameter("joint_positions").get_parameter_value().double_array_value
+    )
+    synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value
+
+    # Move to joint configuration
+    node.get_logger().info(
+        f"Computing FK for {{joint_positions: {list(joint_positions)}}}"
+    )
+    retval = None
+    if synchronous:
+        retval = moveit2.compute_fk(joint_positions)
+    else:
+        future = moveit2.compute_fk_async(joint_positions)
+        if future is not None:
+            rate = node.create_rate(10)
+            while not future.done():
+                rate.sleep()
+            retval = moveit2.get_compute_fk_result(future)
+    if retval is None:
+        print("Failed.")
+    else:
+        print("Succeeded. Result: " + str(retval))
+
+    rclpy.shutdown()
+    executor_thread.join()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()

examples/ex_ik.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+"""
+Example of computing Inverse Kinematics.
+- ros2 run pymoveit2 ex_ik.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"
+- ros2 run pymoveit2 ex_ik.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p synchronous:=False
+"""
+
+from threading import Thread
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+from pymoveit2 import MoveIt2, MoveIt2State
+from pymoveit2.robots import panda
+
+
+def main():
+    rclpy.init()
+
+    # Create node for this example
+    node = Node("ex_ik")
+
+    # Declare parameters for position and orientation
+    node.declare_parameter("position", [0.5, 0.0, 0.25])
+    node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0])
+    node.declare_parameter("synchronous", True)
+
+    # Create callback group that allows execution of callbacks in parallel without restrictions
+    callback_group = ReentrantCallbackGroup()
+
+    # Create MoveIt 2 interface
+    moveit2 = MoveIt2(
+        node=node,
+        joint_names=panda.joint_names(),
+        base_link_name=panda.base_link_name(),
+        end_effector_name=panda.end_effector_name(),
+        group_name=panda.MOVE_GROUP_ARM,
+        callback_group=callback_group,
+    )
+
+    # Spin the node in background thread(s) and wait a bit for initialization
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(node)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+    node.create_rate(1.0).sleep()
+
+    # Get parameters
+    position = node.get_parameter("position").get_parameter_value().double_array_value
+    quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value
+    synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value
+
+    # Move to joint configuration
+    node.get_logger().info(
+        f"Computing IK for {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}"
+    )
+    retval = None
+    if synchronous:
+        retval = moveit2.compute_ik(position, quat_xyzw)
+    else:
+        future = moveit2.compute_ik_async(position, quat_xyzw)
+        if future is not None:
+            rate = node.create_rate(10)
+            while not future.done():
+                rate.sleep()
+            retval = moveit2.get_compute_ik_result(future)
+    if retval is None:
+        print("Failed.")
+    else:
+        print("Succeeded. Result: " + str(retval))
+
+    rclpy.shutdown()
+    executor_thread.join()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()

examples/ex_orientation_path_constraint.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 """
-Example of moving to a joint configuration.
+Example of moving to a joint configuration with orientation path constraints.
 - ros2 run pymoveit2 ex_orientation_path_constraints.py --ros-args -p use_orientation_constraint:=True
 - ros2 run pymoveit2 ex_orientation_path_constraints.py --ros-args -p use_orientation_constraint:=False
 """
@@ -19,7 +19,7 @@ def main():
     rclpy.init()
 
     # Create node for this example
-    node = Node("ex_joint_goal")
+    node = Node("ex_orientation_path_constraints")
 
     # Declare parameter for joint positions
     node.declare_parameter(

pymoveit2/moveit2.py

@@ -479,7 +479,7 @@ def plan(
         if future is None:
             return None
 
-        # 10ms sleep
+        # 100ms sleep
         rate = self._node.create_rate(10)
         while not future.done():
             rate.sleep()
@@ -1120,8 +1120,57 @@ def compute_fk(
         self,
         joint_state: Optional[Union[JointState, List[float]]] = None,
         fk_link_names: Optional[List[str]] = None,
-        wait_for_server_timeout_sec: Optional[float] = 1.0,
-    ) -> Optional[PoseStamped]:
+    ) -> Optional[Union[PoseStamped, List[PoseStamped]]]:
+        """
+        Call compute_fk_async and wait on future
+        """
+        future = self.compute_fk_async(
+            **{key: value for key, value in locals().items() if key != "self"}
+        )
+
+        if future is None:
+            return None
+
+        # 100ms sleep
+        rate = self._node.create_rate(10)
+        while not future.done():
+            rate.sleep()
+
+        return self.get_compute_fk_result(future, fk_link_names=fk_link_names)
+
+    def get_compute_fk_result(
+        self,
+        future: Future,
+        fk_link_names: Optional[List[str]] = None,
+    ) -> Optional[Union[PoseStamped, List[PoseStamped]]]:
+        """
+        Takes in a future returned by compute_fk_async and returns the poses
+        if the future is done and successful, else None.
+        """
+        if not future.done():
+            self._node.get_logger().warn(
+                "Cannot get FK result because future is not done."
+            )
+            return None
+
+        res = future.result()
+
+        if MoveItErrorCodes.SUCCESS == res.error_code.val:
+            if fk_link_names is None:
+                return res.pose_stamped[0]
+            else:
+                return res.pose_stamped
+        else:
+            self._node.get_logger().warn(
+                f"FK computation failed! Error code: {res.error_code.val}."
+            )
+            return None
+
+    def compute_fk_async(
+        self,
+        joint_state: Optional[Union[JointState, List[float]]] = None,
+        fk_link_names: Optional[List[str]] = None,
+    ) -> Optional[Future]:
         """
         Compute forward kinematics for all `fk_link_names` in a given `joint_state`.
           - `fk_link_names` defaults to end-effector
@@ -1150,36 +1199,71 @@ def compute_fk(
         stamp = self._node.get_clock().now().to_msg()
         self.__compute_fk_req.header.stamp = stamp
 
-        if not self.__compute_fk_client.wait_for_service(
-            timeout_sec=wait_for_server_timeout_sec
-        ):
+        if not self.__compute_fk_client.service_is_ready():
             self._node.get_logger().warn(
                 f"Service '{self.__compute_fk_client.srv_name}' is not yet available. Better luck next time!"
             )
             return None
 
-        res = self.__compute_fk_client.call(self.__compute_fk_req)
+        return self.__compute_fk_client.call_async(self.__compute_fk_req)
+
+    def compute_ik(
+        self,
+        position: Union[Point, Tuple[float, float, float]],
+        quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
+        start_joint_state: Optional[Union[JointState, List[float]]] = None,
+        constraints: Optional[Constraints] = None,
+        wait_for_server_timeout_sec: Optional[float] = 1.0,
+    ) -> Optional[JointState]:
+        """
+        Call compute_ik_async and wait on future
+        """
+        future = self.compute_ik_async(
+            **{key: value for key, value in locals().items() if key != "self"}
+        )
+
+        if future is None:
+            return None
+
+        # 10ms sleep
+        rate = self._node.create_rate(10)
+        while not future.done():
+            rate.sleep()
+
+        return self.get_compute_ik_result(future)
+
+    def get_compute_ik_result(
+        self,
+        future: Future,
+    ) -> Optional[JointState]:
+        """
+        Takes in a future returned by compute_ik_async and returns the joint states
+        if the future is done and successful, else None.
+        """
+        if not future.done():
+            self._node.get_logger().warn(
+                "Cannot get IK result because future is not done."
+            )
+            return None
+
+        res = future.result()
 
         if MoveItErrorCodes.SUCCESS == res.error_code.val:
-            pose_stamped = res.pose_stamped
-            if isinstance(pose_stamped, List):
-                return res.pose_stamped[0]
-            else:
-                return res.pose_stamped
+            return res.solution.joint_state
         else:
             self._node.get_logger().warn(
-                f"FK computation failed! Error code: {res.error_code.val}."
+                f"IK computation failed! Error code: {res.error_code.val}."
             )
             return None
 
-    def compute_ik(
+    def compute_ik_async(
         self,
         position: Union[Point, Tuple[float, float, float]],
         quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
         start_joint_state: Optional[Union[JointState, List[float]]] = None,
         constraints: Optional[Constraints] = None,
         wait_for_server_timeout_sec: Optional[float] = 1.0,
-    ) -> Optional[JointState]:
+    ) -> Optional[Future]:
         """
         Compute inverse kinematics for the given pose. To indicate beginning of the search space,
         `start_joint_state` can be specified. Furthermore, `constraints` can be imposed on the
@@ -1248,15 +1332,7 @@ def compute_ik(
             )
             return None
 
-        res = self.__compute_ik_client.call(self.__compute_ik_req)
-
-        if MoveItErrorCodes.SUCCESS == res.error_code.val:
-            return res.solution.joint_state
-        else:
-            self._node.get_logger().warn(
-                f"IK computation failed! Error code: {res.error_code.val}."
-            )
-            return None
+        return self.__compute_ik_client.call_async(self.__compute_ik_req)
 
     def reset_new_joint_state_checker(self):
         """