diff --git a/sr_manipulation_interfaces/CMakeLists.txt b/sr_manipulation_interfaces/CMakeLists.txt index c4e718e..5e4671b 100644 --- a/sr_manipulation_interfaces/CMakeLists.txt +++ b/sr_manipulation_interfaces/CMakeLists.txt @@ -41,6 +41,7 @@ set(srv_files srv/PublishPoseAsTF.srv srv/RemoveObjects.srv srv/GetObjectPose.srv + srv/MoveObject.srv srv/AttachObject.srv srv/DetachObject.srv srv/MoveToPose.srv diff --git a/sr_manipulation_interfaces/action/Manip.action b/sr_manipulation_interfaces/action/Manip.action index 2f49be4..f69bea0 100644 --- a/sr_manipulation_interfaces/action/Manip.action +++ b/sr_manipulation_interfaces/action/Manip.action @@ -22,6 +22,8 @@ bool disable_scene_handling # only plan, not execution bool only_plan +float64 blend_radius +bool add_to_blending_queue # enforce the planner profile (empty means use default) string planner_profile @@ -32,6 +34,7 @@ string gripper_cmd_action_name string[] controller_names float64 velocity_scaling_factor float64 acceleration_scaling_factor +float64 allowed_planning_time bool preempt_ok --- # final state effectively reached diff --git a/sr_manipulation_interfaces/msg/ManipType.msg b/sr_manipulation_interfaces/msg/ManipType.msg index 5e8e30b..fe33bff 100644 --- a/sr_manipulation_interfaces/msg/ManipType.msg +++ b/sr_manipulation_interfaces/msg/ManipType.msg @@ -18,3 +18,5 @@ int8 MANIP_MOVE_POSTPLACE=55 int8 MANIP_GRIPPER_ADJUST=100 int8 MANIP_GRIPPER_OPEN=101 int8 MANIP_GRIPPER_CLOSE=102 +# additional actions +int8 EXECUTE_MOVE_SEQUENCE=120 diff --git a/sr_manipulation_interfaces/srv/MoveObject.srv b/sr_manipulation_interfaces/srv/MoveObject.srv new file mode 100644 index 0000000..382d50a --- /dev/null +++ b/sr_manipulation_interfaces/srv/MoveObject.srv @@ -0,0 +1,6 @@ +# Request +string object_id +geometry_msgs/PoseStamped pose +--- +# Response +ServiceResult result diff --git a/sr_moveit2_utils/sr_moveit2_utils/robot_client_node.py b/sr_moveit2_utils/sr_moveit2_utils/robot_client_node.py index 98db621..adecb9a 100644 --- a/sr_moveit2_utils/sr_moveit2_utils/robot_client_node.py +++ b/sr_moveit2_utils/sr_moveit2_utils/robot_client_node.py @@ -42,6 +42,9 @@ from rclpy.action import ActionServer, GoalResponse, CancelResponse, ActionClient from rclpy.action.server import ServerGoalHandle, GoalStatus from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from moveit_msgs.msg import Constraints, MotionSequenceItem, MoveItErrorCodes, OrientationConstraint, PlanningOptions, PositionConstraint +from moveit_msgs.action import MoveGroupSequence, ExecuteTrajectory_GetResult_Response, MoveGroupSequence_GetResult_Response +from shape_msgs.msg import SolidPrimitive from sr_manipulation_interfaces.action import PlanMoveTo, Manip from sr_manipulation_interfaces.msg import ( ManipType, @@ -66,6 +69,7 @@ import numpy as np from std_srvs.srv import Trigger from std_msgs.msg import String, Float64 +from asyncio import Future def wait_for_response(future, client): @@ -83,6 +87,50 @@ def wait_for_response(future, client): return response + + +def construct_goal_constraints(link_name: str, pose: PoseStamped, + tolerance_pos: float=1e-3, tolerance_angle: float = 1e-2): + # Create the goal constraints message + goal = Constraints() + + # Create and configure the position constraint + position_constraint = PositionConstraint() + position_constraint.link_name = link_name + position_constraint.target_point_offset.x = 0.0 + position_constraint.target_point_offset.y = 0.0 + position_constraint.target_point_offset.z = 0.0 + + # Define a spherical constraint region + sphere = SolidPrimitive() + sphere.type = SolidPrimitive.SPHERE + sphere.dimensions = [tolerance_pos] # Sphere radius + position_constraint.constraint_region.primitives.append(sphere) + + # Position of the sphere + position_constraint.constraint_region.primitive_poses.append(pose.pose) + position_constraint.header = pose.header + position_constraint.weight = 1.0 + + # Add the position constraint to the goal + goal.position_constraints.append(position_constraint) + + # Create and configure the orientation constraint + orientation_constraint = OrientationConstraint() + orientation_constraint.parameterization = OrientationConstraint.ROTATION_VECTOR + orientation_constraint.link_name = link_name + orientation_constraint.header = pose.header + orientation_constraint.orientation = pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = tolerance_angle + orientation_constraint.absolute_y_axis_tolerance = tolerance_angle + orientation_constraint.absolute_z_axis_tolerance = tolerance_angle + orientation_constraint.weight = 1.0 + + # Add the orientation constraint to the goal + goal.orientation_constraints.append(orientation_constraint) + + return goal + class RobotClient(Node): def __init__(self): """Create a new client for managing manipulation requests.""" @@ -106,6 +154,7 @@ def __init__(self): ManipType.MANIP_GRIPPER_ADJUST: "Adjust Gripper gauge", ManipType.MANIP_GRIPPER_OPEN: "Open Gripper", ManipType.MANIP_GRIPPER_CLOSE: "Close Gripper", + ManipType.EXECUTE_MOVE_SEQUENCE: "Execute the sequence stored in the queue." } self.SUPPORTED_MANIP_ACTIONS = list(self.MANIP_ACTIONS_TO_STR.keys()) @@ -169,7 +218,7 @@ def __init__(self): if gripper_cmd_action_names: self.default_gripper_cmd_action_name = gripper_cmd_action_names[0] - + self.move_sequence : List[MotionSequenceItem] = [] self.saved_plan = None self.attach_object_cli = self.create_client( @@ -195,6 +244,10 @@ def __init__(self): self.default_acceleration_scaling_factor = self.declare_parameter( "default_acceleration_scaling_factor", 0.1 ).value + + self.wait_for_action_server_timeout = self.declare_parameter( + "wait_for_action_server_timeout", 5.0 + ).value self.use_parallel_gripper = self.declare_parameter( "use_parallel_gripper", True @@ -236,6 +289,13 @@ def __init__(self): for gripper_cmd_action_name in gripper_cmd_action_names } + self.move_sequence_action_client = ActionClient( + self, + MoveGroupSequence, + "/sequence_move_group", + callback_group=self.action_client_callback_group, + ) + self.moveit_client = MoveitClient( node=self, pose_reference_frame=self.fixed_frame, @@ -544,6 +604,21 @@ def compute_manip_pose( self.fixed_frame, apply_tool_offset, ) + + def create_move_seq_item(self, request: Manip.Goal, pose: Pose) -> MotionSequenceItem: + item = MotionSequenceItem() + item.blend_radius = request.blend_radius + item.req.group_name = request.planning_group + item.req.planner_id = "LIN" if "lin" in request.planner_profile.lower() else "PTP" + item.req.pipeline_id = "pilz" + item.req.allowed_planning_time = 5.0 + item.req.max_velocity_scaling_factor = request.velocity_scaling_factor + item.req.max_acceleration_scaling_factor = request.acceleration_scaling_factor + pose_stamped = PoseStamped() + pose_stamped.header.frame_id = self.fixed_frame + pose_stamped.pose = pose + item.req.goal_constraints.append(construct_goal_constraints(link_name=request.end_effector_link, pose=pose_stamped)) + return item def manip_execute_cb(self, goal_handle: ServerGoalHandle): self.get_logger().info("executing") @@ -620,78 +695,82 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle): is_static=True, ) - # perform the action - # do the actual planning and execution - if manip == ManipType.MANIP_REACH_PREGRASP: - ret = self.send_move_request( - reach_pose_robot_base_frame, - end_effector_link=request.end_effector_link, - cartesian_trajectory=request.cartesian_trajectory, - planner_profile=( - # request.planner_profile if request.planner_profile else "ompl" - request.planner_profile - if request.planner_profile - else "pilz_ptp" - ), - plan_only=self.plan_first, - planning_group=request.planning_group, - controller_names=( - request.controller_names if request.controller_names else None - ), - velocity_scaling_factor=( - request.velocity_scaling_factor - if request.velocity_scaling_factor - else None - ), - acceleration_scaling_factor=( - request.acceleration_scaling_factor - if request.acceleration_scaling_factor - else None - ), - preempt_ok=(request.preempt_ok if request.preempt_ok else False), - ) - if manip == ManipType.MANIP_REACH_PREPLACE: - ret = self.send_move_request( - reach_pose_robot_base_frame, - end_effector_link=request.end_effector_link, - cartesian_trajectory=request.cartesian_trajectory, - planner_profile=( - request.planner_profile - if request.planner_profile - # else "ompl_with_constraints" - else "pilz_ptp" - ), - plan_only=self.plan_first, - planning_group=request.planning_group, - controller_names=( - request.controller_names if request.controller_names else None - ), - velocity_scaling_factor=( - request.velocity_scaling_factor - if request.velocity_scaling_factor - else None - ), - acceleration_scaling_factor=( - request.acceleration_scaling_factor - if request.acceleration_scaling_factor - else None - ), - preempt_ok=(request.preempt_ok if request.preempt_ok else False), - ) - - if not self.did_manip_plan_succeed(ret, "Reach", goal_handle): - result.state.exec_state = PlanExecState.EXEC_ERROR - result.state.exec_message = "Failed to reach" - self.get_logger().warn( - f"reach pre-place with ori {reach_pose_robot_base_frame.orientation} failed" - ) - self.get_logger().warn( - f"reach pre-place with pos {reach_pose_robot_base_frame.position} failed" - ) - break + if request.add_to_blending_queue: + self.move_sequence.append(self.create_move_seq_item(request, reach_pose_robot_base_frame)) else: - continue + # perform the action + # do the actual planning and execution + if manip == ManipType.MANIP_REACH_PREGRASP: + ret = self.send_move_request( + reach_pose_robot_base_frame, + end_effector_link=request.end_effector_link, + cartesian_trajectory=request.cartesian_trajectory, + planner_profile=( + # request.planner_profile if request.planner_profile else "ompl" + request.planner_profile + if request.planner_profile + else "pilz_ptp" + ), + plan_only=self.plan_first, + planning_group=request.planning_group, + controller_names=( + request.controller_names if request.controller_names else None + ), + velocity_scaling_factor=( + request.velocity_scaling_factor + if request.velocity_scaling_factor + else None + ), + acceleration_scaling_factor=( + request.acceleration_scaling_factor + if request.acceleration_scaling_factor + else None + ), + preempt_ok=(request.preempt_ok if request.preempt_ok else False), + ) + if manip == ManipType.MANIP_REACH_PREPLACE: + ret = self.send_move_request( + reach_pose_robot_base_frame, + end_effector_link=request.end_effector_link, + cartesian_trajectory=request.cartesian_trajectory, + planner_profile=( + request.planner_profile + if request.planner_profile + # else "ompl_with_constraints" + else "pilz_ptp" + ), + plan_only=self.plan_first, + planning_group=request.planning_group, + controller_names=( + request.controller_names if request.controller_names else None + ), + velocity_scaling_factor=( + request.velocity_scaling_factor + if request.velocity_scaling_factor + else None + ), + acceleration_scaling_factor=( + request.acceleration_scaling_factor + if request.acceleration_scaling_factor + else None + ), + preempt_ok=(request.preempt_ok if request.preempt_ok else False), + ) + + if not self.did_manip_plan_succeed(ret, "Reach", goal_handle): + result.state.exec_state = PlanExecState.EXEC_ERROR + result.state.exec_message = "Failed to reach" + self.get_logger().warn( + f"reach pre-place with ori {reach_pose_robot_base_frame.orientation} failed" + ) + self.get_logger().warn( + f"reach pre-place with pos {reach_pose_robot_base_frame.position} failed" + ) + + break + else: + continue # move actions (cartesian) if manip in [ @@ -803,57 +882,64 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle): "pre_grip_pose_compensation_link", is_static=True, ) - # perform the action - # do the actual planning and execution - ret = self.send_move_request( - move_pose_robot_base_frame, - end_effector_link=request.end_effector_link, - cartesian_trajectory=( - request.cartesian_trajectory if request.planner_profile else True - ), - planner_profile=( - request.planner_profile if request.planner_profile else "pilz_lin" - ), - plan_only=self.plan_first, - planning_group=request.planning_group, - controller_names=( - request.controller_names if request.controller_names else None - ), - velocity_scaling_factor=( - request.velocity_scaling_factor - if request.velocity_scaling_factor - else None - ), - acceleration_scaling_factor=( - request.acceleration_scaling_factor - if request.acceleration_scaling_factor - else None - ), - preempt_ok=(request.preempt_ok if request.preempt_ok else False), - ) - if not self.did_manip_plan_succeed(ret, "Move", goal_handle): - result.state.exec_state = PlanExecState.EXEC_ERROR - result.state.exec_message = "Failed to Move" - break + + if request.add_to_blending_queue: + self.move_sequence.append(self.create_move_seq_item(request, move_pose_robot_base_frame)) else: - disable_touch_links = TouchLinks() - if manip == ManipType.MANIP_MOVE_POSTGRASP: - # Remove allowed collisions after grasping - self.get_logger().warn( - f"SETTING POSTGRASP DISALLOWED COLLISIONS TO {request.disable_allowed_touch_objects_after_pick}" - ) - disable_touch_links.frame_id = request.object_id - disable_touch_links.touch_links = list(set([request.pick.grasp_pose.header.frame_id] + request.disable_allowed_touch_objects_after_pick)) + # perform the action + # do the actual planning and execution + ret = self.send_move_request( + move_pose_robot_base_frame, + end_effector_link=request.end_effector_link, + cartesian_trajectory=( + request.cartesian_trajectory if request.planner_profile else True + ), + planner_profile=( + request.planner_profile if request.planner_profile else "pilz_lin" + ), + plan_only=self.plan_first, + planning_group=request.planning_group, + controller_names=( + request.controller_names if request.controller_names else None + ), + velocity_scaling_factor=( + request.velocity_scaling_factor + if request.velocity_scaling_factor + else None + ), + acceleration_scaling_factor=( + request.acceleration_scaling_factor + if request.acceleration_scaling_factor + else None + ), + preempt_ok=(request.preempt_ok if request.preempt_ok else False), + ) + + if not self.did_manip_plan_succeed(ret, "Move", goal_handle): + result.state.exec_state = PlanExecState.EXEC_ERROR + result.state.exec_message = "Failed to Move" + break + else: + disable_touch_links = TouchLinks() + if manip == ManipType.MANIP_MOVE_POSTGRASP: + # Remove allowed collisions after grasping + self.get_logger().warn( + f"SETTING POSTGRASP DISALLOWED COLLISIONS TO {request.disable_allowed_touch_objects_after_pick}" + ) + disable_touch_links.frame_id = request.object_id + disable_touch_links.touch_links = list(set([request.pick.grasp_pose.header.frame_id] + request.disable_allowed_touch_objects_after_pick)) + self.apply_touch_links(TouchLinks(), disable_touch_links) self.apply_touch_links(TouchLinks(), disable_touch_links) - ## FIXME(destogl): probably not needed --> remove if you see it commented out - # if manip == ManipType.MANIP_MOVE_POSTPLACE: - # # Remove objects to ignore collision when placing - # disable_touch_links.frame_id = request.object_id - # disable_touch_links.touch_links = list(set([request.place.place_pose.header.frame_id + request.place.allowed_touch_objects])) - self.apply_touch_links(TouchLinks(), disable_touch_links) - - continue + + continue + + # Execute the sequence + if manip == ManipType.EXECUTE_MOVE_SEQUENCE and len(self.move_sequence) > 0: + if not self.send_move_sequence_goal(timeout=self.wait_for_action_server_timeout): + goal_handle.abort() + self.move_sequence = [] + # Attach/Detach actions if manip in [ ManipType.MANIP_GRASP, @@ -936,6 +1022,40 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle): goal_handle.succeed() self.active_goal = None return result + + + def send_move_sequence_goal(self, timeout: Float64): + goal = MoveGroupSequence.Goal() + goal.request.items = self.move_sequence + goal.planning_options = PlanningOptions() + goal.planning_options.planning_scene_diff.is_diff = True + goal.planning_options.planning_scene_diff.robot_state.is_diff = True + self.get_logger().info(f"Goal request is:{goal}") + + if not self.move_sequence_action_client.wait_for_server(timeout): + return False + + goal_handle : MoveGroupSequence_GetResult_Response = self.move_sequence_action_client.send_goal(goal) + if not goal_handle.status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info(f"Move sequence is not executed by ActionServer. Status: {goal_handle.status}") + return False + + self.get_logger().info('Move sequence executed by ActionServer)') + + result: MoveGroupSequence.Result = goal_handle.result + if not result: + self.get_logger().error("No result received from the action server.") + False + + if result.response.error_code.val == MoveItErrorCodes.SUCCESS: + self.get_logger().info("Move sequence executed successfully.") + return True + else: + self.get_logger().error( + f"Move sequence failed with error code: {result.response.error_code.val}" + ) + return False + def did_manip_plan_succeed( self, plan_exec_success: bool, action_name: str, goal_handle: ServerGoalHandle @@ -999,9 +1119,10 @@ def main(args=None): executor = MultiThreadedExecutor() mc = RobotClient() + executor.add_node(mc) try: - rclpy.spin(mc, executor) + executor.spin() except KeyboardInterrupt: pass