From d389977138be38a264862a75ceaf7d3a042847fe Mon Sep 17 00:00:00 2001 From: Henri Fung Date: Fri, 16 Aug 2024 17:14:24 -0700 Subject: [PATCH 01/11] Added Pre-bin Joint States in State Machine note: included flag to return earlier for move to prebin this is such that the robot can compute grasp pose while moving to prebin pose However, this has not been implemented in this commit We will need to check that the moveit goal has been finished, before moving to pregrasp --- aurmr_tasks/scripts/aurmr_demo | 29 ++- aurmr_tasks/src/aurmr_tasks/common/motion.py | 18 ++ .../src/aurmr_tasks/common/perception.py | 34 ++- aurmr_tasks/src/aurmr_tasks/common/tahoma.py | 12 +- tahoma_moveit_config/config/tahoma.srdf | 199 ++++++++++++++++-- 5 files changed, 259 insertions(+), 33 deletions(-) diff --git a/aurmr_tasks/scripts/aurmr_demo b/aurmr_tasks/scripts/aurmr_demo index 0aeceb0..aa01f9b 100755 --- a/aurmr_tasks/scripts/aurmr_demo +++ b/aurmr_tasks/scripts/aurmr_demo @@ -135,19 +135,32 @@ def main(): # These are pre-terminating states that note what went wrong in the userdata. This avoids # encoding the same information in an explosion of named state transitions cf.splat_auto("SPLAT_PICK", "request", ["target_bin_id", "target_object_id", "target_object_asin"]) - # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", planning_scene.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"]) - StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.015), - {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"}) - cf.inject_userdata("NOTE_PASS", "done", "status", PickRequestResponse.OUTCOME_PERCEPTION_FAILURE) + # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", motion.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"]) + + StateMachine.add_auto("CLEAR_SCENE_PREMOVE", motion.ClearCollisionGeometry(robot), ["succeeded", "aborted"]) + StateMachine.add_auto("SETUP_COLLISION_SCENE_PREMOVE", motion.AddFullPodCollisionGeometry(robot), ["succeeded", "aborted"]) + + StateMachine.add_auto("CAPTURE_POINTS", perception.GetPoints(), ["succeeded", "aborted"]) + StateMachine.add_auto("MOVE_TO_PREBIN", motion.MoveToBin(robot), ["succeeded", "aborted"]) + + StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.1), + {"succeeded": "PUBLISH_CURRENTLY_PICKING", "aborted":"PASS", "preempted":"PASS"}) + StateMachine.add("PASS", PublishStatus(status_pub, wait_time=3), {"succeeded": "done"}) + + #StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.015), + # {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"}) + #cf.inject_userdata("NOTE_PASS", "done", "status", PickRequestResponse.OUTCOME_PERCEPTION_FAILURE) + + StateMachine.add_auto("PUBLISH_CURRENTLY_PICKING", PublishStatus(status_pub), ["succeeded"]) StateMachine.add_auto("REMAP_PRE_GRASP_POSE", cf.input_to_output("pre_grasp_pose", "pose"), ["succeeded"]) StateMachine.add_auto("CLEAR_SCENE_PREMOVE", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"]) StateMachine.add_auto("SETUP_COLLISION_SCENE_PREMOVE", planning_scene.AddFullPodCollisionGeometry(robot), ["succeeded", "aborted"]) - StateMachine.add("MOVE_TO_PRE_GRASP", motion.MoveEndEffectorToPoseManipulable(robot), {"succeeded": "CLEAR_SCENE_PREGRASP", "preempted": "NOTE_PREEMPTION", "aborted": "NOTE_PLANNING_FAILURE"}) - cf.inject_userdata("NOTE_PLANNING_FAILURE", "done", "status", PickRequestResponse.OUTCOME_PLANNING_FAILURE) - StateMachine.add_auto("CLEAR_SCENE_PREGRASP", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"]) - StateMachine.add_auto("SETUP_COLLISION_SCENE_PREGRASP", planning_scene.AddPartialPodCollisionGeometry(robot), ["succeeded", "aborted"]) + # FIXME: if `MOVE_TO_PREBIN`` returns early, IMPLEMENT a method to check that move group has reached goal + StateMachine.add_auto("MOVE_TO_PRE_GRASP", motion.MoveEndEffectorToPose(robot), ["succeeded", "aborted", "preempted"]) + StateMachine.add_auto("CLEAR_SCENE_PREGRASP", motion.ClearCollisionGeometry(robot), ["succeeded", "aborted"]) + StateMachine.add_auto("SETUP_COLLISION_SCENE_PREGRASP", motion.AddPartialPodCollisionGeometry(robot), ["succeeded", "aborted"]) StateMachine.add_auto("MOVE_TO_GRASP", motion.grasp_move_to_offset(robot, (0, 0, 0.5)), ["succeeded", "aborted", "preempted"]) StateMachine.add_auto("LIFT_OBJ", motion.robust_move_to_offset(robot, (0, 0, 0.035), 'base_link'), ["succeeded", "aborted", "preempted"]) StateMachine.add_auto("ADJUST_RIGHT_IF_COLUMN_1", motion.AdjustRightIfColumn1(robot, (0, 0.05, 0)), ["succeeded", "aborted", "pass"]) diff --git a/aurmr_tasks/src/aurmr_tasks/common/motion.py b/aurmr_tasks/src/aurmr_tasks/common/motion.py index 1e87363..7905d5d 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/motion.py +++ b/aurmr_tasks/src/aurmr_tasks/common/motion.py @@ -299,6 +299,24 @@ def execute(self, userdata): else: return "succeeded" +class MoveToBin(State): + def __init__(self, robot): + State.__init__(self, input_keys=["target_bin_id"], outcomes=['succeeded', 'aborted']) + self.robot = robot + + def execute(self, ud): + target = ud["target_bin_id"] + if isinstance(target, JointState): + to_log = target.position + else: + target = "pre_bin_" + target.lower() + to_log = target + rospy.loginfo(f"Moving to {to_log}") + success = self.robot.move_to_joint_angles(target, return_before_done=False) + if success: + return "succeeded" + else: + return "aborted" class CloseGripper(State): def __init__(self, robot, return_before_done=False): diff --git a/aurmr_tasks/src/aurmr_tasks/common/perception.py b/aurmr_tasks/src/aurmr_tasks/common/perception.py index acb952e..78dc64a 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/perception.py +++ b/aurmr_tasks/src/aurmr_tasks/common/perception.py @@ -150,6 +150,37 @@ def execute(self, userdata): return "aborted" +<<<<<<< HEAD +======= +class GetPoints(State): + def __init__(self, frame_id='base_link'): + State.__init__( + self, + input_keys=['target_bin_id', 'target_object_id'], + output_keys=['points_response'], + outcomes=['succeeded', 'aborted'] + ) + self.get_points = rospy.ServiceProxy('/aurmr_perception/get_object_points', GetObjectPoints) + self.get_points.wait_for_service(timeout=5) + self.frame_id = frame_id + + def execute(self, userdata): + get_points_req = GetObjectPointsRequest( + bin_id=userdata['target_bin_id'], + object_id=userdata['target_object_id'], + frame_id=self.frame_id + ) + print(get_points_req, get_points_req) + points_response = self.get_points(get_points_req) + + if not points_response.success: + userdata["status"] = "pass" + return "aborted" + + userdata['points_response'] = points_response + return "succeeded" + +>>>>>>> 781599a (Added Pre-bin Joint States in State Machine) class GetGraspPose(State): def __init__(self, tf_buffer, frame_id='base_link', pre_grasp_offset=.12): State.__init__( @@ -158,10 +189,8 @@ def __init__(self, tf_buffer, frame_id='base_link', pre_grasp_offset=.12): output_keys=['grasp_pose', 'pre_grasp_pose'], outcomes=['succeeded', 'preempted', 'aborted'] ) - self.get_points = rospy.ServiceProxy('/aurmr_perception/get_object_points', GetObjectPoints) self.get_grasp = rospy.ServiceProxy('/grasp_detection/detect_grasps', DetectGraspPoses) # Crash during initialization if these aren't running so see the problem early - self.get_points.wait_for_service(timeout=5) self.get_grasp.wait_for_service(timeout=5) self.frame_id = frame_id self.pre_grasp_offset = pre_grasp_offset @@ -180,6 +209,7 @@ def execute(self, userdata): bin_id=userdata['target_bin_id'], object_id=userdata['target_object_id'], frame_id=self.frame_id + ) points_response = self.get_points(get_points_req) diff --git a/aurmr_tasks/src/aurmr_tasks/common/tahoma.py b/aurmr_tasks/src/aurmr_tasks/common/tahoma.py index f694d94..077b7e5 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/tahoma.py +++ b/aurmr_tasks/src/aurmr_tasks/common/tahoma.py @@ -361,8 +361,9 @@ def move_to_joint_angles(self, plan_only=False, replan=False, replan_attempts=5, - tolerance=0.01, - path_constraints=None): + tolerance=0.005, + path_constraints=None, + return_before_done=False): """Moves the end-effector to a pose, using motion planning. Args: @@ -407,7 +408,7 @@ def move_to_joint_angles(self, self.move_group.set_named_target(joints) else: self.move_group.set_joint_value_target(joints) - self.move_group.go(wait=True) + self.move_group.go(wait=not return_before_done) # Calling ``stop()`` ensures that there is no residual movement self.move_group.stop() @@ -941,3 +942,8 @@ def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)): def cancel_all_goals(self): self._move_group_client.cancel_all_goals() self._joint_traj_client.cancel_all_goals() + + def reached_goal(self): + while self._move_group_client.get_state() == GoalStatus.ACTIVE: + continue + return True \ No newline at end of file diff --git a/tahoma_moveit_config/config/tahoma.srdf b/tahoma_moveit_config/config/tahoma.srdf index b5bd223..aeda5f9 100644 --- a/tahoma_moveit_config/config/tahoma.srdf +++ b/tahoma_moveit_config/config/tahoma.srdf @@ -48,30 +48,189 @@ - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 2e081db930ed6bd3d1e88f5496a9e36445f63c5e Mon Sep 17 00:00:00 2001 From: Henri Fung Date: Tue, 20 Aug 2024 18:08:50 -0700 Subject: [PATCH 02/11] update pre bin poses --- tahoma_moveit_config/config/tahoma.srdf | 49 +++++++++++++------------ 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/tahoma_moveit_config/config/tahoma.srdf b/tahoma_moveit_config/config/tahoma.srdf index aeda5f9..3110507 100644 --- a/tahoma_moveit_config/config/tahoma.srdf +++ b/tahoma_moveit_config/config/tahoma.srdf @@ -60,13 +60,14 @@ JOINT STATES SHOULD BE SET TO ENABLE MAX MANIPULABILITY --> - - - - - - + + + + + + + @@ -101,12 +102,12 @@ - - - - - - + + + + + + @@ -126,20 +127,20 @@ - - - - - - + + + + + + - - - - - - + + + + + + From 8923d93b1b0741a3ea643b6ec2d40bffd7a59c86 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:09:16 -0700 Subject: [PATCH 03/11] change motion planning parameters --- aurmr_tasks/src/aurmr_tasks/common/motion.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/aurmr_tasks/src/aurmr_tasks/common/motion.py b/aurmr_tasks/src/aurmr_tasks/common/motion.py index 7905d5d..e84a837 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/motion.py +++ b/aurmr_tasks/src/aurmr_tasks/common/motion.py @@ -85,13 +85,13 @@ def execute(self, userdata): pose = userdata["pose"] success = self.robot.move_to_pose( pose, - allowed_planning_time=10.0, + allowed_planning_time=20.0, execution_timeout=15.0, - num_planning_attempts=12, + num_planning_attempts=40, orientation_constraint=None, replan=True, - replan_attempts=8, - tolerance=0.01) + replan_attempts=20, + tolerance=0.02) if success: return "succeeded" else: @@ -368,4 +368,3 @@ def execute(self, ud): if not self.robot.check_gripper_item(): self.robot.open_gripper(return_before_done=True) return 'succeeded' - From 37a80114259a00759022e078a8881cc1e5d39945 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:09:40 -0700 Subject: [PATCH 04/11] add try/catch blocks to perception states --- .../src/aurmr_tasks/common/perception.py | 78 ++++++++++--------- 1 file changed, 42 insertions(+), 36 deletions(-) diff --git a/aurmr_tasks/src/aurmr_tasks/common/perception.py b/aurmr_tasks/src/aurmr_tasks/common/perception.py index 78dc64a..5e51b92 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/perception.py +++ b/aurmr_tasks/src/aurmr_tasks/common/perception.py @@ -150,8 +150,6 @@ def execute(self, userdata): return "aborted" -<<<<<<< HEAD -======= class GetPoints(State): def __init__(self, frame_id='base_link'): State.__init__( @@ -165,27 +163,29 @@ def __init__(self, frame_id='base_link'): self.frame_id = frame_id def execute(self, userdata): - get_points_req = GetObjectPointsRequest( - bin_id=userdata['target_bin_id'], - object_id=userdata['target_object_id'], - frame_id=self.frame_id - ) - print(get_points_req, get_points_req) - points_response = self.get_points(get_points_req) + try: + get_points_req = GetObjectPointsRequest( + bin_id=userdata['target_bin_id'], + object_id=userdata['target_object_id'], + frame_id=self.frame_id + ) + print(get_points_req, get_points_req) + points_response = self.get_points(get_points_req) - if not points_response.success: - userdata["status"] = "pass" - return "aborted" + if not points_response.success: + userdata["status"] = "pass" + return "aborted" - userdata['points_response'] = points_response - return "succeeded" + userdata['points_response'] = points_response + return "succeeded" + except: + return "aborted" ->>>>>>> 781599a (Added Pre-bin Joint States in State Machine) class GetGraspPose(State): def __init__(self, tf_buffer, frame_id='base_link', pre_grasp_offset=.12): State.__init__( self, - input_keys=['target_bin_id', 'target_object_id'], + input_keys=['target_bin_id', 'target_object_id', 'points_response'], output_keys=['grasp_pose', 'pre_grasp_pose'], outcomes=['succeeded', 'preempted', 'aborted'] ) @@ -211,33 +211,39 @@ def execute(self, userdata): frame_id=self.frame_id ) - points_response = self.get_points(get_points_req) - if not points_response.success: - return "aborted" + try: - grasp_response = self.get_grasp(points=points_response.points, - mask=points_response.mask, - dist_threshold=self.pre_grasp_offset, bin_id=userdata['target_bin_id']) + points_response = userdata["points_response"] - if not grasp_response.success: - return "aborted" + if not points_response.success: + return "aborted" - # NOTE: No extra filtering or ranking on our part. Just take the first one - # As the arm_tool0 is 26cm in length w.r.t tip of suction cup thus adding 0.26m offset - # TODO(henrifung): Pull this number from xacro, perhaps from userdata - grasp_pose = grasp_response.poses[0] - grasp_pose = add_offset(-0.26, grasp_pose) + grasp_response = self.get_grasp(points=points_response.points, + mask=points_response.mask, + dist_threshold=self.pre_grasp_offset, bin_id=userdata['target_bin_id']) - userdata['grasp_pose'] = grasp_pose + if not grasp_response.success: + return "aborted" - # adding 0.12m offset for pre grasp pose to prepare it for grasp pose which is use to pick the object - pregrasp_pose = add_offset(-self.pre_grasp_offset, grasp_pose) + # NOTE: No extra filtering or ranking on our part. Just take the first one + # As the arm_tool0 is 26cm in length w.r.t tip of suction cup thus adding 0.26m offset + # TODO(henrifung): Pull this number from xacro, perhaps from userdata + grasp_pose = grasp_response.poses[0] - userdata['pre_grasp_pose'] = pregrasp_pose + grasp_pose = add_offset(-0.26, grasp_pose) - self.pose_viz.publish(grasp_pose) - self.pre_grasp_viz.publish(pregrasp_pose) + userdata['grasp_pose'] = grasp_pose - return "succeeded" + # adding 0.12m offset for pre grasp pose to prepare it for grasp pose which is use to pick the object + pregrasp_pose = add_offset(-self.pre_grasp_offset, grasp_pose) + + userdata['pre_grasp_pose'] = pregrasp_pose + + self.pose_viz.publish(grasp_pose) + self.pre_grasp_viz.publish(pregrasp_pose) + + return "succeeded" + except: + return "aborted" From 36215ea11ca0057c4b8c72d22585713cf383b3e2 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:10:41 -0700 Subject: [PATCH 05/11] configure moveit to use STOMP --- .../config/stomp_planning.yaml | 37 +++++++++++++++++++ tahoma_moveit_config/launch/demo.launch | 2 +- tahoma_moveit_config/launch/move_group.launch | 2 +- .../launch/stomp_planning_pipeline.launch.xml | 33 +++++++++++++++++ 4 files changed, 72 insertions(+), 2 deletions(-) create mode 100644 tahoma_moveit_config/config/stomp_planning.yaml create mode 100644 tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml diff --git a/tahoma_moveit_config/config/stomp_planning.yaml b/tahoma_moveit_config/config/stomp_planning.yaml new file mode 100644 index 0000000..97192c5 --- /dev/null +++ b/tahoma_moveit_config/config/stomp_planning.yaml @@ -0,0 +1,37 @@ +optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 +task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized diff --git a/tahoma_moveit_config/launch/demo.launch b/tahoma_moveit_config/launch/demo.launch index 8db5e25..d66b770 100644 --- a/tahoma_moveit_config/launch/demo.launch +++ b/tahoma_moveit_config/launch/demo.launch @@ -1,7 +1,7 @@ - + diff --git a/tahoma_moveit_config/launch/move_group.launch b/tahoma_moveit_config/launch/move_group.launch index ae38de5..906a8da 100644 --- a/tahoma_moveit_config/launch/move_group.launch +++ b/tahoma_moveit_config/launch/move_group.launch @@ -12,7 +12,7 @@ - + diff --git a/tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml b/tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..3beccab --- /dev/null +++ b/tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + From fcaf84181ee4cf05de762a9852e0de03e684f329 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:11:10 -0700 Subject: [PATCH 06/11] tune parameters for compliance controller --- tahoma_bringup/config/controllers.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tahoma_bringup/config/controllers.yaml b/tahoma_bringup/config/controllers.yaml index ab98dcb..0a1b14f 100644 --- a/tahoma_bringup/config/controllers.yaml +++ b/tahoma_bringup/config/controllers.yaml @@ -221,11 +221,11 @@ ur_cartesian_compliance_controller: rot_x: 5.0 rot_y: 5.0 rot_z: 5.0 - max_translation_error_norm: 25 - max_rotation_error_norm: 5.14 + max_translation_error_norm: 0.2 + max_rotation_error_norm: 0.1 solver: - error_scale: 0.35 + error_scale: 0.5 iterations: 1 publish_state_feedback: True From 85b2567b951702c943d078b07072bb9d37a6b1f1 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:11:54 -0700 Subject: [PATCH 07/11] revert to drop_hide pose --- aurmr_tasks/scripts/aurmr_demo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aurmr_tasks/scripts/aurmr_demo b/aurmr_tasks/scripts/aurmr_demo index aa01f9b..535cb98 100755 --- a/aurmr_tasks/scripts/aurmr_demo +++ b/aurmr_tasks/scripts/aurmr_demo @@ -189,7 +189,7 @@ def main(): with pick_retry_sm: StateMachine.add_auto("BLOW_OFF_GRIPPER", motion.BlowOffGripper(robot, return_before_done=True), ["succeeded", "aborted", "preempted"]) - StateMachine.add_auto("UNBLOCK_VIEW", motion.MoveToJointAngles(robot, "hide"), ["succeeded", "aborted"]) + StateMachine.add_auto("UNBLOCK_VIEW", motion.MoveToJointAngles(robot, "drop_hide"), ["succeeded", "aborted"]) StateMachine.add_auto("PICTURE_WAIT", Wait(2), ["succeeded"]) StateMachine.add_auto("UPDATE_BIN_MASKS", perception.UpdateBin(), ["succeeded", "preempted", "aborted"]) StateMachine.add('USER_PROMPT_FOR_RETRY', UserPromptForRetry(rospy.get_param("bin_bounds"), robot.tf2_buffer, timeout_connection_secs=1.0), {"retry": "REMAP_GRASP", "continue": "continue"}) From 18ea521716a7a9d9c84d3fd6a9b5af739ad582ab Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:13:07 -0700 Subject: [PATCH 08/11] move robot to pre bin pose --- aurmr_tasks/scripts/aurmr_demo | 41 +++++++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 11 deletions(-) diff --git a/aurmr_tasks/scripts/aurmr_demo b/aurmr_tasks/scripts/aurmr_demo index 535cb98..7d3089f 100755 --- a/aurmr_tasks/scripts/aurmr_demo +++ b/aurmr_tasks/scripts/aurmr_demo @@ -23,10 +23,28 @@ from aurmr_perception.util import vec_to_quat_msg, I_QUAT from aurmr_tasks.common.states import Wait from aurmr_tasks.common.hri import UserPromptForRetry +from aurmr_tasks.common import compliance_controller from queue import Empty, Queue - - +class PublishStatus(State): + def __init__(self, publisher, wait_time=0): + State.__init__(self, outcomes=['succeeded'], input_keys=["status", "start_time", "target_object_id"]) + self.publisher=publisher + self.wait_time = wait_time + def execute(self, userdata): + now = rospy.get_rostime() + now = now.secs + now.nsecs*10**(-9) + time_delta = now - userdata["start_time"] + + + msg = PickStatus() + msg.status = userdata["status"] + msg.object_id = userdata["target_object_id"] + msg.time = time_delta + # self.publisher.publish("test") + self.publisher.publish(msg) + rospy.sleep(self.wait_time) + return "succeeded" class PickStowQueue: def __init__(self, pick_sm) -> None: @@ -137,21 +155,22 @@ def main(): cf.splat_auto("SPLAT_PICK", "request", ["target_bin_id", "target_object_id", "target_object_asin"]) # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", motion.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"]) - StateMachine.add_auto("CLEAR_SCENE_PREMOVE", motion.ClearCollisionGeometry(robot), ["succeeded", "aborted"]) - StateMachine.add_auto("SETUP_COLLISION_SCENE_PREMOVE", motion.AddFullPodCollisionGeometry(robot), ["succeeded", "aborted"]) StateMachine.add_auto("CAPTURE_POINTS", perception.GetPoints(), ["succeeded", "aborted"]) - StateMachine.add_auto("MOVE_TO_PREBIN", motion.MoveToBin(robot), ["succeeded", "aborted"]) - StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.1), - {"succeeded": "PUBLISH_CURRENTLY_PICKING", "aborted":"PASS", "preempted":"PASS"}) - StateMachine.add("PASS", PublishStatus(status_pub, wait_time=3), {"succeeded": "done"}) + StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.065), + {"succeeded": "MOVE_TO_BIN", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"}) + + #StateMachine.add("PASS", PublishStatus(status_pub, wait_time=3), {"succeeded": "done"}) - #StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.015), + #StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.035), # {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"}) - #cf.inject_userdata("NOTE_PASS", "done", "status", PickRequestResponse.OUTCOME_PERCEPTION_FAILURE) + cf.inject_userdata("NOTE_PASS", "done", "status", PickRequestResponse.OUTCOME_PERCEPTION_FAILURE) + + StateMachine.add_auto("MOVE_TO_BIN", motion.MoveToBin(robot), ["succeeded", "aborted"]) - StateMachine.add_auto("PUBLISH_CURRENTLY_PICKING", PublishStatus(status_pub), ["succeeded"]) + #StateMachine.add_auto("PUBLISH_CURRENTLY_PICKING", PublishStatus(status_pub), ["succeeded"]) + # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", planning_scene.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"]) StateMachine.add_auto("REMAP_PRE_GRASP_POSE", cf.input_to_output("pre_grasp_pose", "pose"), ["succeeded"]) StateMachine.add_auto("CLEAR_SCENE_PREMOVE", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"]) From 9c81aa9bf25112dba3d4adec448e9b997fa9ae54 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:13:37 -0700 Subject: [PATCH 09/11] update compliance controller --- .../common/compliance_controller.py | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py b/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py index ccc1950..ea353d4 100644 --- a/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py +++ b/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py @@ -4,11 +4,14 @@ from geometry_msgs.msg import PoseStamped import time +import numpy as np +from aurmr_tasks.util import pose_dist + from aurmr_tasks.common.visualization_utils import EEFVisualization class MoveToOffset(State): - def __init__(self, robot, offset, frame_id, detect_object,idle_timeout=3): - super().__init__(outcomes=['succeeded', 'aborted'], output_keys=['starting_grasp_pose_out'], input_keys=['target_pose_in']) + def __init__(self, robot, offset, frame_id, detect_object, idle_timeout=3): + super().__init__(outcomes=['succeeded', 'aborted'], input_keys=['offset_moved_in'], output_keys=['offset_moved_out']) self.robot = robot self.offset = offset self.frame_id = frame_id @@ -28,30 +31,36 @@ def stop(self): def execute(self, userdata): self.close_robot_gripper() - start_pose = self.robot.move_group.get_current_pose() - start_pose.header.stamp = rospy.Time(0) - start_pose = self.robot.tf2_buffer.transform(start_pose, self.target_frame, rospy.Duration(1)) - - userdata.starting_grasp_pose_out = start_pose - rospy.logdebug("setting starting pose to ", start_pose) - self.vis_out.visualize_eef(start_pose) + start_pose = self.current_pose_in_target_frame() - if userdata.target_pose_in: - target_pose = userdata.target_pose_in + if self.offset is None: + offset = userdata.offset_moved_in else: - target_pose = self.calculate_target_pose(start_pose) - self.robot.force_values_init = True + offset = self.offset + + target_pose = self.calculate_target_pose(start_pose, offset) + self.robot.force_values_init = True self.vis.visualize_eef(target_pose) self.publish_target_pose(target_pose) - return self.wait_until_stopped(0.001, self.idle_timeout) + result = self.wait_until_stopped(0.001, self.idle_timeout) + + current_pose = self.current_pose_in_target_frame() + current_pose = current_pose.pose + start_pose = start_pose.pose + offset_moved = np.array([start_pose.position.x, start_pose.position.y, start_pose.position.z]) + offset_moved = offset_moved - np.array([current_pose.position.x, current_pose.position.y, current_pose.position.z]) + + userdata.offset_moved_out = offset_moved + + return result def close_robot_gripper(self): self.robot.close_gripper(return_before_done=True) - def calculate_target_pose(self, start_pose): - target_pose = apply_offset_to_pose(start_pose, self.offset, self.frame_id, self.robot.tf2_buffer) + def calculate_target_pose(self, start_pose, offset): + target_pose = apply_offset_to_pose(start_pose, offset, self.frame_id, self.robot.tf2_buffer) return self.robot.tf2_buffer.transform(target_pose, self.target_frame, rospy.Duration(1)) def publish_target_pose(self, target_pose): @@ -99,7 +108,6 @@ def current_pose_in_target_frame(self): return current_pose - def check_object_condition(self): return (self.detect_object and self.robot.check_gripper_item()) From b102230b3eaa83fa5246fc0579929c51480ab1be Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:15:14 -0700 Subject: [PATCH 10/11] add stomp libraries to third-party --- .gitmodules | 6 ++++++ third-party/stomp | 1 + third-party/stomp_ros | 1 + 3 files changed, 8 insertions(+) create mode 160000 third-party/stomp create mode 160000 third-party/stomp_ros diff --git a/.gitmodules b/.gitmodules index e27d30d..822a172 100644 --- a/.gitmodules +++ b/.gitmodules @@ -43,3 +43,9 @@ [submodule "third-party/usb_cam"] path = third-party/usb_cam url = git@github.com:ros-drivers/usb_cam.git +[submodule "third-party/stomp_ros"] + path = third-party/stomp_ros + url = https://github.com/ros-industrial/stomp_ros.git +[submodule "third-party/stomp"] + path = third-party/stomp + url = https://github.com/ros-industrial/stomp.git diff --git a/third-party/stomp b/third-party/stomp new file mode 160000 index 0000000..f9e9d78 --- /dev/null +++ b/third-party/stomp @@ -0,0 +1 @@ +Subproject commit f9e9d78f741f0779b65333d4f54d68ade740520b diff --git a/third-party/stomp_ros b/third-party/stomp_ros new file mode 160000 index 0000000..882784c --- /dev/null +++ b/third-party/stomp_ros @@ -0,0 +1 @@ +Subproject commit 882784c6952e6456c85d0f61686913a33309bcfe From ebbdd7ecf244d0584951ca651e7a10241e619708 Mon Sep 17 00:00:00 2001 From: Markus Grotz Date: Tue, 20 Aug 2024 18:15:58 -0700 Subject: [PATCH 11/11] add ros_industrial_cmake_boilerplate dependency --- .gitmodules | 3 +++ third-party/ros_industrial_cmake_boilerplate | 1 + 2 files changed, 4 insertions(+) create mode 160000 third-party/ros_industrial_cmake_boilerplate diff --git a/.gitmodules b/.gitmodules index 822a172..7456888 100644 --- a/.gitmodules +++ b/.gitmodules @@ -49,3 +49,6 @@ [submodule "third-party/stomp"] path = third-party/stomp url = https://github.com/ros-industrial/stomp.git +[submodule "third-party/ros_industrial_cmake_boilerplate"] + path = third-party/ros_industrial_cmake_boilerplate + url = https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git diff --git a/third-party/ros_industrial_cmake_boilerplate b/third-party/ros_industrial_cmake_boilerplate new file mode 160000 index 0000000..559e91b --- /dev/null +++ b/third-party/ros_industrial_cmake_boilerplate @@ -0,0 +1 @@ +Subproject commit 559e91b734f3e23131567404be323926883dcce7