Corrected stretch_demos contact threshold values in effort_pct

stretch_demos/nodes/clean_surface

@@ -135,8 +135,8 @@ class CleanSurfaceNode(hm.HelloNode):
                     self.lower_tool_until_contact()
                 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 = 33.7 #effort_pct #20.0 from funmap
+                    extension_contact_effort = 16.45 #effort_pct #40.0 from funmap
                     pose = {'joint_lift': (lift_m, lift_contact_effort)}
                     self.move_to_pose(pose, custom_contact_thresholds=True)
                     

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 = 18.5 #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 = 32.9 #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 = 18.5 #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 = 32.5 #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 = 41.4 #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 = 64.4 #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