stretch_demos Contact Forces ported to effort_pct

stretch_demos/nodes/clean_surface

@@ -26,7 +26,6 @@ import hello_helpers.hello_misc as hm
 import stretch_funmap.navigate as nv
 import stretch_funmap.manipulation_planning as mp
 
-
 class CleanSurfaceNode(hm.HelloNode):
 
     def __init__(self):
@@ -40,18 +39,18 @@ class CleanSurfaceNode(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: 
+        with self.joint_states_lock:
             self.joint_states = joint_states
         wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states)
         self.wrist_position = wrist_position
         lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
         self.lift_position = lift_position
-        
+
     def lower_tool_until_contact(self):
         rospy.loginfo('lower_tool_until_contact')
-        trigger_request = TriggerRequest() 
+        trigger_request = TriggerRequest()
         trigger_result = self.trigger_lower_until_contact_service(trigger_request)
         rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
@@ -91,15 +90,15 @@ class CleanSurfaceNode(hm.HelloNode):
             manip.save_scan(dirname + filename)
         else:
             rospy.loginfo('CleanSurfaceNode: No debug directory provided, so debugging data will not be saved.')
-        
+
     def trigger_clean_surface_callback(self, request):
 
         tool_width_m = 0.08
         tool_length_m = 0.08
-        step_size_m = 0.04 
+        step_size_m = 0.04
         min_extension_m = 0.01
         max_extension_m = 0.5
-        
+
         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)
 
@@ -107,10 +106,10 @@ class CleanSurfaceNode(hm.HelloNode):
 
         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
-                
+
                 lift_above_surface_m = self.lift_position + lift_to_surface_m + above_surface_m
                 pose = {'joint_lift': lift_above_surface_m}
                 rospy.loginfo('Raise tool above surface.')
@@ -122,7 +121,7 @@ class CleanSurfaceNode(hm.HelloNode):
                 rospy.loginfo('Drive to the start of the cleaning region.')
                 self.move_to_pose(pose)
 
-                initial_wrist_position = self.wrist_position 
+                initial_wrist_position = self.wrist_position
                 extension_m = start['start_wrist_extension_m']
 
                 start_extension_m = initial_wrist_position + extension_m
@@ -131,15 +130,15 @@ class CleanSurfaceNode(hm.HelloNode):
                 self.move_to_pose(pose)
 
                 use_old_code = False
-                if use_old_code: 
+                if use_old_code:
                     self.lower_tool_until_contact()
-                else: 
+                else:
                     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)}
                     self.move_to_pose(pose, custom_contact_thresholds=True)
-                    
+
                     use_correction = True
                     if use_correction:
                         # raise due to drop down after contact detection
@@ -198,15 +197,15 @@ class CleanSurfaceNode(hm.HelloNode):
             message='Completed surface cleaning!'
             )
 
-    
+
     def main(self):
         hm.HelloNode.main(self, 'clean_surface', 'clean_surface', 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.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',
                                                            Trigger,
                                                            self.trigger_clean_surface_callback)
@@ -228,12 +227,12 @@ class CleanSurfaceNode(hm.HelloNode):
         rospy.wait_for_service(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)
-        
+
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
             rate.sleep()
 
-        
+
 if __name__ == '__main__':
     try:
         parser = ap.ArgumentParser(description='Clean Surface behavior for stretch.')

stretch_demos/nodes/hello_world

@@ -69,7 +69,7 @@ class HelloWorldNode(hm.HelloNode):
             wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states)
         extension_m = wrist_position + max_reach_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)}
         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 = min(extension_m, max_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)}
         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.

stretch_demos/nodes/open_drawer

@@ -57,7 +57,7 @@ class OpenDrawerNode(hm.HelloNode):
         max_reach_m = 0.4
         extension_m = self.wrist_position + max_reach_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)}
         self.move_to_pose(pose, custom_contact_thresholds=True)
         
@@ -65,7 +65,7 @@ class OpenDrawerNode(hm.HelloNode):
         rospy.loginfo('lower_hook_until_contact')
         max_drop_m = 0.15
         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)}
         self.move_to_pose(pose, custom_contact_thresholds=True)
 
@@ -82,7 +82,7 @@ class OpenDrawerNode(hm.HelloNode):
         rospy.loginfo('raise_hook_until_contact')
         max_raise_m = 0.15
         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)}
         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 = min(extension_m, max_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)}
             self.move_to_pose(pose, custom_contact_thresholds=True)
             return True