3.2.0 (#41)

* Add asynchronous planning and execution (#40) * Add joint goal example for Kinova JACO2 * [WIP] Added execution cancellation and polling * Switch to ExecuteTrajectory action * [WIP] Goal cancellation is broken * Added cancellation via topic publication * Full cancellation example * Need option for both move action and direct planning * Created get_trajectory, so users of plan_async can easily get the result * Reset last error code before action execution Consider two cases, one where action server (either for execute or MoveGroup) is not available, and another where the action succeeds very fast. Both cases are currently indistinguishable from the client perspective, because they will request a goal, and then when they query the state it will be IDLE. This commit resolves that, because if the error code is set that means the action completed very fast, whereas if it is None that means the action did not complete. * Reverted to original example * Small fixes from rebase * Update examples * Update docstrings * Update docstrings * Addressed PR comments * Ran pre-commit hook * Addressed PR comments * Fixed gripper interface --------- Co-authored-by: Ethan K. Gordon <[email protected]> * Add Path Constraints (#42) * [WIP] Add ability to do path constraints * [WIP] Change API to be more intelligible * Allowed different orientation tolerances per axes * Make change not breaking by adding float option * Added parameterization option * Updated set_pose_goal * Rearranged parameterization to not be a breaking change * Formatting changes form pre-commit * Add orientation path constraint example * Reused constraint creation code from goal constraints * Pre-commit formatting fix --------- Co-authored-by: Ethan Gordon <[email protected]> * 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]> * Allow users to set `planner_id` and `pipeline_id` (#48) * Added set_planner_id * Make planner_id and pipeline_id properties * Add planner_id param to example files * Formatting --------- Co-authored-by: Amal Nanavati <[email protected]> Co-authored-by: Ethan K. Gordon <[email protected]>
AndrejOrsula · Mar 6, 2024 · aaac98f · aaac98f
1 parent 5234a7e
commit aaac98f
Show file tree

Hide file tree

Showing 11 changed files with 1,004 additions and 217 deletions.
diff --git a/CMakeLists.txt b/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}

diff --git a/examples/ex_fk.py b/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()
diff --git a/examples/ex_gripper.py b/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",
     )
 

diff --git a/examples/ex_ik.py b/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()
diff --git a/examples/ex_joint_goal.py b/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()