From 73e1c50cab54e9310f0aef7d2222de2dda2360de Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 17:06:57 -0700 Subject: [PATCH 01/10] Silence noisy debug ContactDetector msgs --- stretch_funmap/nodes/funmap | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/stretch_funmap/nodes/funmap b/stretch_funmap/nodes/funmap index 89d5b9d..7e33ef3 100755 --- a/stretch_funmap/nodes/funmap +++ b/stretch_funmap/nodes/funmap @@ -239,10 +239,10 @@ class FunmapNode(hm.HelloNode): av_effort_threshold = 34.0 if (effort >= single_effort_threshold): - rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format( + rospy.logdebug('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format( effort, single_effort_threshold)) if (av_effort >= av_effort_threshold): - rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format( + rospy.logdebug('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format( av_effort, av_effort_threshold)) return ((effort >= single_effort_threshold) or @@ -257,10 +257,10 @@ class FunmapNode(hm.HelloNode): av_effort_threshold = 40.0 if (effort >= single_effort_threshold): - rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format( + rospy.logdebug('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format( effort, single_effort_threshold)) if (av_effort >= av_effort_threshold): - rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format( + rospy.logdebug('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format( av_effort, av_effort_threshold)) return ((effort >= single_effort_threshold) or @@ -274,10 +274,10 @@ class FunmapNode(hm.HelloNode): av_effort_threshold = 20.0 if (effort <= single_effort_threshold): - rospy.loginfo('Lift single effort less than single_effort_threshold: {0} <= {1}'.format( + rospy.logdebug('Lift single effort less than single_effort_threshold: {0} <= {1}'.format( effort, single_effort_threshold)) if (av_effort <= av_effort_threshold): - rospy.loginfo('Lift average effort less than av_effort_threshold: {0} <= {1}'.format( + rospy.logdebug('Lift average effort less than av_effort_threshold: {0} <= {1}'.format( av_effort, av_effort_threshold)) return ((effort <= single_effort_threshold) or From 8626c4af365d2fa61593f72de197a7761c606a93 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 17:07:36 -0700 Subject: [PATCH 02/10] Add home/stow/stop_the_robot funcs to HelloNode --- hello_helpers/src/hello_helpers/hello_misc.py | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index d916b09..880d4e9 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -164,6 +164,24 @@ class HelloNode: continue rate.sleep() + def home_the_robot(self): + trigger_request = TriggerRequest() + trigger_result = self.home_the_robot_service(trigger_request) + rospy.logdebug(f"{self.node_name}'s HelloNode.home_the_robot: got message {trigger_result.message}") + return trigger_result.success + + def stow_the_robot(self): + trigger_request = TriggerRequest() + trigger_result = self.stow_the_robot_service(trigger_request) + rospy.logdebug(f"{self.node_name}'s HelloNode.stow_the_robot: got message {trigger_result.message}") + return trigger_result.success + + def stop_the_robot(self): + trigger_request = TriggerRequest() + trigger_result = self.stop_the_robot_service(trigger_request) + rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}") + return trigger_result.success + def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True): rospy.init_node(node_name) self.node_name = rospy.get_name() @@ -181,10 +199,14 @@ class HelloNode: self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback) self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1) + rospy.wait_for_service('/home_the_robot') + rospy.wait_for_service('/stow_the_robot') rospy.wait_for_service('/stop_the_robot') - rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.') + rospy.loginfo('Node ' + self.node_name + ' connected to robot services.') + self.home_the_robot_service = rospy.ServiceProxy('/home_the_robot', Trigger) + self.stow_the_robot_service = rospy.ServiceProxy('/stow_the_robot', Trigger) self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger) - + if wait_for_first_pointcloud: # Do not start until a point cloud has been received point_cloud_msg = self.point_cloud From 78661e8dc3324ded402fdbfd0be57ec6ec59dfee Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 17:08:33 -0700 Subject: [PATCH 03/10] Add stow step to grasp_object + minor fixes --- stretch_demos/nodes/grasp_object | 52 ++++++++++---------------------- 1 file changed, 16 insertions(+), 36 deletions(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 8c8ea3f..0fe7371 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -41,7 +41,7 @@ class GraspObjectNode(hm.HelloNode): self.lift_position = None self.manipulation_view = None self.debug_directory = None - + def joint_states_callback(self, joint_states): with self.joint_states_lock: self.joint_states = joint_states @@ -50,20 +50,12 @@ class GraspObjectNode(hm.HelloNode): lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states) self.lift_position = lift_position self.left_finger_position, temp1, temp2 = hm.get_left_finger_state(joint_states) - + def lower_tool_until_contact(self): rospy.loginfo('lower_tool_until_contact') trigger_request = TriggerRequest() trigger_result = self.trigger_lower_until_contact_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - - def move_to_initial_configuration(self): - initial_pose = {'wrist_extension': 0.01, - 'joint_wrist_yaw': 0.0, - 'gripper_aperture': 0.125} - - rospy.loginfo('Move to the initial configuration for drawer opening.') - self.move_to_pose(initial_pose) def look_at_surface(self, scan_time_s=None): self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory) @@ -99,7 +91,8 @@ class GraspObjectNode(hm.HelloNode): max_lift_m = 1.09 min_extension_m = 0.01 max_extension_m = 0.5 - + + # 1. Initial configuration use_default_mode = False if use_default_mode: # Set the D435i to Default mode for obstacle detection @@ -108,16 +101,11 @@ class GraspObjectNode(hm.HelloNode): rospy.loginfo('trigger_result = {0}'.format(trigger_result)) if actually_move: - rospy.loginfo('Retract the tool.') - pose = {'wrist_extension': 0.01} - self.move_to_pose(pose) + rospy.loginfo('Stow the arm.') + self.stow_the_robot() - rospy.loginfo('Reorient the wrist.') - pose = {'joint_wrist_yaw': 0.0} - self.move_to_pose(pose) - + # 2. Scan surface and find grasp target self.look_at_surface(scan_time_s = 3.0) - grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer) if grasp_target is None: return TriggerResponse( @@ -125,14 +113,13 @@ class GraspObjectNode(hm.HelloNode): message='Failed to find grasp target' ) + # 3. Move to pregrasp pose pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer) - if (self.lift_position is None): return TriggerResponse( success=False, message='lift position unavailable' ) - if actually_move: rospy.loginfo('Raise tool to pregrasp height.') lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) @@ -141,23 +128,20 @@ class GraspObjectNode(hm.HelloNode): self.move_to_pose(pose) pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) - print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) - print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) - + rospy.loginfo('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) + rospy.loginfo('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) if actually_move: rospy.loginfo('Rotate the gripper for grasping.') pose = {'joint_wrist_yaw': pregrasp_yaw} self.move_to_pose(pose) - + rospy.loginfo('Open the gripper.') pose = {'gripper_aperture': 0.125} self.move_to_pose(pose) pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) - - print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) - print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) - + rospy.loginfo('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) + rospy.loginfo('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) if actually_move: rospy.loginfo('Drive to pregrasp location.') self.drive(pregrasp_mobile_base_m) @@ -169,11 +153,11 @@ class GraspObjectNode(hm.HelloNode): pose = {'wrist_extension': extension_m} self.move_to_pose(pose) else: - print('negative wrist extension for pregrasp, so not extending or retracting.') + rospy.loginfo('negative wrist extension for pregrasp, so not extending or retracting.') + # 4. Grasp the object and lift it grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) - print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) - + rospy.loginfo('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) if actually_move: rospy.loginfo('Move the grasp pose from the pregrasp pose.') @@ -200,10 +184,8 @@ class GraspObjectNode(hm.HelloNode): rospy.loginfo('Attempt to lift the object.') object_lift_height_m = 0.1 - lift_m = max(self.lift_position + object_lift_height_m, 0.2) lift_m = min(lift_m, max_lift_m) - pose = {'joint_lift': lift_m} self.move_to_pose(pose) @@ -211,7 +193,6 @@ class GraspObjectNode(hm.HelloNode): pose = {'gripper_aperture': gripper_aperture_m + 0.005} self.move_to_pose(pose) - if actually_move: rospy.loginfo('Retract the tool.') pose = {'wrist_extension': 0.01} self.move_to_pose(pose) @@ -225,7 +206,6 @@ class GraspObjectNode(hm.HelloNode): message='Completed object grasp!' ) - def main(self): hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=False) From ba3e4c8b249363b0093543273d295f005ec7dd5c Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 18:38:54 -0700 Subject: [PATCH 04/10] Add stretch_driver topic for end_of_arm tool --- hello_helpers/src/hello_helpers/hello_misc.py | 15 +++++++++++++-- stretch_core/nodes/stretch_driver | 6 ++++++ stretch_demos/nodes/grasp_object | 2 +- .../src/stretch_funmap/manipulation_planning.py | 15 ++++++++++----- 4 files changed, 30 insertions(+), 8 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 880d4e9..4301786 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -20,6 +20,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint import tf2_ros from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger, TriggerRequest +from std_msgs.msg import String ####################### @@ -69,6 +70,7 @@ class HelloNode: def __init__(self): self.joint_state = None self.point_cloud = None + self.tool = None @classmethod def quick_create(cls, name, wait_for_first_pointcloud=False): @@ -81,7 +83,10 @@ class HelloNode: def point_cloud_callback(self, point_cloud): self.point_cloud = point_cloud - + + def tool_callback(self, tool_string): + self.tool = tool_string.data + def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(0.0) @@ -182,6 +187,10 @@ class HelloNode: rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}") return trigger_result.success + def get_tool(self): + assert(self.tool is not None) + return self.tool + def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True): rospy.init_node(node_name) self.node_name = rospy.get_name() @@ -195,7 +204,9 @@ class HelloNode: self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) - + + self.tool_subscriber = rospy.Subscriber('/tool', String, self.tool_callback) + self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback) self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 66b2220..249dc6a 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -197,6 +197,11 @@ class StretchDriverNode: mode_msg.data = self.robot_mode self.mode_pub.publish(mode_msg) + # publish end of arm tool + tool_msg = String() + tool_msg.data = self.robot.end_of_arm.name + self.tool_pub.publish(tool_msg) + ################################################## # publish joint state joint_state = JointState() @@ -508,6 +513,7 @@ class StretchDriverNode: self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1) self.homed_pub = rospy.Publisher('is_homed', Bool, queue_size=1) self.mode_pub = rospy.Publisher('mode', String, queue_size=1) + self.tool_pub = rospy.Publisher('tool', String, queue_size=1) self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1) self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 0fe7371..3e6aa1a 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -58,7 +58,7 @@ class GraspObjectNode(hm.HelloNode): rospy.loginfo('trigger_result = {0}'.format(trigger_result)) def look_at_surface(self, scan_time_s=None): - self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory) + self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory, self.get_tool()) manip = self.manipulation_view head_settle_time_s = 0.02 #1.0 manip.move_head(self.move_to_pose) diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index 0629c2a..16e43ac 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -198,10 +198,15 @@ def detect_cliff(image, m_per_pix, m_per_height_unit, robot_xy_pix, display_text class ManipulationView(): - def __init__(self, tf2_buffer, debug_directory=None): + def __init__(self, tf2_buffer, debug_directory=None, tool='tool_stretch_gripper'): self.debug_directory = debug_directory print('ManipulationView __init__: self.debug_directory =', self.debug_directory) - + self.gripper_links = { + 'tool_stretch_gripper': 'link_gripper', + 'tool_stretch_dex_wrist': 'link_straight_gripper' + } + self.tool = tool + # Define the volume of interest for planning using the current # view. @@ -420,7 +425,7 @@ class ManipulationView(): # The planar component of the link gripper x_axis is parallel # to the middle of the gripper, but points in the opposite # direction. - gripper_frame = 'link_gripper' + gripper_frame = self.gripper_links[self.tool] gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer) # # forward_direction = np.array([1.0, 0.0, 0.0]) @@ -469,7 +474,7 @@ class ManipulationView(): # The planar component of the link gripper x_axis is parallel # to the middle of the gripper, but points in the opposite # direction. - gripper_frame = 'link_gripper' + gripper_frame = self.gripper_links[self.tool] gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer) # Obtain the gripper yaw axis location in the image by @@ -582,7 +587,7 @@ class ManipulationView(): # The planar component of the link gripper x_axis is parallel # to the middle of the gripper, but points in the opposite # direction. - gripper_frame = 'link_gripper' + gripper_frame = self.gripper_links[self.tool] gripper_points_to_image_mat, ip_timestamp = h.get_points_to_image_mat(gripper_frame, tf2_buffer) # Obtain the gripper yaw axis location in the image by From cb6b800b676e78efc0e3d3eefcd18685c22bb554 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 19:33:34 -0700 Subject: [PATCH 05/10] Add pregrasp pitch/roll to grasp_object --- stretch_demos/nodes/grasp_object | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 3e6aa1a..32e90e3 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -127,6 +127,11 @@ class GraspObjectNode(hm.HelloNode): pose = {'joint_lift': lift_to_pregrasp_m} self.move_to_pose(pose) + if actually_move and self.tool == "tool_stretch_dex_wrist": + rospy.loginfo('Rotate pitch/roll for grasping.') + pose = {'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0} + self.move_to_pose(pose) + pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) rospy.loginfo('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) rospy.loginfo('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) From 1c6e1de17f3e28b28a98a05fd9f12761453a9322 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 19:34:02 -0700 Subject: [PATCH 06/10] Add hack to align link_straight_gripper like link_gripper --- stretch_demos/launch/grasp_object.launch | 4 ++++ stretch_funmap/src/stretch_funmap/manipulation_planning.py | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/stretch_demos/launch/grasp_object.launch b/stretch_demos/launch/grasp_object.launch index 2c845af..2de04cc 100644 --- a/stretch_demos/launch/grasp_object.launch +++ b/stretch_demos/launch/grasp_object.launch @@ -30,6 +30,10 @@ + + + + diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index 16e43ac..a979d13 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -203,7 +203,7 @@ class ManipulationView(): print('ManipulationView __init__: self.debug_directory =', self.debug_directory) self.gripper_links = { 'tool_stretch_gripper': 'link_gripper', - 'tool_stretch_dex_wrist': 'link_straight_gripper' + 'tool_stretch_dex_wrist': 'link_straight_gripper_aligned' } self.tool = tool From ec3907518fa8cdf9d2f39f6860040dec93680b0a Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 21:05:46 -0700 Subject: [PATCH 07/10] Fix keyboard_teleop menu not printing after service call --- stretch_core/nodes/keyboard_teleop | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop index cf1b96a..453f6a2 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/stretch_core/nodes/keyboard_teleop @@ -126,48 +126,56 @@ class GetKeyboardCommands: trigger_request = TriggerRequest() trigger_result = node.trigger_drive_to_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger localizing the robot to a new pose anywhere on the current map if ((c == '+') or (c == '=')) and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_global_localization_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger localizing the robot to a new pose that is near its current pose on the map if ((c == '-') or (c == '_')) and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_local_localization_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger driving the robot to the estimated next best place to perform a 3D scan if ((c == '\\') or (c == '|')) and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_drive_to_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger performing a 3D scan using the D435i if (c == ' ') and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_head_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger rotating the mobile base to align with the nearest 3D cliff detected visually if ((c == '[') or (c == '{')) and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_align_with_nearest_cliff_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # DEPRECATED: Trigger extend arm until contact if ((c == ']') or (c == '}')) and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_reach_until_contact_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # DEPRECATED: Trigger lower arm until contact if ((c == ':') or (c == ';')) and self.mapping_on: trigger_request = TriggerRequest() trigger_result = node.trigger_lower_until_contact_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' #################################################### @@ -179,36 +187,42 @@ class GetKeyboardCommands: trigger_request = TriggerRequest() trigger_result = node.trigger_write_hello_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger open drawer demo with downward hook motion if ((c == 'z') or (c == 'Z')) and self.open_drawer_on: trigger_request = TriggerRequest() trigger_result = node.trigger_open_drawer_down_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger open drawer demo with upward hook motion if ((c == '.') or (c == '>')) and self.open_drawer_on: trigger_request = TriggerRequest() trigger_result = node.trigger_open_drawer_up_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger clean surface demo if ((c == '/') or (c == '?')) and self.clean_surface_on: trigger_request = TriggerRequest() trigger_result = node.trigger_clean_surface_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger grasp object demo if ((c == '\'') or (c == '\"')) and self.grasp_object_on: trigger_request = TriggerRequest() trigger_result = node.trigger_grasp_object_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' # Trigger deliver object demo if ((c == 'y') or (c == 'Y')) and self.deliver_object_on: trigger_request = TriggerRequest() trigger_result = node.trigger_deliver_object_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + command = 'service_occurred' #################################################### @@ -311,7 +325,7 @@ class KeyboardTeleopNode(hm.HelloNode): def send_command(self, command): joint_state = self.joint_state - if (joint_state is not None) and (command is not None): + if (joint_state is not None) and (command is not None) and (isinstance(command, dict)): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(0.0) trajectory_goal = FollowJointTrajectoryGoal() From 5349cd01ecc7fc777f93c42eb59b8ebda3dad73a Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Mon, 10 Jul 2023 21:06:26 -0700 Subject: [PATCH 08/10] Add grasp_object rviz config --- stretch_demos/launch/grasp_object.launch | 12 +- stretch_demos/rviz/grasp_object.rviz | 582 +++++++++++++++++++++++ 2 files changed, 590 insertions(+), 4 deletions(-) create mode 100644 stretch_demos/rviz/grasp_object.rviz diff --git a/stretch_demos/launch/grasp_object.launch b/stretch_demos/launch/grasp_object.launch index 2de04cc..0f72a72 100644 --- a/stretch_demos/launch/grasp_object.launch +++ b/stretch_demos/launch/grasp_object.launch @@ -9,7 +9,7 @@ - + @@ -25,7 +25,7 @@ - + @@ -39,9 +39,13 @@ - + - + + + + + diff --git a/stretch_demos/rviz/grasp_object.rviz b/stretch_demos/rviz/grasp_object.rviz new file mode 100644 index 0000000..167a091 --- /dev/null +++ b/stretch_demos/rviz/grasp_object.rviz @@ -0,0 +1,582 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_inner_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_left_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_right_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_top_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_grasp_center: + Alpha: 1 + Show Axes: false + Show Trail: false + link_gripper_finger_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_finger_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_fingertip_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_fingertip_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head_pan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head_tilt: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_lift: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_mast: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_straight_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_wrist_pitch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_wrist_roll: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_wrist_yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_wrist_yaw_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + respeaker_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: false + camera_accel_frame: + Value: false + camera_accel_optical_frame: + Value: false + camera_aligned_depth_to_color_frame: + Value: false + camera_bottom_screw_frame: + Value: false + camera_color_frame: + Value: false + camera_color_optical_frame: + Value: false + camera_depth_frame: + Value: false + camera_depth_optical_frame: + Value: false + camera_gyro_frame: + Value: false + camera_gyro_optical_frame: + Value: false + camera_infra1_frame: + Value: false + camera_infra1_optical_frame: + Value: false + camera_infra2_frame: + Value: false + camera_infra2_optical_frame: + Value: false + camera_link: + Value: false + caster_link: + Value: false + laser: + Value: false + link_arm_l0: + Value: false + link_arm_l1: + Value: false + link_arm_l2: + Value: false + link_arm_l3: + Value: false + link_arm_l4: + Value: false + link_aruco_inner_wrist: + Value: false + link_aruco_left_base: + Value: false + link_aruco_right_base: + Value: false + link_aruco_shoulder: + Value: false + link_aruco_top_wrist: + Value: false + link_grasp_center: + Value: false + link_gripper_finger_left: + Value: false + link_gripper_finger_right: + Value: false + link_gripper_fingertip_left: + Value: false + link_gripper_fingertip_right: + Value: false + link_head: + Value: false + link_head_pan: + Value: false + link_head_tilt: + Value: false + link_left_wheel: + Value: false + link_lift: + Value: false + link_mast: + Value: false + link_right_wheel: + Value: false + link_straight_gripper: + Value: false + link_straight_gripper_aligned: + Value: false + link_wrist_pitch: + Value: false + link_wrist_roll: + Value: false + link_wrist_yaw: + Value: false + link_wrist_yaw_bottom: + Value: false + map: + Value: false + odom: + Value: false + respeaker_base: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + caster_link: + {} + laser: + {} + link_aruco_left_base: + {} + link_aruco_right_base: + {} + link_left_wheel: + {} + link_mast: + link_head: + link_head_pan: + link_head_tilt: + camera_bottom_screw_frame: + camera_link: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_aligned_depth_to_color_frame: + camera_color_optical_frame: + {} + camera_color_frame: + {} + camera_depth_frame: + camera_depth_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + {} + camera_infra1_frame: + camera_infra1_optical_frame: + {} + camera_infra2_frame: + camera_infra2_optical_frame: + {} + link_lift: + link_arm_l4: + link_arm_l3: + link_arm_l2: + link_arm_l1: + link_arm_l0: + link_aruco_inner_wrist: + {} + link_aruco_top_wrist: + {} + link_wrist_yaw: + link_wrist_yaw_bottom: + link_wrist_pitch: + link_wrist_roll: + link_straight_gripper: + link_grasp_center: + {} + link_gripper_finger_left: + link_gripper_fingertip_left: + {} + link_gripper_finger_right: + link_gripper_fingertip_right: + {} + link_straight_gripper_aligned: + {} + link_aruco_shoulder: + {} + respeaker_base: + {} + link_right_wheel: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /scan_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.687999725341797 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000085c000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2488 + X: 72 + Y: 27 From f115b4159f91f0017a84e3b7500fb51f2e4342f2 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 11 Jul 2023 00:10:32 -0700 Subject: [PATCH 09/10] Add dryrun option to grasp_object --- hello_helpers/README.md | 39 +++++ hello_helpers/src/hello_helpers/hello_misc.py | 10 ++ stretch_core/README.md | 4 + stretch_demos/launch/grasp_object.launch | 5 +- stretch_demos/nodes/grasp_object | 144 +++++++++--------- 5 files changed, 125 insertions(+), 77 deletions(-) diff --git a/hello_helpers/README.md b/hello_helpers/README.md index b567311..9ef056e 100644 --- a/hello_helpers/README.md +++ b/hello_helpers/README.md @@ -57,6 +57,33 @@ temp = hm.HelloNode.quick_create('temp') temp.move_to_pose({'joint_lift': 0.4}) ``` +#### Attributes + +##### `dryrun` + +This attribute allows you to control whether the robot actually moves when calling `move_to_pose()`, `home_the_robot()`, `stow_the_robot()`, or other motion methods in this class. When `dryrun` is set to True, these motion methods return immediately. This attribute is helpful when you want to run just the perception/planning part of your node without actually moving the robot. For example, you could replace the following verbose snippet: + +```python +# roslaunch the stretch launch file beforehand +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +actually_move = False +[...] +if actually_move: + temp.move_to_pose({'translate_mobile_base': 1.0}) +``` + +to be more consise: + +```python +# roslaunch the stretch launch file beforehand +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +[...] +temp.dryrun = True +temp.move_to_pose({'translate_mobile_base': 1.0}) +``` + #### Methods ##### `move_to_pose(pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False)` @@ -81,6 +108,18 @@ If you set `custom_full_goal` to True, the dictionary format is string/tuple key self.move_to_pose({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True) ``` +##### `home_the_robot()` + +This is a convenience method to interact with the driver's [`/home_the_robot` service](../stretch_core/README.md#home_the_robot-std_srvstrigger). + +##### `stow_the_robot()` + +This is a convenience method to interact with the driver's [`/stow_the_robot` service](../stretch_core/README.md#stow_the_robot-std_srvstrigger). + +##### `stop_the_robot()` + +This is a convenience method to interact with the driver's [`/stop_the_robot` service](../stretch_core/README.md#stop_the_robot-std_srvstrigger). + ##### `get_tf(from_frame, to_frame)` Use this method to get the transform ([geometry_msgs/TransformStamped](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html)) between two frames. This method is blocking. For example, this method can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using: diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 4301786..459f7a8 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -71,6 +71,7 @@ class HelloNode: self.joint_state = None self.point_cloud = None self.tool = None + self.dryrun = False @classmethod def quick_create(cls, name, wait_for_first_pointcloud=False): @@ -88,6 +89,9 @@ class HelloNode: self.tool = tool_string.data def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False): + if self.dryrun: + return + point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(0.0) trajectory_goal = FollowJointTrajectoryGoal() @@ -170,12 +174,18 @@ class HelloNode: rate.sleep() def home_the_robot(self): + if self.dryrun: + return + trigger_request = TriggerRequest() trigger_result = self.home_the_robot_service(trigger_request) rospy.logdebug(f"{self.node_name}'s HelloNode.home_the_robot: got message {trigger_result.message}") return trigger_result.success def stow_the_robot(self): + if self.dryrun: + return + trigger_request = TriggerRequest() trigger_result = self.stow_the_robot_service(trigger_request) rospy.logdebug(f"{self.node_name}'s HelloNode.stow_the_robot: got message {trigger_result.message}") diff --git a/stretch_core/README.md b/stretch_core/README.md index 4e8a283..a19b234 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -81,6 +81,10 @@ Other ways to stow the robot include using the `stretch_robot_stow.py` CLI tool Runstopping the robot while this service is running will yield undefined behavior and likely leave the driver in a bad state. +##### /stop_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html)) + +This service immediately stops any currently active motion. + ##### /runstop ([std_srvs/SetBool](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/SetBool.html)) This service can put Stretch into runstop or take Stretch out of runstop. It's common to put the robot into/out of runstop by pressing the [glowing white button](https://docs.hello-robot.com/0.2/stretch-tutorials/getting_started/safety_guide/#runstop) in Stretch's head (at which point the robot will beep and the button will be blinking to indicate that it's runstopped), and holding the button down for two seconds to take it out of runstop (the button will return to non-blinking). This service acts as a programmatic way to achieve the same effect. When this service is triggered, the [mode topic](#mode-stdmsgsstringhttpsdocsrosorgennoeticapistdmsgshtmlmsgstringhtml) will reflect that the robot is in "runstopped" mode, and after the robot is taken out of runstop, the driver will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "runstopped" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted. diff --git a/stretch_demos/launch/grasp_object.launch b/stretch_demos/launch/grasp_object.launch index 0f72a72..681b7dc 100644 --- a/stretch_demos/launch/grasp_object.launch +++ b/stretch_demos/launch/grasp_object.launch @@ -1,6 +1,8 @@ + + @@ -37,6 +39,7 @@ + @@ -45,7 +48,7 @@ - + diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 32e90e3..abfc3a5 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -87,22 +87,20 @@ class GraspObjectNode(hm.HelloNode): at_goal = self.move_base.backward(forward_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m) def trigger_grasp_object_callback(self, request): - actually_move = True max_lift_m = 1.09 min_extension_m = 0.01 max_extension_m = 0.5 # 1. Initial configuration use_default_mode = False - if use_default_mode: + if use_default_mode: # Set the D435i to Default mode for obstacle detection - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = self.trigger_d435i_default_mode_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - if actually_move: - rospy.loginfo('Stow the arm.') - self.stow_the_robot() + rospy.loginfo('Stow the arm.') + self.stow_the_robot() # 2. Scan surface and find grasp target self.look_at_surface(scan_time_s = 3.0) @@ -120,14 +118,13 @@ class GraspObjectNode(hm.HelloNode): success=False, message='lift position unavailable' ) - if actually_move: - rospy.loginfo('Raise tool to pregrasp height.') - lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) - lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) - pose = {'joint_lift': lift_to_pregrasp_m} - self.move_to_pose(pose) + rospy.loginfo('Raise tool to pregrasp height.') + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) + pose = {'joint_lift': lift_to_pregrasp_m} + self.move_to_pose(pose) - if actually_move and self.tool == "tool_stretch_dex_wrist": + if self.tool == "tool_stretch_dex_wrist": rospy.loginfo('Rotate pitch/roll for grasping.') pose = {'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0} self.move_to_pose(pose) @@ -135,88 +132,83 @@ class GraspObjectNode(hm.HelloNode): pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) rospy.loginfo('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) rospy.loginfo('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) - if actually_move: - rospy.loginfo('Rotate the gripper for grasping.') - pose = {'joint_wrist_yaw': pregrasp_yaw} - self.move_to_pose(pose) + rospy.loginfo('Rotate the gripper for grasping.') + pose = {'joint_wrist_yaw': pregrasp_yaw} + self.move_to_pose(pose) - rospy.loginfo('Open the gripper.') - pose = {'gripper_aperture': 0.125} - self.move_to_pose(pose) + rospy.loginfo('Open the gripper.') + pose = {'gripper_aperture': 0.125} + self.move_to_pose(pose) pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) rospy.loginfo('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) rospy.loginfo('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) - if actually_move: - rospy.loginfo('Drive to pregrasp location.') - self.drive(pregrasp_mobile_base_m) - - if pregrasp_wrist_extension_m > 0.0: - extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) - extension_m = min(extension_m, max_extension_m) - rospy.loginfo('Extend tool above surface.') - pose = {'wrist_extension': extension_m} - self.move_to_pose(pose) - else: - rospy.loginfo('negative wrist extension for pregrasp, so not extending or retracting.') - - # 4. Grasp the object and lift it - grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) - rospy.loginfo('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) - if actually_move: - rospy.loginfo('Move the grasp pose from the pregrasp pose.') + rospy.loginfo('Drive to pregrasp location.') + self.drive(pregrasp_mobile_base_m) - lift_m = max(self.lift_position + grasp_lift_m, 0.1) - lift_m = min(lift_m, max_lift_m) - - extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + if pregrasp_wrist_extension_m > 0.0: + extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) extension_m = min(extension_m, max_extension_m) - - pose = {'translate_mobile_base': grasp_mobile_base_m, - 'joint_lift': lift_m, - 'wrist_extension': extension_m} - self.move_to_pose(pose) - - rospy.loginfo('Attempt to close the gripper on the object.') - gripper_aperture_m = grasp_target['width_m'] - 0.18 - pose = {'gripper_aperture': gripper_aperture_m} - self.move_to_pose(pose) - - # Lifting appears to happen before the gripper has - # finished unless there is this sleep. Need to look - # into this issue. - rospy.sleep(3.0) - - rospy.loginfo('Attempt to lift the object.') - object_lift_height_m = 0.1 - lift_m = max(self.lift_position + object_lift_height_m, 0.2) - lift_m = min(lift_m, max_lift_m) - pose = {'joint_lift': lift_m} - self.move_to_pose(pose) - - rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') - pose = {'gripper_aperture': gripper_aperture_m + 0.005} - self.move_to_pose(pose) - - rospy.loginfo('Retract the tool.') - pose = {'wrist_extension': 0.01} + rospy.loginfo('Extend tool above surface.') + pose = {'wrist_extension': extension_m} self.move_to_pose(pose) + else: + rospy.loginfo('negative wrist extension for pregrasp, so not extending or retracting.') - rospy.loginfo('Reorient the wrist.') - pose = {'joint_wrist_yaw': 0.0} - self.move_to_pose(pose) + # 4. Grasp the object and lift it + grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) + rospy.loginfo('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) + rospy.loginfo('Move the grasp pose from the pregrasp pose.') + + lift_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_m = min(lift_m, max_lift_m) + extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + extension_m = min(extension_m, max_extension_m) + pose = {'translate_mobile_base': grasp_mobile_base_m, + 'joint_lift': lift_m, + 'wrist_extension': extension_m} + self.move_to_pose(pose) + + rospy.loginfo('Attempt to close the gripper on the object.') + gripper_aperture_m = grasp_target['width_m'] - 0.18 + pose = {'gripper_aperture': gripper_aperture_m} + self.move_to_pose(pose) + + # Lifting appears to happen before the gripper has + # finished unless there is this sleep. Need to look + # into this issue. + rospy.sleep(3.0) + + rospy.loginfo('Attempt to lift the object.') + object_lift_height_m = 0.1 + lift_m = max(self.lift_position + object_lift_height_m, 0.2) + lift_m = min(lift_m, max_lift_m) + pose = {'joint_lift': lift_m} + self.move_to_pose(pose) + + rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') + pose = {'gripper_aperture': gripper_aperture_m + 0.005} + self.move_to_pose(pose) + + rospy.loginfo('Retract the tool.') + pose = {'wrist_extension': 0.01} + self.move_to_pose(pose) + + rospy.loginfo('Reorient the wrist.') + pose = {'joint_wrist_yaw': 0.0} + self.move_to_pose(pose) return TriggerResponse( success=True, message='Completed object grasp!' - ) + ) def main(self): hm.HelloNode.main(self, 'grasp_object', 'grasp_object', wait_for_first_pointcloud=False) self.debug_directory = rospy.get_param('~debug_directory') rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory)) - + self.dryrun = rospy.get_param('~dryrun', False) self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) From eb85fa4f8fcb421c3c74659432b5cf6e5e6de5a2 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 11 Jul 2023 00:22:37 -0700 Subject: [PATCH 10/10] Add hack for dex wrist height difference --- stretch_demos/nodes/grasp_object | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index abfc3a5..467f2fa 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -103,7 +103,7 @@ class GraspObjectNode(hm.HelloNode): self.stow_the_robot() # 2. Scan surface and find grasp target - self.look_at_surface(scan_time_s = 3.0) + self.look_at_surface(scan_time_s = 4.0) grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer) if grasp_target is None: return TriggerResponse( @@ -113,6 +113,8 @@ class GraspObjectNode(hm.HelloNode): # 3. Move to pregrasp pose pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer) + if self.tool == "tool_stretch_dex_wrist": + pregrasp_lift_m += 0.02 if (self.lift_position is None): return TriggerResponse( success=False,