Open Drawer RE2V0 bump extend_hook_until_contact thresh

stretch_demos/nodes/open_drawer

@@ -24,6 +24,7 @@ import argparse as ap
 
 import hello_helpers.hello_misc as hm
 import stretch_funmap.navigate as nv
+from stretch_body.robot_params import RobotParams
 
 class OpenDrawerNode(hm.HelloNode):
 
@@ -36,18 +37,18 @@ class OpenDrawerNode(hm.HelloNode):
         self.letter_height_m = 0.2
         self.wrist_position = None
         self.lift_position = 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        
+        self.wrist_position = wrist_position
         lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
         self.lift_position = lift_position
-        
+
     def align_to_surface(self):
         rospy.loginfo('align_to_surface')
-        trigger_request = TriggerRequest() 
+        trigger_request = TriggerRequest()
         trigger_result = self.trigger_align_with_nearest_cliff_service(trigger_request)
         rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
@@ -57,10 +58,12 @@ 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 = 18.5 #effort_pct #42.0 #40.0 from funmap
+        extension_contact_effort = 40 #effort_pct #42.0 #40.0 from funmap
+        if RobotParams.get_params()[1]['robot']['model_name']=='RE1V0':
+            extension_contact_effort = 18.5
         pose = {'wrist_extension': (extension_m, extension_contact_effort)}
         self.move_to_pose(pose, custom_contact_thresholds=True)
-        
+
     def lower_hook_until_contact(self):
         rospy.loginfo('lower_hook_until_contact')
         max_drop_m = 0.15
@@ -94,7 +97,7 @@ class OpenDrawerNode(hm.HelloNode):
             pose = {'joint_lift': lift_m}
             self.move_to_pose(pose)
             rospy.sleep(0.5) # wait for new lift position
-        
+
     def backoff_from_surface(self):
         rospy.loginfo('backoff_from_surface')
         if self.wrist_position is not None:
@@ -131,7 +134,7 @@ class OpenDrawerNode(hm.HelloNode):
         else:
             rospy.logerr('pull_open: self.wrist_position is None!')
             return False
-        
+
     def move_to_initial_configuration(self):
         initial_pose = {'wrist_extension': 0.01,
                         'joint_wrist_yaw': 1.570796327,
@@ -141,7 +144,7 @@ class OpenDrawerNode(hm.HelloNode):
 
     def trigger_open_drawer_down_callback(self, request):
         return self.open_drawer('down')
-        
+
     def trigger_open_drawer_up_callback(self, request):
         return self.open_drawer('up')
 
@@ -158,11 +161,11 @@ class OpenDrawerNode(hm.HelloNode):
                 message='Failed to backoff from the surface.'
             )
 
-        if direction == 'down': 
+        if direction == 'down':
             self.lower_hook_until_contact()
         elif direction == 'up':
             self.raise_hook_until_contact()
-            
+
         success = self.pull_open()
         if not success:
             return TriggerResponse(
@@ -171,21 +174,21 @@ class OpenDrawerNode(hm.HelloNode):
             )
 
         push_drawer_closed = False
-        if push_drawer_closed: 
+        if push_drawer_closed:
             rospy.sleep(3.0)
             self.push_closed()
-        
+
         return TriggerResponse(
             success=True,
             message='Completed opening the drawer!'
             )
 
-    
+
     def main(self):
         hm.HelloNode.main(self, 'open_drawer', 'open_drawer', wait_for_first_pointcloud=False)
 
         self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
-        
+
         self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer_down',
                                                          Trigger,
                                                          self.trigger_open_drawer_down_callback)
@@ -193,8 +196,8 @@ class OpenDrawerNode(hm.HelloNode):
         self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer_up',
                                                          Trigger,
                                                          self.trigger_open_drawer_up_callback)
-        
-        
+
+
         rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
         self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
@@ -206,12 +209,12 @@ class OpenDrawerNode(hm.HelloNode):
         rospy.wait_for_service('/funmap/trigger_lower_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
         self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
-        
+
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
             rate.sleep()
 
-        
+
 if __name__ == '__main__':
     try:
         parser = ap.ArgumentParser(description='Open Drawer behavior for stretch.')