cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Add stow step to grasp_object + minor fixes

pull/106/head
Binit Shah 1 year ago
parent
8626c4af36
commit
78661e8dc3
1 changed files with 16 additions and 36 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +16
    -36
      stretch_demos/nodes/grasp_object

+ 16
- 36
stretch_demos/nodes/grasp_object View File

@ -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)

Write Preview
Loading…
Cancel
Save