From 1911c4ee774db413122f06e7747043207f2dda28 Mon Sep 17 00:00:00 2001 From: Admin Date: Mon, 4 Apr 2022 13:27:53 +0200 Subject: [PATCH 1/3] added cartesian planning --- pymoveit2/moveit2.py | 80 ++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 74 insertions(+), 6 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 88f64c1..fc83765 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -12,7 +12,7 @@ OrientationConstraint, PositionConstraint, ) -from moveit_msgs.srv import GetMotionPlan, GetPositionFK, GetPositionIK +from moveit_msgs.srv import GetMotionPlan, GetPositionFK, GetPositionIK, GetCartesianPath from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup from rclpy.node import Node @@ -131,6 +131,20 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() + # create a separate service client for cartesian planning + self._plan_cartesian_path_service = self._node.create_client( + srv_type=GetCartesianPath, + srv_name="compute_cartesian_path", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, + ) + self.__cartesian_path_request = GetCartesianPath.Request() + # Create action client for trajectory execution self.__follow_joint_trajectory_action_client = ActionClient( node=self._node, @@ -208,6 +222,7 @@ def move_to_pose( tolerance_position: float = 0.001, tolerance_orientation: float = 0.001, weight_position: float = 1.0, + cartesian = False, weight_orientation: float = 1.0, ): """ @@ -255,6 +270,7 @@ def move_to_pose( tolerance_orientation=tolerance_orientation, weight_position=weight_position, weight_orientation=weight_orientation, + cartesian=cartesian, ) ) @@ -263,6 +279,7 @@ def move_to_configuration( joint_positions: List[float], joint_names: Optional[List[str]] = None, tolerance: float = 0.001, + cartesian = False, weight: float = 1.0, ): """ @@ -303,6 +320,7 @@ def move_to_configuration( joint_names=joint_names, tolerance_joint_position=tolerance, weight_joint_position=weight, + cartesian = cartesian, ) ) @@ -322,6 +340,7 @@ def plan( weight_orientation: float = 1.0, weight_joint_position: float = 1.0, start_joint_state: Optional[Union[JointState, List[float]]] = None, + cartesian = False, ) -> Optional[JointTrajectory]: """ Plan motion based on previously set goals. Optional arguments can be passed in to @@ -372,12 +391,15 @@ def plan( self.__move_action_goal.request.start_state.joint_state = self.joint_state # Plan trajectory by sending a goal (blocking) - if self.__execute_via_moveit: - # Use action client - joint_trajectory = self._send_goal_move_action_plan_only() + if cartesian: + joint_trajectory = self._plan_cartesian_path() else: - # Use service - joint_trajectory = self._plan_kinematic_path() + 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() # Clear all previous goal constrains self.clear_goal_constraints() @@ -872,6 +894,52 @@ def _plan_kinematic_path( f"Planning failed! Error code: {res.error_code.val}." ) return None + def _plan_cartesian_path( + self, wait_for_server_timeout_sec: Optional[float] = 1.0 + ) -> Optional[JointTrajectory]: + + # Re-use request from move action goal + self.__cartesian_path_request.start_state = self.__move_action_goal.request.start_state + self.__cartesian_path_request.group_name = self.__move_action_goal.request.group_name + self.__cartesian_path_request.link_name = self.__end_effector_name + self.__cartesian_path_request.max_step = 0.0025 + + stamp = self._node.get_clock().now().to_msg() + self.__cartesian_path_request.header.stamp = stamp + + self.__cartesian_path_request.path_constraints = self.__move_action_goal.request.path_constraints + for position_constraint in self.__cartesian_path_request.path_constraints.position_constraints: + position_constraint.header.stamp = stamp + for orientation_constraint in self.__cartesian_path_request.path_constraints.orientation_constraints: + orientation_constraint.header.stamp = stamp + #for joint_constraint in self.__cartesian_path_request.path_constraints.joint_constraints: + #joint_constraint.header.stamp = stamp + + target_pose = Pose() + target_pose.position = self.__move_action_goal.request.goal_constraints[-1].position_constraints[-1].constraint_region.primitive_poses[0].position + target_pose.orientation = self.__move_action_goal.request.goal_constraints[-1].orientation_constraints[-1].orientation + + self.__cartesian_path_request.waypoints = [target_pose] + + if not self._plan_cartesian_path_service.wait_for_service( + timeout_sec=wait_for_server_timeout_sec + ): + 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 def _send_goal_async_move_action( self, wait_for_server_timeout_sec: Optional[float] = 1.0 From c612fc6cd971fde87c8ef1d596ba549f8c3631af Mon Sep 17 00:00:00 2001 From: Nils Schulte <47043622+Schnilz@users.noreply.github.com> Date: Mon, 4 Apr 2022 13:55:56 +0200 Subject: [PATCH 2/3] added cartesian parameter to example --- examples/ex_pose_goal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index 13075be..c9337ad 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -52,7 +52,7 @@ def main(args=None): 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) + moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=False) moveit2.wait_until_executed() rclpy.shutdown() From 1b5f13af12560bd9a9aa582c9a21b7f1fba1ce1f Mon Sep 17 00:00:00 2001 From: Admin Date: Fri, 8 Apr 2022 11:10:38 +0200 Subject: [PATCH 3/3] address review comments --- pymoveit2/moveit2.py | 65 ++++++++++++++++++++++++++++++-------------- 1 file changed, 44 insertions(+), 21 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index fc83765..716bf28 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -12,7 +12,12 @@ OrientationConstraint, PositionConstraint, ) -from moveit_msgs.srv import GetMotionPlan, GetPositionFK, GetPositionIK, GetCartesianPath +from moveit_msgs.srv import ( + GetCartesianPath, + GetMotionPlan, + GetPositionFK, + GetPositionIK, +) from rclpy.action import ActionClient from rclpy.callback_groups import CallbackGroup from rclpy.node import Node @@ -131,7 +136,7 @@ def __init__( ) self.__kinematic_path_request = GetMotionPlan.Request() - # create a separate service client for cartesian planning + # Create a separate service client for Cartesian planning self._plan_cartesian_path_service = self._node.create_client( srv_type=GetCartesianPath, srv_name="compute_cartesian_path", @@ -222,7 +227,7 @@ def move_to_pose( tolerance_position: float = 0.001, tolerance_orientation: float = 0.001, weight_position: float = 1.0, - cartesian = False, + cartesian: bool = False, weight_orientation: float = 1.0, ): """ @@ -279,7 +284,7 @@ def move_to_configuration( joint_positions: List[float], joint_names: Optional[List[str]] = None, tolerance: float = 0.001, - cartesian = False, + cartesian: bool = False, weight: float = 1.0, ): """ @@ -320,7 +325,7 @@ def move_to_configuration( joint_names=joint_names, tolerance_joint_position=tolerance, weight_joint_position=weight, - cartesian = cartesian, + cartesian=cartesian, ) ) @@ -340,7 +345,7 @@ def plan( weight_orientation: float = 1.0, weight_joint_position: float = 1.0, start_joint_state: Optional[Union[JointState, List[float]]] = None, - cartesian = False, + cartesian: bool = False, ) -> Optional[JointTrajectory]: """ Plan motion based on previously set goals. Optional arguments can be passed in to @@ -894,30 +899,50 @@ def _plan_kinematic_path( f"Planning failed! Error code: {res.error_code.val}." ) return None + def _plan_cartesian_path( - self, wait_for_server_timeout_sec: Optional[float] = 1.0 + self, + max_step: float = 0.0025, + wait_for_server_timeout_sec: Optional[float] = 1.0, ) -> Optional[JointTrajectory]: - # Re-use request from move action goal - self.__cartesian_path_request.start_state = self.__move_action_goal.request.start_state - self.__cartesian_path_request.group_name = self.__move_action_goal.request.group_name + self.__cartesian_path_request.start_state = ( + self.__move_action_goal.request.start_state + ) + self.__cartesian_path_request.group_name = ( + self.__move_action_goal.request.group_name + ) self.__cartesian_path_request.link_name = self.__end_effector_name - self.__cartesian_path_request.max_step = 0.0025 + self.__cartesian_path_request.max_step = max_step stamp = self._node.get_clock().now().to_msg() self.__cartesian_path_request.header.stamp = stamp - self.__cartesian_path_request.path_constraints = self.__move_action_goal.request.path_constraints - for position_constraint in self.__cartesian_path_request.path_constraints.position_constraints: + self.__cartesian_path_request.path_constraints = ( + self.__move_action_goal.request.path_constraints + ) + for ( + position_constraint + ) in self.__cartesian_path_request.path_constraints.position_constraints: position_constraint.header.stamp = stamp - for orientation_constraint in self.__cartesian_path_request.path_constraints.orientation_constraints: + for ( + orientation_constraint + ) in self.__cartesian_path_request.path_constraints.orientation_constraints: orientation_constraint.header.stamp = stamp - #for joint_constraint in self.__cartesian_path_request.path_constraints.joint_constraints: - #joint_constraint.header.stamp = stamp + # no header in joint_constraint message type target_pose = Pose() - target_pose.position = self.__move_action_goal.request.goal_constraints[-1].position_constraints[-1].constraint_region.primitive_poses[0].position - target_pose.orientation = self.__move_action_goal.request.goal_constraints[-1].orientation_constraints[-1].orientation + target_pose.position = ( + self.__move_action_goal.request.goal_constraints[-1] + .position_constraints[-1] + .constraint_region.primitive_poses[0] + .position + ) + target_pose.orientation = ( + self.__move_action_goal.request.goal_constraints[-1] + .orientation_constraints[-1] + .orientation + ) self.__cartesian_path_request.waypoints = [target_pose] @@ -929,9 +954,7 @@ def _plan_cartesian_path( ) return None - res = self._plan_cartesian_path_service.call( - self.__cartesian_path_request - ) + res = self._plan_cartesian_path_service.call(self.__cartesian_path_request) if MoveItErrorCodes.SUCCESS == res.error_code.val: return res.solution.joint_trajectory