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

stretch_demos Contact Forces ported to effort_pct

pull/77/head
Mohamed Fazil 2 years ago
parent
3572d5bd99
commit
dc69b7d420
3 changed files with 26 additions and 27 deletions
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +20
    -21
      stretch_demos/nodes/clean_surface
  2. +2
    -2
      stretch_demos/nodes/hello_world
  3. +4
    -4
      stretch_demos/nodes/open_drawer

+ 20
- 21
stretch_demos/nodes/clean_surface View File

@ -26,7 +26,6 @@ import hello_helpers.hello_misc as hm
import stretch_funmap.navigate as nv import stretch_funmap.navigate as nv
import stretch_funmap.manipulation_planning as mp import stretch_funmap.manipulation_planning as mp
class CleanSurfaceNode(hm.HelloNode): class CleanSurfaceNode(hm.HelloNode):
def __init__(self): def __init__(self):
@ -40,18 +39,18 @@ class CleanSurfaceNode(hm.HelloNode):
self.lift_position = None self.lift_position = None
self.manipulation_view = None self.manipulation_view = None
self.debug_directory = None self.debug_directory = None
def joint_states_callback(self, joint_states): def joint_states_callback(self, joint_states):
with self.joint_states_lock:
with self.joint_states_lock:
self.joint_states = joint_states self.joint_states = joint_states
wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states) wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states)
self.wrist_position = wrist_position self.wrist_position = wrist_position
lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states) lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
self.lift_position = lift_position self.lift_position = lift_position
def lower_tool_until_contact(self): def lower_tool_until_contact(self):
rospy.loginfo('lower_tool_until_contact') rospy.loginfo('lower_tool_until_contact')
trigger_request = TriggerRequest()
trigger_request = TriggerRequest()
trigger_result = self.trigger_lower_until_contact_service(trigger_request) trigger_result = self.trigger_lower_until_contact_service(trigger_request)
rospy.loginfo('trigger_result = {0}'.format(trigger_result)) rospy.loginfo('trigger_result = {0}'.format(trigger_result))
@ -91,15 +90,15 @@ class CleanSurfaceNode(hm.HelloNode):
manip.save_scan(dirname + filename) manip.save_scan(dirname + filename)
else: else:
rospy.loginfo('CleanSurfaceNode: No debug directory provided, so debugging data will not be saved.') rospy.loginfo('CleanSurfaceNode: No debug directory provided, so debugging data will not be saved.')
def trigger_clean_surface_callback(self, request): def trigger_clean_surface_callback(self, request):
tool_width_m = 0.08 tool_width_m = 0.08
tool_length_m = 0.08 tool_length_m = 0.08
step_size_m = 0.04
step_size_m = 0.04
min_extension_m = 0.01 min_extension_m = 0.01
max_extension_m = 0.5 max_extension_m = 0.5
self.look_at_surface() self.look_at_surface()
strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer, tool_width_m, tool_length_m, step_size_m) strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer, tool_width_m, tool_length_m, step_size_m)
@ -107,10 +106,10 @@ class CleanSurfaceNode(hm.HelloNode):
if True and (strokes is not None) and (len(strokes) > 0): if True and (strokes is not None) and (len(strokes) > 0):
if (self.lift_position is not None) and (self.wrist_position is not None):
if (self.lift_position is not None) and (self.wrist_position is not None):
above_surface_m = 0.1 above_surface_m = 0.1
lift_above_surface_m = self.lift_position + lift_to_surface_m + above_surface_m lift_above_surface_m = self.lift_position + lift_to_surface_m + above_surface_m
pose = {'joint_lift': lift_above_surface_m} pose = {'joint_lift': lift_above_surface_m}
rospy.loginfo('Raise tool above surface.') rospy.loginfo('Raise tool above surface.')
@ -122,7 +121,7 @@ class CleanSurfaceNode(hm.HelloNode):
rospy.loginfo('Drive to the start of the cleaning region.') rospy.loginfo('Drive to the start of the cleaning region.')
self.move_to_pose(pose) self.move_to_pose(pose)
initial_wrist_position = self.wrist_position
initial_wrist_position = self.wrist_position
extension_m = start['start_wrist_extension_m'] extension_m = start['start_wrist_extension_m']
start_extension_m = initial_wrist_position + extension_m start_extension_m = initial_wrist_position + extension_m
@ -131,15 +130,15 @@ class CleanSurfaceNode(hm.HelloNode):
self.move_to_pose(pose) self.move_to_pose(pose)
use_old_code = False use_old_code = False
if use_old_code:
if use_old_code:
self.lower_tool_until_contact() self.lower_tool_until_contact()
else:
else:
lift_m = self.lift_position - (above_surface_m - 0.02) lift_m = self.lift_position - (above_surface_m - 0.02)
lift_contact_effort = 20.0 #20.0 from funmap
extension_contact_effort = 40.0 #40.0 from funmap
lift_contact_effort = 0.061 #effort_pct 20.0 pseudo_N from funmap
extension_contact_effort = 0.164 #effort_pct 40.0 pseudo_N from funmap
pose = {'joint_lift': (lift_m, lift_contact_effort)} pose = {'joint_lift': (lift_m, lift_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
use_correction = True use_correction = True
if use_correction: if use_correction:
# raise due to drop down after contact detection # raise due to drop down after contact detection
@ -198,15 +197,15 @@ class CleanSurfaceNode(hm.HelloNode):
message='Completed surface cleaning!' message='Completed surface cleaning!'
) )
def main(self): def main(self):
hm.HelloNode.main(self, 'clean_surface', 'clean_surface', wait_for_first_pointcloud=False) hm.HelloNode.main(self, 'clean_surface', 'clean_surface', wait_for_first_pointcloud=False)
self.debug_directory = rospy.get_param('~debug_directory') self.debug_directory = rospy.get_param('~debug_directory')
rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory)) rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory))
self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
self.trigger_clean_surface_service = rospy.Service('/clean_surface/trigger_clean_surface', self.trigger_clean_surface_service = rospy.Service('/clean_surface/trigger_clean_surface',
Trigger, Trigger,
self.trigger_clean_surface_callback) self.trigger_clean_surface_callback)
@ -228,12 +227,12 @@ class CleanSurfaceNode(hm.HelloNode):
rospy.wait_for_service(high_accuracy_service) rospy.wait_for_service(high_accuracy_service)
rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service) rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service)
self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger) self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger)
rate = rospy.Rate(self.rate) rate = rospy.Rate(self.rate)
while not rospy.is_shutdown(): while not rospy.is_shutdown():
rate.sleep() rate.sleep()
if __name__ == '__main__': if __name__ == '__main__':
try: try:
parser = ap.ArgumentParser(description='Clean Surface behavior for stretch.') parser = ap.ArgumentParser(description='Clean Surface behavior for stretch.')

+ 2
- 2
stretch_demos/nodes/hello_world View File

@ -69,7 +69,7 @@ class HelloWorldNode(hm.HelloNode):
wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states) wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
extension_m = wrist_position + max_reach_m extension_m = wrist_position + max_reach_m
extension_m = min(extension_m, max_extension_m) extension_m = min(extension_m, max_extension_m)
extension_contact_effort = 45.0
extension_contact_effort = 0.185 # effort_pct
pose = {'wrist_extension': (extension_m, extension_contact_effort)} pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
@ -83,7 +83,7 @@ class HelloWorldNode(hm.HelloNode):
extension_m = wrist_position - backoff_m extension_m = wrist_position - backoff_m
extension_m = min(extension_m, max_extension_m) extension_m = min(extension_m, max_extension_m)
extension_m = max(min_extension_m, extension_m) extension_m = max(min_extension_m, extension_m)
extension_contact_effort = 80.0 # to avoid stopping due to contact
extension_contact_effort = 0.329 #effort_pct # to avoid stopping due to contact
pose = {'wrist_extension': (extension_m, extension_contact_effort)} pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
rospy.sleep(1.0) # Give it time to backoff before the next move. Otherwise it may not backoff. rospy.sleep(1.0) # Give it time to backoff before the next move. Otherwise it may not backoff.

+ 4
- 4
stretch_demos/nodes/open_drawer View File

@ -57,7 +57,7 @@ class OpenDrawerNode(hm.HelloNode):
max_reach_m = 0.4 max_reach_m = 0.4
extension_m = self.wrist_position + max_reach_m extension_m = self.wrist_position + max_reach_m
extension_m = min(extension_m, max_extension_m) extension_m = min(extension_m, max_extension_m)
extension_contact_effort = 45.0 #42.0 #40.0 from funmap
extension_contact_effort = 0.185 #effort_pct #42.0 #40.0 from funmap
pose = {'wrist_extension': (extension_m, extension_contact_effort)} pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
@ -65,7 +65,7 @@ class OpenDrawerNode(hm.HelloNode):
rospy.loginfo('lower_hook_until_contact') rospy.loginfo('lower_hook_until_contact')
max_drop_m = 0.15 max_drop_m = 0.15
lift_m = self.lift_position - max_drop_m lift_m = self.lift_position - max_drop_m
lift_contact_effort = 16.0 #18.0 #20.0 #20.0 from funmap
lift_contact_effort = 0.049 #effort_pct #18.0 #20.0 #20.0 from funmap
pose = {'joint_lift': (lift_m, lift_contact_effort)} pose = {'joint_lift': (lift_m, lift_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
@ -82,7 +82,7 @@ class OpenDrawerNode(hm.HelloNode):
rospy.loginfo('raise_hook_until_contact') rospy.loginfo('raise_hook_until_contact')
max_raise_m = 0.15 max_raise_m = 0.15
lift_m = self.lift_position + max_raise_m lift_m = self.lift_position + max_raise_m
lift_contact_effort = 45.0
lift_contact_effort = 0.138 #effort_pct
pose = {'joint_lift': (lift_m, lift_contact_effort)} pose = {'joint_lift': (lift_m, lift_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
@ -113,7 +113,7 @@ class OpenDrawerNode(hm.HelloNode):
extension_m = self.wrist_position - 0.2 extension_m = self.wrist_position - 0.2
extension_m = min(extension_m, max_extension_m) extension_m = min(extension_m, max_extension_m)
extension_m = max(0.01, extension_m) extension_m = max(0.01, extension_m)
extension_contact_effort = 120 #100.0 #40.0 from funmap
extension_contact_effort = 0.493 #effort_pct #100.0 #40.0 from funmap
pose = {'wrist_extension': (extension_m, extension_contact_effort)} pose = {'wrist_extension': (extension_m, extension_contact_effort)}
self.move_to_pose(pose, custom_contact_thresholds=True) self.move_to_pose(pose, custom_contact_thresholds=True)
return True return True

Write Preview
Loading…
Cancel
Save