From ae4c8bcc5bc68db3588f3136f1f8f38fa9a8c514 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Dec 2023 13:27:56 -0800 Subject: [PATCH 1/4] 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 --- examples/ex_gripper.py | 1 - examples/ex_joint_goal.py | 45 +++- examples/ex_pose_goal.py | 43 +++- pymoveit2/__init__.py | 2 +- pymoveit2/gripper_interface.py | 19 +- pymoveit2/moveit2.py | 400 ++++++++++++++++++++------------- pymoveit2/moveit2_gripper.py | 24 +- 7 files changed, 362 insertions(+), 172 deletions(-) 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_joint_goal.py b/examples/ex_joint_goal.py index 45e97b2..cece348 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,9 @@ 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) # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -58,15 +63,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_pose_goal.py b/examples/ex_pose_goal.py index 4a76569..cbe7a70 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,9 @@ 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) # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -53,13 +58,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..f86c6b0 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( @@ -339,13 +410,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( @@ -395,6 +467,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 + + # 10ms 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: 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 +597,81 @@ 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() - return joint_trajectory + 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 + + 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 +703,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, ) @@ -957,8 +1097,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 +1450,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 +1468,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 +1528,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 +1559,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 +1571,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 +1579,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( @@ -1733,23 +1827,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() From cff6677e20d356f69e1611c30bae6281d8e97cfc Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 5 Jan 2024 07:11:31 -0800 Subject: [PATCH 2/4] 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 --- CMakeLists.txt | 1 + examples/ex_orientation_path_constraint.py | 147 ++++++++++++++ pymoveit2/moveit2.py | 218 +++++++++++++++++++-- 3 files changed, 345 insertions(+), 21 deletions(-) create mode 100755 examples/ex_orientation_path_constraint.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a88115..22fbc16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ install(PROGRAMS ${EXAMPLES_DIR}/ex_collision_primitive.py ${EXAMPLES_DIR}/ex_gripper.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_orientation_path_constraint.py b/examples/ex_orientation_path_constraint.py new file mode 100755 index 0000000..4d7aa1c --- /dev/null +++ b/examples/ex_orientation_path_constraint.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +""" +Example of moving to a joint configuration. +- 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_joint_goal") + + # 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/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index f86c6b0..95e7781 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -381,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 @@ -435,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 @@ -459,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, @@ -496,7 +498,7 @@ def plan_async( 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, @@ -610,6 +612,7 @@ def plan_async( # Clear all previous goal constrains self.clear_goal_constraints() + self.clear_path_constraints() return future @@ -722,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, ): @@ -784,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 """ @@ -832,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 """ @@ -872,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 @@ -918,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): """ @@ -940,6 +1027,95 @@ 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, @@ -1678,7 +1854,7 @@ 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" From 09ce8d8a75afaf74da907b07a3a63e53cde70393 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 20 Feb 2024 02:08:29 -0800 Subject: [PATCH 3/4] 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 * Update examples/ex_ik.py Co-authored-by: Andrej Orsula * Update moveit2.py --------- Co-authored-by: Ethan Gordon Co-authored-by: Andrej Orsula --- CMakeLists.txt | 2 + examples/ex_fk.py | 90 +++++++++++++++ examples/ex_ik.py | 79 +++++++++++++ examples/ex_orientation_path_constraint.py | 4 +- pymoveit2/moveit2.py | 124 +++++++++++++++++---- 5 files changed, 273 insertions(+), 26 deletions(-) create mode 100755 examples/ex_fk.py create mode 100755 examples/ex_ik.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 22fbc16..43d5fc3 100644 --- a/CMakeLists.txt +++ b/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 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_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_orientation_path_constraint.py b/examples/ex_orientation_path_constraint.py index 4d7aa1c..e2efabd 100755 --- a/examples/ex_orientation_path_constraint.py +++ b/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( diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 95e7781..5285cc3 100644 --- a/pymoveit2/moveit2.py +++ b/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): """ From 8bf71aef4ede2b301580c5fe6c8d4a12c48400c4 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 27 Feb 2024 04:54:10 -0800 Subject: [PATCH 4/4] 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 --- examples/ex_joint_goal.py | 5 +++++ examples/ex_pose_goal.py | 5 +++++ pymoveit2/moveit2.py | 20 ++++++++++++++++++-- 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py index cece348..d756138 100755 --- a/examples/ex_joint_goal.py +++ b/examples/ex_joint_goal.py @@ -38,6 +38,8 @@ def main(): 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() @@ -51,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) diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index cbe7a70..401fd19 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -29,6 +29,8 @@ def main(): 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() @@ -42,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) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 5285cc3..6d7684e 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1933,8 +1933,8 @@ def __init_move_action_goal( 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 @@ -2056,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],