use new approach to custom contact thresholds

stretch_demos/nodes/clean_surface

@@ -112,7 +112,9 @@ class CleanSurfaceNode(hm.HelloNode):
 
             if (self.lift_position is not None) and (self.wrist_position is not None): 
 
-                lift_above_surface_m = self.lift_position + lift_to_surface_m + 0.1 
+                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.')
                 self.move_to_pose(pose)
@@ -130,8 +132,16 @@ class CleanSurfaceNode(hm.HelloNode):
                 pose = {'wrist_extension': start_extension_m}
                 rospy.loginfo('Extend tool above surface.')
                 self.move_to_pose(pose)
-                
-                self.lower_tool_until_contact()
+
+                use_old_code = False
+                if use_old_code: 
+                    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
+                    pose = {'joint_lift': (lift_m, lift_contact_effort)}
+                    self.move_to_pose(pose, custom_contact_thresholds=True)
 
                 extension_m = start['end_wrist_extension_m']
                 end_extension_m = initial_wrist_position + extension_m - (tool_length_m/2.0) - safety_from_edge