diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a88115..43d5fc3 100644 --- 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 new file mode 100755 index 0000000..b08ea26 --- /dev/null +++ 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 index 28961f7..2af8906 100755 --- 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 new file mode 100755 index 0000000..098a81d --- /dev/null +++ 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 index 45e97b2..d756138 100755 --- 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() diff --git a/examples/ex_orientation_path_constraint.py b/examples/ex_orientation_path_constraint.py new file mode 100755 index 0000000..e2efabd --- /dev/null +++ b/examples/ex_orientation_path_constraint.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +""" +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 +""" + +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_orientation_path_constraints") + + # Declare parameter for joint positions + node.declare_parameter( + "initial_joint_positions", + [1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854], + ) + node.declare_parameter( + "goal_joint_positions", + [ + 1.1967347888074664, + -1.3973650345565432, + 0.1342628652196778, + -2.508581053909259, + -2.412084037045761, + 0.5267181456160516, + 1.3027041597804059, + ], + ) + node.declare_parameter("use_orientation_constraint", True) + node.declare_parameter( + "orientation_constraint_quaternion", + [ + 0.5, + -0.5, + 0.5, + 0.5, + ], + ) + node.declare_parameter( + "orientation_constraint_tolerance", + [ + 3.14159, + 0.5, + 0.5, + ], + ) + node.declare_parameter("orientation_constraint_parameterization", 1) + + # 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() + + # Scale down velocity and acceleration of joints (percentage of maximum) + moveit2.max_velocity = 0.5 + moveit2.max_acceleration = 0.5 + + # Get parameters + initial_joint_positions = ( + node.get_parameter("initial_joint_positions") + .get_parameter_value() + .double_array_value + ) + goal_joint_positions = ( + node.get_parameter("goal_joint_positions") + .get_parameter_value() + .double_array_value + ) + use_orientation_constraint = ( + node.get_parameter("use_orientation_constraint") + .get_parameter_value() + .bool_value + ) + orientation_constraint_quaternion = ( + node.get_parameter("orientation_constraint_quaternion") + .get_parameter_value() + .double_array_value + ) + orientation_constraint_tolerance = ( + node.get_parameter("orientation_constraint_tolerance") + .get_parameter_value() + .double_array_value + ) + orientation_constraint_parameterization = ( + node.get_parameter("orientation_constraint_parameterization") + .get_parameter_value() + .integer_value + ) + + # Move to initial joint configuration + node.get_logger().info( + f"Moving to {{joint_positions: {list(initial_joint_positions)}}}" + ) + moveit2.move_to_configuration(initial_joint_positions) + moveit2.wait_until_executed() + + # Set orientation path constraint + if use_orientation_constraint: + node.get_logger().info(f"Setting orientation path constraint") + moveit2.set_path_orientation_constraint( + quat_xyzw=orientation_constraint_quaternion, + tolerance=orientation_constraint_tolerance, + parameterization=orientation_constraint_parameterization, + ) + + # Move to goal joint configuration + node.get_logger().info( + f"Moving to {{joint_positions: {list(goal_joint_positions)}}}" + ) + moveit2.move_to_configuration(goal_joint_positions) + moveit2.wait_until_executed() + + # Shutdown + rclpy.shutdown() + executor_thread.join() + exit(0) + + +if __name__ == "__main__": + main() diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index 4a76569..401fd19 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -2,6 +2,8 @@ """ Example of moving to a pose goal. - ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False +- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=1.0 +- ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -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 @@ -24,6 +26,11 @@ def main(): 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("cartesian", False) + 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() @@ -37,6 +44,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) @@ -53,13 +63,47 @@ def main(): position = node.get_parameter("position").get_parameter_value().double_array_value quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value cartesian = node.get_parameter("cartesian").get_parameter_value().bool_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 pose node.get_logger().info( f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" ) moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=cartesian) - 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() diff --git a/pymoveit2/__init__.py b/pymoveit2/__init__.py index c10e6d6..6c54a9d 100644 --- a/pymoveit2/__init__.py +++ b/pymoveit2/__init__.py @@ -1,6 +1,6 @@ from . import robots from .gripper_command import GripperCommand from .gripper_interface import GripperInterface -from .moveit2 import MoveIt2 +from .moveit2 import MoveIt2, MoveIt2State from .moveit2_gripper import MoveIt2Gripper from .moveit2_servo import MoveIt2Servo diff --git a/pymoveit2/gripper_interface.py b/pymoveit2/gripper_interface.py index 7ce5b5f..0503805 100644 --- a/pymoveit2/gripper_interface.py +++ b/pymoveit2/gripper_interface.py @@ -26,14 +26,26 @@ def __init__( skip_planning_fixed_motion_duration: float = 0.5, max_effort: float = 0.0, callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "gripper_trajectory_controller/follow_joint_trajectory", + follow_joint_trajectory_action_name: str = "DEPRECATED", gripper_command_action_name: str = "gripper_action_controller/gripper_cmd", + use_move_group_action: bool = False, ): """ Combination of `MoveIt2Gripper` and `GripperCommand` interfaces that automatically selects the appropriate interface based on the available actions. """ + # Check for deprecated parameters + if execute_via_moveit: + node.get_logger().warn( + "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." + ) + use_move_group_action = True + if follow_joint_trajectory_action_name != "DEPRECATED": + node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + MoveIt2Gripper.__init__( self=self, node=node, @@ -41,12 +53,11 @@ def __init__( open_gripper_joint_positions=open_gripper_joint_positions, closed_gripper_joint_positions=closed_gripper_joint_positions, gripper_group_name=gripper_group_name, - execute_via_moveit=execute_via_moveit, ignore_new_calls_while_executing=ignore_new_calls_while_executing, skip_planning=skip_planning, skip_planning_fixed_motion_duration=skip_planning_fixed_motion_duration, callback_group=callback_group, - follow_joint_trajectory_action_name=follow_joint_trajectory_action_name, + use_move_group_action=use_move_group_action, ) GripperCommand.__init__( @@ -66,7 +77,7 @@ def __init__( def __determine_interface(self, timeout_sec=1.0): if self.gripper_command_action_client.wait_for_server(timeout_sec=timeout_sec): self._interface = GripperCommand - elif self.follow_joint_trajectory_action_client.wait_for_server( + elif self._execute_trajectory_action_client.wait_for_server( timeout_sec=timeout_sec ): self._interface = MoveIt2Gripper diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index eac7bc0..6d7684e 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1,10 +1,10 @@ import threading +from enum import Enum from typing import List, Optional, Tuple, Union from action_msgs.msg import GoalStatus -from control_msgs.action import FollowJointTrajectory from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion -from moveit_msgs.action import MoveGroup +from moveit_msgs.action import ExecuteTrajectory, MoveGroup from moveit_msgs.msg import ( AttachedCollisionObject, CollisionObject, @@ -29,12 +29,28 @@ QoSProfile, QoSReliabilityPolicy, ) +from rclpy.task import Future from sensor_msgs.msg import JointState from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive -from std_msgs.msg import Header +from std_msgs.msg import Header, String from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +class MoveIt2State(Enum): + """ + An enum the represents the current execution state of the MoveIt2 interface. + - IDLE: No motion is being requested or executed + - REQUESTING: Execution has been requested, but the request has not yet been + accepted. + - EXECUTING: Execution has been requested and accepted, and has not yet been + completed. + """ + + IDLE = 0 + REQUESTING = 1 + EXECUTING = 2 + + class MoveIt2: """ Python interface for MoveIt 2 that enables planning and execution of trajectories. @@ -51,7 +67,8 @@ def __init__( execute_via_moveit: bool = False, ignore_new_calls_while_executing: bool = False, callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "joint_trajectory_controller/follow_joint_trajectory", + follow_joint_trajectory_action_name: str = "DEPRECATED", + use_move_group_action: bool = False, ): """ Construct an instance of `MoveIt2` interface. @@ -60,18 +77,32 @@ def __init__( - `base_link_name` - Name of the robot base link - `end_effector_name` - Name of the robot end effector - `group_name` - Name of the planning group for robot arm - - `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) FollowJointTrajectory action (controller) is employed otherwise together with a separate planning service client - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories while previous is still being executed - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - `follow_joint_trajectory_action_name` - Name of the action server for the controller + - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client """ self._node = node self._callback_group = callback_group + # Check for deprecated parameters + if execute_via_moveit: + self._node.get_logger().warn( + "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." + ) + use_move_group_action = True + if follow_joint_trajectory_action_name != "DEPRECATED": + self._node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + # Create subscriber for current joint states self._node.create_subscription( msg_type=JointState, @@ -124,7 +155,7 @@ def __init__( callback_group=self._callback_group, ) - # Otherwise create a separate service client for planning + # Also create a separate service client for planning self._plan_kinematic_path_service = self._node.create_client( srv_type=GetMotionPlan, srv_name="plan_kinematic_path", @@ -153,10 +184,10 @@ def __init__( self.__cartesian_path_request = GetCartesianPath.Request() # Create action client for trajectory execution - self.__follow_joint_trajectory_action_client = ActionClient( + self._execute_trajectory_action_client = ActionClient( node=self._node, - action_type=FollowJointTrajectory, - action_name=follow_joint_trajectory_action_name, + action_type=ExecuteTrajectory, + action_name="execute_trajectory", goal_service_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, @@ -197,6 +228,10 @@ def __init__( AttachedCollisionObject, "/attached_collision_object", 10 ) + self.__cancellation_pub = self._node.create_publisher( + String, "/trajectory_execution_event", 1 + ) + self.__joint_state_mutex = threading.Lock() self.__joint_state = None self.__new_joint_state_available = False @@ -206,11 +241,12 @@ def __init__( end_effector=end_effector_name, ) - # Flag to determine whether to execute trajectories via MoveIt2, or rather by calling a separate action with the controller itself - # Applies to `move_to_pose()` and `move_to_configuraion()` - self.__execute_via_moveit = execute_via_moveit + # Flag to determine whether to execute trajectories via Move Group Action, or rather by calling + # the separate ExecuteTrajectory action + # Applies to `move_to_pose()` and `move_to_configuration()` + self.__use_move_group_action = use_move_group_action - # Flag that determines whether a new goal can be send while the previous one is being executed + # Flag that determines whether a new goal can be sent while the previous one is being executed self.__ignore_new_calls_while_executing = ignore_new_calls_while_executing # Store additional variables for later use @@ -223,11 +259,45 @@ def __init__( self.__is_motion_requested = False self.__is_executing = False self.motion_suceeded = False + self.__execution_goal_handle = None + self.__last_error_code = None self.__wait_until_executed_rate = self._node.create_rate(1000.0) + self.__execution_mutex = threading.Lock() # Event that enables waiting until async future is done self.__future_done_event = threading.Event() + #### Execution Polling Functions + def query_state(self) -> MoveIt2State: + with self.__execution_mutex: + if self.__is_motion_requested: + return MoveIt2State.REQUESTING + elif self.__is_executing: + return MoveIt2State.EXECUTING + else: + return MoveIt2State.IDLE + + def cancel_execution(self): + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn("Attempted to cancel without active goal.") + return None + + cancel_string = String() + cancel_string.data = "stop" + self.__cancellation_pub.publish(cancel_string) + + def get_execution_future(self) -> Optional[Future]: + if self.query_state() != MoveIt2State.EXECUTING: + self._node.get_logger().warn("Need active goal for future.") + return None + + return self.__execution_goal_handle.get_result_async() + + def get_last_execution_error_code(self) -> Optional[MoveItErrorCodes]: + return self.__last_error_code + + #### + def move_to_pose( self, pose: Optional[Union[PoseStamped, Pose]] = None, @@ -282,13 +352,14 @@ def move_to_pose( pose=Pose(position=position, orientation=quat_xyzw), ) - if self.__execute_via_moveit and not cartesian: - if self.__ignore_new_calls_while_executing and self.__is_executing: + if self.__use_move_group_action and not cartesian: + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) return - self.__is_motion_requested = True # Set goal self.set_pose_goal( @@ -310,6 +381,7 @@ def move_to_pose( self._send_goal_async_move_action() # Clear all previous goal constrains self.clear_goal_constraints() + self.clear_path_constraints() else: # Plan via MoveIt 2 and then execute directly with the controller @@ -339,13 +411,14 @@ def move_to_configuration( passed in to internally use `set_joint_goal()` to define a goal during the call. """ - if self.__execute_via_moveit: - if self.__ignore_new_calls_while_executing and self.__is_executing: + if self.__use_move_group_action: + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) return - self.__is_motion_requested = True # Set goal self.set_joint_goal( @@ -363,6 +436,7 @@ def move_to_configuration( self._send_goal_async_move_action() # Clear all previous goal constrains self.clear_goal_constraints() + self.clear_path_constraints() else: # Plan via MoveIt 2 and then execute directly with the controller @@ -387,7 +461,7 @@ def plan( frame_id: Optional[str] = None, target_link: Optional[str] = None, tolerance_position: float = 0.001, - tolerance_orientation: float = 0.001, + tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, tolerance_joint_position: float = 0.001, weight_position: float = 1.0, weight_orientation: float = 1.0, @@ -395,6 +469,43 @@ def plan( start_joint_state: Optional[Union[JointState, List[float]]] = None, cartesian: bool = False, ) -> Optional[JointTrajectory]: + """ + Call plan_async and wait on future + """ + future = self.plan_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_trajectory(future, cartesian=cartesian) + + def plan_async( + self, + pose: Optional[Union[PoseStamped, Pose]] = None, + position: Optional[Union[Point, Tuple[float, float, float]]] = None, + quat_xyzw: Optional[ + Union[Quaternion, Tuple[float, float, float, float]] + ] = None, + joint_positions: Optional[List[float]] = None, + joint_names: Optional[List[str]] = None, + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance_position: float = 0.001, + tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, + tolerance_joint_position: float = 0.001, + weight_position: float = 1.0, + weight_orientation: float = 1.0, + weight_joint_position: float = 1.0, + start_joint_state: Optional[Union[JointState, List[float]]] = None, + cartesian: bool = False, + ) -> Optional[Future]: """ Plan motion based on previously set goals. Optional arguments can be passed in to internally use `set_position_goal()`, `set_orientation_goal()` or `set_joint_goal()` @@ -488,50 +599,82 @@ def plan( elif self.joint_state is not None: self.__move_action_goal.request.start_state.joint_state = self.joint_state - # Plan trajectory by sending a goal (blocking) + # Plan trajectory asynchronously by service call if cartesian: - joint_trajectory = self._plan_cartesian_path( + future = self._plan_cartesian_path( frame_id=pose_stamped.header.frame_id if pose_stamped is not None else frame_id ) else: - if self.__execute_via_moveit: - # Use action client - joint_trajectory = self._send_goal_move_action_plan_only() - else: - # Use service - joint_trajectory = self._plan_kinematic_path() + # Use service + future = self._plan_kinematic_path() # Clear all previous goal constrains self.clear_goal_constraints() + self.clear_path_constraints() + + return future + + def get_trajectory( + self, future: Future, cartesian: bool = False + ) -> Optional[JointTrajectory]: + """ + Takes in a future returned by plan_async and returns the trajectory if the future is done + and planning was successful, else None. + """ + if not future.done(): + self._node.get_logger().warn( + "Cannot get trajectory because future is not done." + ) + return None - return joint_trajectory + res = future.result() + + # Cartesian + if cartesian: + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.solution.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None + + # Else Kinematic + res = res.motion_plan_response + if MoveItErrorCodes.SUCCESS == res.error_code.val: + return res.trajectory.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Error code: {res.error_code.val}." + ) + return None def execute(self, joint_trajectory: JointTrajectory): """ Execute joint_trajectory by communicating directly with the controller. """ - if self.__ignore_new_calls_while_executing and self.__is_executing: + if self.__ignore_new_calls_while_executing and ( + self.__is_motion_requested or self.__is_executing + ): self._node.get_logger().warn( "Controller is already following a trajectory. Skipping motion." ) return - self.__is_motion_requested = True - follow_joint_trajectory_goal = init_follow_joint_trajectory_goal( + execute_trajectory_goal = init_execute_trajectory_goal( joint_trajectory=joint_trajectory ) - if follow_joint_trajectory_goal is None: + if execute_trajectory_goal is None: self._node.get_logger().warn( "Cannot execute motion because the provided/planned trajectory is invalid." ) - self.__is_motion_requested = False return - self._send_goal_async_follow_joint_trajectory(goal=follow_joint_trajectory_goal) + self._send_goal_async_execute_trajectory(goal=execute_trajectory_goal) def wait_until_executed(self) -> bool: """ @@ -563,12 +706,12 @@ def reset_controller( joint_positions=joint_state, ) joint_trajectory = init_dummy_joint_trajectory_from_state(joint_state) - follow_joint_trajectory_goal = init_follow_joint_trajectory_goal( + execute_trajectory_goal = init_execute_trajectory_goal( joint_trajectory=joint_trajectory ) - self._send_goal_async_follow_joint_trajectory( - goal=follow_joint_trajectory_goal, + self._send_goal_async_execute_trajectory( + goal=execute_trajectory_goal, wait_until_response=sync, ) @@ -582,7 +725,7 @@ def set_pose_goal( frame_id: Optional[str] = None, target_link: Optional[str] = None, tolerance_position: float = 0.001, - tolerance_orientation: float = 0.001, + tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.001, weight_position: float = 1.0, weight_orientation: float = 1.0, ): @@ -644,16 +787,16 @@ def set_pose_goal( weight=weight_orientation, ) - def set_position_goal( + def create_position_constraint( self, position: Union[Point, Tuple[float, float, float]], frame_id: Optional[str] = None, target_link: Optional[str] = None, tolerance: float = 0.001, weight: float = 1.0, - ): + ) -> PositionConstraint: """ - Set Cartesian position goal of `target_link` with respect to `frame_id`. + Create Cartesian position constraint of `target_link` with respect to `frame_id`. - `frame_id` defaults to the base link - `target_link` defaults to end effector """ @@ -692,21 +835,46 @@ def set_position_goal( # Set weight of the constraint constraint.weight = weight + return constraint + + def set_position_goal( + self, + position: Union[Point, Tuple[float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set Cartesian position goal of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_position_constraint( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + ) + # Append to other constraints self.__move_action_goal.request.goal_constraints[ -1 ].position_constraints.append(constraint) - def set_orientation_goal( + def create_orientation_constraint( self, quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], frame_id: Optional[str] = None, target_link: Optional[str] = None, - tolerance: float = 0.001, + tolerance: Union[float, Tuple[float, float, float]] = 0.001, weight: float = 1.0, - ): + parameterization: int = 0, # 0: Euler, 1: Rotation Vector + ) -> OrientationConstraint: """ - Set Cartesian orientation goal of `target_link` with respect to `frame_id`. + Create a Cartesian orientation constraint of `target_link` with respect to `frame_id`. - `frame_id` defaults to the base link - `target_link` defaults to end effector """ @@ -732,31 +900,66 @@ def set_orientation_goal( constraint.orientation.w = float(quat_xyzw[3]) # Define tolerances - constraint.absolute_x_axis_tolerance = tolerance - constraint.absolute_y_axis_tolerance = tolerance - constraint.absolute_z_axis_tolerance = tolerance + if type(tolerance) == float: + tolerance_xyz = (tolerance, tolerance, tolerance) + else: + tolerance_xyz = tolerance + constraint.absolute_x_axis_tolerance = tolerance_xyz[0] + constraint.absolute_y_axis_tolerance = tolerance_xyz[1] + constraint.absolute_z_axis_tolerance = tolerance_xyz[2] + + # Define parameterization (how to interpret the tolerance) + constraint.parameterization = parameterization # Set weight of the constraint constraint.weight = weight + return constraint + + def set_orientation_goal( + self, + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: Union[float, Tuple[float, float, float]] = 0.001, + weight: float = 1.0, + parameterization: int = 0, # 0: Euler, 1: Rotation Vector + ): + """ + Set Cartesian orientation goal of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_orientation_constraint( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + parameterization=parameterization, + ) + # Append to other constraints self.__move_action_goal.request.goal_constraints[ -1 ].orientation_constraints.append(constraint) - def set_joint_goal( + def create_joint_constraints( self, joint_positions: List[float], joint_names: Optional[List[str]] = None, tolerance: float = 0.001, weight: float = 1.0, - ): + ) -> List[JointConstraint]: """ - Set joint space goal. With `joint_names` specified, `joint_positions` can be + Creates joint space constraints. With `joint_names` specified, `joint_positions` can be defined for specific joints in an arbitrary order. Otherwise, first **n** joints passed into the constructor is used, where **n** is the length of `joint_positions`. """ + constraints = [] + # Use default joint names if not specified if joint_names == None: joint_names = self.__joint_names @@ -778,10 +981,34 @@ def set_joint_goal( # Set weight of the constraint constraint.weight = weight - # Append to other constraints - self.__move_action_goal.request.goal_constraints[ - -1 - ].joint_constraints.append(constraint) + constraints.append(constraint) + + return constraints + + def set_joint_goal( + self, + joint_positions: List[float], + joint_names: Optional[List[str]] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set joint space goal. With `joint_names` specified, `joint_positions` can be + defined for specific joints in an arbitrary order. Otherwise, first **n** joints + passed into the constructor is used, where **n** is the length of `joint_positions`. + """ + + constraints = self.create_joint_constraints( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.goal_constraints[-1].joint_constraints.extend( + constraints + ) def clear_goal_constraints(self): """ @@ -800,12 +1027,150 @@ def create_new_goal_constraint(self): self.__move_action_goal.request.goal_constraints.append(Constraints()) + def set_path_joint_constraint( + self, + joint_positions: List[float], + joint_names: Optional[List[str]] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set joint space path constraints. With `joint_names` specified, `joint_positions` can be + defined for specific joints in an arbitrary order. Otherwise, first **n** joints + passed into the constructor is used, where **n** is the length of `joint_positions`. + """ + + constraints = self.create_joint_constraints( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.path_constraints.joint_constraints.extend( + constraints + ) + + def set_path_position_constraint( + self, + position: Union[Point, Tuple[float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: float = 0.001, + weight: float = 1.0, + ): + """ + Set Cartesian position path constraint of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_position_constraint( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + ) + + # Append to other constraints + self.__move_action_goal.request.path_constraints.position_constraints.append( + constraint + ) + + def set_path_orientation_constraint( + self, + quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]], + frame_id: Optional[str] = None, + target_link: Optional[str] = None, + tolerance: Union[float, Tuple[float, float, float]] = 0.001, + weight: float = 1.0, + parameterization: int = 0, # 0: Euler Angles, 1: Rotation Vector + ): + """ + Set Cartesian orientation path constraint of `target_link` with respect to `frame_id`. + - `frame_id` defaults to the base link + - `target_link` defaults to end effector + """ + + constraint = self.create_orientation_constraint( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + parameterization=parameterization, + ) + + # Append to other constraints + self.__move_action_goal.request.path_constraints.orientation_constraints.append( + constraint + ) + + def clear_path_constraints(self): + """ + Clear all path constraints that were previously set. + Note that this function is called automatically after each `plan_kinematic_path()`. + """ + + self.__move_action_goal.request.path_constraints = Constraints() + 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 @@ -834,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 @@ -932,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): """ @@ -957,8 +1349,10 @@ def force_reset_executing_state(self): used. This function is applicable only in a very few edge-cases, so it should almost never be used. """ + self.__execution_mutex.acquire() self.__is_motion_requested = False self.__is_executing = False + self.__execution_mutex.release() def add_collision_primitive( self, @@ -1308,40 +1702,7 @@ def __joint_state_callback(self, msg: JointState): self.__new_joint_state_available = True self.__joint_state_mutex.release() - def _send_goal_move_action_plan_only( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 - ) -> Optional[JointTrajectory]: - # Set action goal to only do planning without execution - original_plan_only = self.__move_action_goal.planning_options.plan_only - self.__move_action_goal.planning_options.plan_only = True - - stamp = self._node.get_clock().now().to_msg() - self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - - if not self.__move_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): - self._node.get_logger().warn( - f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" - ) - return None - - move_action_result = self.__move_action_client.send_goal( - goal=self.__move_action_goal, - feedback_callback=None, - ) - - # Revert back to original planning/execution mode - self.__move_action_goal.planning_options.plan_only = original_plan_only - - if move_action_result.status == GoalStatus.STATUS_SUCCEEDED: - return move_action_result.result.planned_trajectory.joint_trajectory - else: - return None - - def _plan_kinematic_path( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 - ) -> Optional[JointTrajectory]: + def _plan_kinematic_path(self) -> Optional[Future]: # Re-use request from move action goal self.__kinematic_path_request.motion_plan_request = ( self.__move_action_goal.request @@ -1359,32 +1720,21 @@ def _plan_kinematic_path( for orientation_constraint in constraints.orientation_constraints: orientation_constraint.header.stamp = stamp - if not self._plan_kinematic_path_service.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): + if not self._plan_kinematic_path_service.service_is_ready(): self._node.get_logger().warn( f"Service '{self._plan_kinematic_path_service.srv_name}' is not yet available. Better luck next time!" ) return None - res = self._plan_kinematic_path_service.call( + return self._plan_kinematic_path_service.call_async( self.__kinematic_path_request - ).motion_plan_response - - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.trajectory.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None + ) def _plan_cartesian_path( self, max_step: float = 0.0025, - wait_for_server_timeout_sec: Optional[float] = 1.0, frame_id: Optional[str] = None, - ) -> Optional[JointTrajectory]: + ) -> Optional[Future]: # Re-use request from move action goal self.__cartesian_path_request.start_state = ( self.__move_action_goal.request.start_state @@ -1430,39 +1780,28 @@ def _plan_cartesian_path( self.__cartesian_path_request.waypoints = [target_pose] - if not self._plan_cartesian_path_service.wait_for_service( - timeout_sec=wait_for_server_timeout_sec - ): + if not self._plan_cartesian_path_service.service_is_ready(): self._node.get_logger().warn( f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!" ) return None - res = self._plan_cartesian_path_service.call(self.__cartesian_path_request) - - if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.solution.joint_trajectory - else: - self._node.get_logger().warn( - f"Planning failed! Error code: {res.error_code.val}." - ) - return None + return self._plan_cartesian_path_service.call_async( + self.__cartesian_path_request + ) - def _send_goal_async_move_action( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 - ): + def _send_goal_async_move_action(self): + self.__execution_mutex.acquire() stamp = self._node.get_clock().now().to_msg() self.__move_action_goal.request.workspace_parameters.header.stamp = stamp - - if not self.__move_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): + if not self.__move_action_client.server_is_ready(): self._node.get_logger().warn( f"Action server '{self.__move_action_client._action_name}' is not yet available. Better luck next time!" ) - self.__is_motion_requested = False return + self.__last_error_code = None + self.__is_motion_requested = True self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( goal=self.__move_action_goal, feedback_callback=None, @@ -1472,7 +1811,10 @@ def _send_goal_async_move_action( self.__response_callback_move_action ) + self.__execution_mutex.release() + def __response_callback_move_action(self, response): + self.__execution_mutex.acquire() goal_handle = response.result() if not goal_handle.accepted: self._node.get_logger().warn( @@ -1481,6 +1823,7 @@ def __response_callback_move_action(self, response): self.__is_motion_requested = False return + self.__execution_goal_handle = goal_handle self.__is_executing = True self.__is_motion_requested = False @@ -1488,86 +1831,89 @@ def __response_callback_move_action(self, response): self.__get_result_future_move_action.add_done_callback( self.__result_callback_move_action ) + self.__execution_mutex.release() def __result_callback_move_action(self, res): + self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().error( + self._node.get_logger().warn( f"Action '{self.__move_action_client._action_name}' was unsuccessful: {res.result().status}." ) self.motion_suceeded = False else: self.motion_suceeded = True + self.__last_error_code = res.result().result.error_code + + self.__execution_goal_handle = None self.__is_executing = False + self.__execution_mutex.release() - def _send_goal_async_follow_joint_trajectory( + def _send_goal_async_execute_trajectory( self, - goal: FollowJointTrajectory, - wait_for_server_timeout_sec: Optional[float] = 1.0, + goal: ExecuteTrajectory, wait_until_response: bool = False, ): - if not self.__follow_joint_trajectory_action_client.wait_for_server( - timeout_sec=wait_for_server_timeout_sec - ): + self.__execution_mutex.acquire() + + if not self._execute_trajectory_action_client.server_is_ready(): self._node.get_logger().warn( - f"Action server '{self.__follow_joint_trajectory_action_client._action_name}' is not yet available. Better luck next time!" + f"Action server '{self._execute_trajectory_action_client._action_name}' is not yet available. Better luck next time!" ) - self.__is_motion_requested = False - return None + return - action_result = self.__follow_joint_trajectory_action_client.send_goal_async( - goal=goal, - feedback_callback=None, + self.__last_error_code = None + self.__is_motion_requested = True + self.__send_goal_future_execute_trajectory = ( + self._execute_trajectory_action_client.send_goal_async( + goal=goal, + feedback_callback=None, + ) ) - action_result.add_done_callback( - self.__response_callback_follow_joint_trajectory + self.__send_goal_future_execute_trajectory.add_done_callback( + self.__response_callback_execute_trajectory ) + self.__execution_mutex.release() - if wait_until_response: - self.__future_done_event.clear() - action_result.add_done_callback( - self.__response_callback_with_event_set_follow_joint_trajectory - ) - self.__future_done_event.wait(timeout=wait_for_server_timeout_sec) - else: - action_result.add_done_callback( - self.__response_callback_follow_joint_trajectory - ) - - def __response_callback_follow_joint_trajectory(self, response): + def __response_callback_execute_trajectory(self, response): + self.__execution_mutex.acquire() goal_handle = response.result() if not goal_handle.accepted: self._node.get_logger().warn( - f"Action '{self.__follow_joint_trajectory_action_client._action_name}' was rejected." + f"Action '{self._execute_trajectory_action_client._action_name}' was rejected." ) self.__is_motion_requested = False return + self.__execution_goal_handle = goal_handle self.__is_executing = True self.__is_motion_requested = False - self.__get_result_future_follow_joint_trajectory = ( - goal_handle.get_result_async() - ) - self.__get_result_future_follow_joint_trajectory.add_done_callback( - self.__result_callback_follow_joint_trajectory + self.__get_result_future_execute_trajectory = goal_handle.get_result_async() + self.__get_result_future_execute_trajectory.add_done_callback( + self.__result_callback_execute_trajectory ) + self.__execution_mutex.release() - def __response_callback_with_event_set_follow_joint_trajectory(self, response): - self.__response_callback_follow_joint_trajectory(response) + def __response_callback_with_event_set_execute_trajectory(self, response): self.__future_done_event.set() - def __result_callback_follow_joint_trajectory(self, res): + def __result_callback_execute_trajectory(self, res): + self.__execution_mutex.acquire() if res.result().status != GoalStatus.STATUS_SUCCEEDED: - self._node.get_logger().error( - f"Action '{self.__follow_joint_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." + self._node.get_logger().warn( + f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {res.result().status}." ) self.motion_suceeded = False else: self.motion_suceeded = True + self.__last_error_code = res.result().result.error_code + + self.__execution_goal_handle = None self.__is_executing = False + self.__execution_mutex.release() @classmethod def __init_move_action_goal( @@ -1584,11 +1930,11 @@ def __init_move_action_goal( move_action_goal.request.workspace_parameters.max_corner.z = 1.0 # move_action_goal.request.start_state = "Set during request" move_action_goal.request.goal_constraints = [Constraints()] - # move_action_goal.request.path_constraints = "Ignored" + move_action_goal.request.path_constraints = Constraints() # move_action_goal.request.trajectory_constraints = "Ignored" # move_action_goal.request.reference_trajectories = "Ignored" - # move_action_goal.request.pipeline_id = "Ignored" - # move_action_goal.request.planner_id = "Ignored" + move_action_goal.request.pipeline_id = "" + move_action_goal.request.planner_id = "" move_action_goal.request.group_name = group_name move_action_goal.request.num_planning_attempts = 5 move_action_goal.request.allowed_planning_time = 0.5 @@ -1710,6 +2056,22 @@ def allowed_planning_time(self) -> float: def allowed_planning_time(self, value: float): self.__move_action_goal.request.allowed_planning_time = value + @property + def pipeline_id(self) -> int: + return self.__move_action_goal.request.pipeline_id + + @pipeline_id.setter + def pipeline_id(self, value: str): + self.__move_action_goal.request.pipeline_id = value + + @property + def planner_id(self) -> int: + return self.__move_action_goal.request.planner_id + + @planner_id.setter + def planner_id(self, value: str): + self.__move_action_goal.request.planner_id = value + def init_joint_state( joint_names: List[str], @@ -1733,23 +2095,17 @@ def init_joint_state( return joint_state -def init_follow_joint_trajectory_goal( +def init_execute_trajectory_goal( joint_trajectory: JointTrajectory, -) -> Optional[FollowJointTrajectory.Goal]: +) -> Optional[ExecuteTrajectory.Goal]: if joint_trajectory is None: return None - follow_joint_trajectory_goal = FollowJointTrajectory.Goal() + execute_trajectory_goal = ExecuteTrajectory.Goal() - follow_joint_trajectory_goal.trajectory = joint_trajectory - # follow_joint_trajectory_goal.multi_dof_trajectory = "Ignored" - # follow_joint_trajectory_goal.path_tolerance = "Ignored" - # follow_joint_trajectory_goal.component_path_tolerance = "Ignored" - # follow_joint_trajectory_goal.goal_tolerance = "Ignored" - # follow_joint_trajectory_goal.component_goal_tolerance = "Ignored" - # follow_joint_trajectory_goal.goal_time_tolerance = "Ignored" + execute_trajectory_goal.trajectory.joint_trajectory = joint_trajectory - return follow_joint_trajectory_goal + return execute_trajectory_goal def init_dummy_joint_trajectory_from_state( diff --git a/pymoveit2/moveit2_gripper.py b/pymoveit2/moveit2_gripper.py index 2d0b1fd..35fcdc1 100644 --- a/pymoveit2/moveit2_gripper.py +++ b/pymoveit2/moveit2_gripper.py @@ -25,7 +25,8 @@ def __init__( skip_planning: bool = False, skip_planning_fixed_motion_duration: float = 0.5, callback_group: Optional[CallbackGroup] = None, - follow_joint_trajectory_action_name: str = "gripper_trajectory_controller/follow_joint_trajectory", + follow_joint_trajectory_action_name: str = "DEPRECATED", + use_move_group_action: bool = False, ): """ Construct an instance of `MoveIt2Gripper` interface. @@ -34,7 +35,7 @@ def __init__( - `open_gripper_joint_positions` - Configuration of gripper joints when open - `closed_gripper_joint_positions` - Configuration of gripper joints when fully closed - `gripper_group_name` - Name of the planning group for robot gripper - - `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) + - [DEPRECATED] `execute_via_moveit` - Flag that enables execution via MoveGroup action (MoveIt 2) FollowJointTrajectory action (controller) is employed otherwise together with a separate planning service client - `ignore_new_calls_while_executing` - Flag to ignore requests to execute new trajectories @@ -45,19 +46,32 @@ def __init__( - `skip_planning_fixed_motion_duration` - Desired duration for the closing and opening motions when `skip_planning` mode is enabled. - `callback_group` - Optional callback group to use for ROS 2 communication (topics/services/actions) - - `follow_joint_trajectory_action_name` - Name of the action server for the controller + - [DEPRECATED] `follow_joint_trajectory_action_name` - Name of the action server for the controller + - `use_move_group_action` - Flag that enables execution via MoveGroup action (MoveIt 2) + ExecuteTrajectory action is employed otherwise + together with a separate planning service client """ + # Check for deprecated parameters + if execute_via_moveit: + node.get_logger().warn( + "Parameter `execute_via_moveit` is deprecated. Please use `use_move_group_action` instead." + ) + use_move_group_action = True + if follow_joint_trajectory_action_name != "DEPRECATED": + node.get_logger().warn( + "Parameter `follow_joint_trajectory_action_name` is deprecated. `MoveIt2` uses the `execute_trajectory` action instead." + ) + super().__init__( node=node, joint_names=gripper_joint_names, base_link_name="", end_effector_name="", group_name=gripper_group_name, - execute_via_moveit=execute_via_moveit, ignore_new_calls_while_executing=ignore_new_calls_while_executing, callback_group=callback_group, - follow_joint_trajectory_action_name=follow_joint_trajectory_action_name, + use_move_group_action=use_move_group_action, ) self.__del_redundant_attributes()