Merge branch 'feature/contact_models_fix' into bugfix/navigation_config_noetic

stretch_core/nodes/command_groups.py

@@ -347,8 +347,8 @@ class ArmCommandGroup(SimpleCommandGroup):
             robot.arm.move_by(extension_error_m,
                               v_m=self.goal['velocity'],
                               a_m=self.goal['acceleration'],
-                              contact_thresh_pos_N=self.goal['contact_threshold'],
-                              contact_thresh_neg_N=-self.goal['contact_threshold'] \
+                              contact_thresh_pos=self.goal['contact_threshold'],
+                              contact_thresh_neg=-self.goal['contact_threshold'] \
                                                    if self.goal['contact_threshold'] is not None else None)
             self.retracted = extension_error_m < 0.0
 
@@ -370,7 +370,7 @@ class ArmCommandGroup(SimpleCommandGroup):
         arm_status = robot_status['arm']
         arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
         pos = arm_status['pos'] + arm_backlash_correction
-        return (pos, arm_status['vel'], arm_status['motor']['effort'])
+        return (pos, arm_status['vel'], arm_status['motor']['effort_pct'])
 
 
 class LiftCommandGroup(SimpleCommandGroup):
@@ -392,8 +392,8 @@ class LiftCommandGroup(SimpleCommandGroup):
             robot.lift.move_by(self.update_execution(robot_status)[1],
                                v_m=self.goal['velocity'],
                                a_m=self.goal['acceleration'],
-                               contact_thresh_pos_N=self.goal['contact_threshold'],
-                               contact_thresh_neg_N=-self.goal['contact_threshold'] \
+                               contact_thresh_pos=self.goal['contact_threshold'],
+                               contact_thresh_neg=-self.goal['contact_threshold'] \
                                                     if self.goal['contact_threshold'] is not None else None)
 
     def update_execution(self, robot_status, **kwargs):
@@ -410,7 +410,7 @@ class LiftCommandGroup(SimpleCommandGroup):
 
     def joint_state(self, robot_status, **kwargs):
         lift_status = robot_status['lift']
-        return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort'])
+        return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort_pct'])
 
 
 class MobileBaseCommandGroup(SimpleCommandGroup):
@@ -556,17 +556,17 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                     robot.base.translate_by(mobile_base_error_m,
                                             v_m=self.goal_translate_mobile_base['velocity'],
                                             a_m=self.goal_translate_mobile_base['acceleration'],
-                                            contact_thresh_N=self.goal_translate_mobile_base['contact_threshold'])
+                                            contact_thresh=self.goal_translate_mobile_base['contact_threshold'])
                 elif mobile_base_error_rad is not None:
                     robot.base.rotate_by(mobile_base_error_rad,
                                          v_r=self.goal_rotate_mobile_base['velocity'],
                                          a_r=self.goal_rotate_mobile_base['acceleration'],
-                                         contact_thresh_N=self.goal_rotate_mobile_base['contact_threshold'])
+                                         contact_thresh=self.goal_rotate_mobile_base['contact_threshold'])
             else:
                 robot.base.translate_by(self.update_execution(robot_status)[1],
                                         v_m=self.goal['velocity'],
                                         a_m=self.goal['acceleration'],
-                                        contact_thresh_N=self.goal['contact_threshold'])
+                                        contact_thresh=self.goal['contact_threshold'])
 
     def update_execution(self, robot_status, **kwargs):
         success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None

stretch_core/nodes/stretch_driver

@@ -6,6 +6,7 @@ import threading
 from rwlock import RWLock
 import stretch_body.robot as rb
 from stretch_body.hello_utils import ThreadServiceExit
+import stretch_body
 
 import tf2_ros
 import tf_conversions
@@ -338,6 +339,10 @@ class StretchBodyNode:
 
         rospy.loginfo("{0} started".format(self.node_name))
 
+        if int(stretch_body.__version__.split('.')[1]) < 4:
+            rospy.logerr("ERROR: Found old stretch_body version. Please upgrade stretch_body to 0.4.0 or above.")
+            rospy.signal_shutdown('Found old stretch_body version.')
+
         self.robot = rb.Robot()
         self.robot.startup()
 

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

@@ -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,15 +58,17 @@ 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 = 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
         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 +85,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)
 
@@ -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:
@@ -113,7 +116,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
@@ -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.')

stretch_description/package.xml

@@ -63,6 +63,7 @@
   
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>rviz</exec_depend>
+  <exec_depend>rviz_imu_plugin</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   
   <!-- The export tag contains other, unspecified, tags -->