3.2.0

CMakeLists.txt

@@ -13,8 +13,11 @@ 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_DIR}/ex_servo.py
     DESTINATION lib/${PROJECT_NAME}

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_gripper.py

@@ -39,7 +39,6 @@ def main():
         closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
         gripper_group_name=panda.MOVE_GROUP_GRIPPER,
         callback_group=callback_group,
-        follow_joint_trajectory_action_name="gripper_trajectory_controller/follow_joint_trajectory",
         gripper_command_action_name="gripper_action_controller/gripper_cmd",
     )
 

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_joint_goal.py

@@ -2,6 +2,8 @@
 """
 Example of moving to a joint configuration.
 - ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
+- ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False -p cancel_after_secs:=1.0
+- ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False -p cancel_after_secs:=0.0
 """
 
 from threading import Thread
@@ -10,7 +12,7 @@
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 
-from pymoveit2 import MoveIt2
+from pymoveit2 import MoveIt2, MoveIt2State
 from pymoveit2.robots import panda
 
 
@@ -33,6 +35,11 @@ def main():
             0.7853981633974483,
         ],
     )
+    node.declare_parameter("synchronous", True)
+    # If non-positive, don't cancel. Only used if synchronous is False
+    node.declare_parameter("cancel_after_secs", 0.0)
+    # Planner ID
+    node.declare_parameter("planner_id", "RRTConnectkConfigDefault")
 
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
@@ -46,6 +53,9 @@ def main():
         group_name=panda.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
+    moveit2.planner_id = (
+        node.get_parameter("planner_id").get_parameter_value().string_value
+    )
 
     # Spin the node in background thread(s) and wait a bit for initialization
     executor = rclpy.executors.MultiThreadedExecutor(2)
@@ -58,15 +68,49 @@ def main():
     moveit2.max_velocity = 0.5
     moveit2.max_acceleration = 0.5
 
-    # Get parameter
+    # 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
+    cancel_after_secs = (
+        node.get_parameter("cancel_after_secs").get_parameter_value().double_value
+    )
 
     # Move to joint configuration
     node.get_logger().info(f"Moving to {{joint_positions: {list(joint_positions)}}}")
     moveit2.move_to_configuration(joint_positions)
-    moveit2.wait_until_executed()
+    if synchronous:
+        # Note: the same functionality can be achieved by setting
+        # `synchronous:=false` and `cancel_after_secs` to a negative value.
+        moveit2.wait_until_executed()
+    else:
+        # Wait for the request to get accepted (i.e., for execution to start)
+        print("Current State: " + str(moveit2.query_state()))
+        rate = node.create_rate(10)
+        while moveit2.query_state() != MoveIt2State.EXECUTING:
+            rate.sleep()
+
+        # Get the future
+        print("Current State: " + str(moveit2.query_state()))
+        future = moveit2.get_execution_future()
+
+        # Cancel the goal
+        if cancel_after_secs > 0.0:
+            # Sleep for the specified time
+            sleep_time = node.create_rate(cancel_after_secs)
+            sleep_time.sleep()
+            # Cancel the goal
+            print("Cancelling goal")
+            moveit2.cancel_execution()
+
+        # Wait until the future is done
+        while not future.done():
+            rate.sleep()
+
+        # Print the result
+        print("Result status: " + str(future.result().status))
+        print("Result error code: " + str(future.result().result.error_code))
 
     rclpy.shutdown()
     executor_thread.join()