End effectors are now pluggable into stretch_driver

hello_helpers/src/hello_helpers/simple_command_group.py

@@ -2,7 +2,7 @@ import hello_helpers.hello_misc as hm
 
 
 class SimpleCommandGroup:
-    def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015):
+    def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015, node=None):
         """Simple command group to extend
 
         Attributes
@@ -24,6 +24,8 @@ class SimpleCommandGroup:
         """
         self.name = joint_name
         self.range = joint_range
+        if self.range is None:
+            self.update_joint_range(None, node=node)
         self.active = False
         self.index = None
         self.goal = {"position": None}
@@ -43,6 +45,18 @@ class SimpleCommandGroup:
 
         return 0
 
+    def update_joint_range(self, joint_range, node=None):
+        """Updates the commandable joint range
+
+        Parameters
+        ----------
+        joint_range: tuple(float, float) or None
+            updates range if provided, else calculates it automatically
+        node: StretchBodyNode or None
+            required to calculate range automatically
+        """
+        raise NotImplementedError
+
     def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
         """Activates joints in the group
 

stretch_core/nodes/command_groups.py

@@ -7,12 +7,28 @@ from hello_helpers.gripper_conversion import GripperConversion
 
 
 class HeadPanCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_rad, calibrated_offset_rad=0.0, calibrated_looked_left_offset_rad=0.0):
-        SimpleCommandGroup.__init__(self, 'joint_head_pan', range_rad, acceptable_joint_error=0.15)
+    def __init__(self, range_rad=None, calibrated_offset_rad=None, calibrated_looked_left_offset_rad=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_head_pan', range_rad, acceptable_joint_error=0.15, node=node)
+        if calibrated_offset_rad is None:
+            calibrated_offset_rad = node.controller_parameters['pan_angle_offset']
         self.calibrated_offset_rad = calibrated_offset_rad
+        if calibrated_looked_left_offset_rad is None:
+            calibrated_looked_left_offset_rad = node.controller_parameters['pan_looked_left_offset']
         self.looked_left_offset_rad = calibrated_looked_left_offset_rad
         self.looked_left = False
 
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_ticks = node.robot.head.motors['head_pan'].params['range_t']
+        range_rad = (node.robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[1]),
+                     node.robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[0]))
+        self.range = range_rad
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             _, pan_error = self.update_execution(robot_status)
@@ -37,13 +53,31 @@ class HeadPanCommandGroup(SimpleCommandGroup):
 
 
 class HeadTiltCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_rad, calibrated_offset_rad=0.0, calibrated_looking_up_offset_rad=0.0, backlash_transition_angle_rad=-0.4):
-        SimpleCommandGroup.__init__(self, 'joint_head_tilt', range_rad, acceptable_joint_error=0.52)
+    def __init__(self, range_rad=None, calibrated_offset_rad=None, calibrated_looking_up_offset_rad=None, backlash_transition_angle_rad=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_head_tilt', range_rad, acceptable_joint_error=0.52, node=node)
+        if calibrated_offset_rad is None:
+            calibrated_offset_rad = node.controller_parameters['tilt_angle_offset']
         self.calibrated_offset_rad = calibrated_offset_rad
+        if calibrated_looking_up_offset_rad is None:
+            calibrated_looking_up_offset_rad = node.controller_parameters['tilt_looking_up_offset']
         self.looking_up_offset_rad = calibrated_looking_up_offset_rad
+        if backlash_transition_angle_rad is None:
+            backlash_transition_angle_rad = node.controller_parameters['tilt_angle_backlash_transition']
         self.backlash_transition_angle_rad = backlash_transition_angle_rad
         self.looking_up = False
 
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_ticks = node.robot.head.motors['head_tilt'].params['range_t']
+        range_rad = (node.robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[1]),
+                     node.robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[0]))
+        self.range = range_rad
+
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
             _, tilt_error = self.update_execution(robot_status)
@@ -68,8 +102,20 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
 
 
 class WristYawCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_rad):
-        SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad)
+    def __init__(self, range_rad=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', range_rad, node=node)
+
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_ticks = node.robot.end_of_arm.motors['wrist_yaw'].params['range_t']
+        range_rad = (node.robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[1]),
+                     node.robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[0]))
+        self.range = range_rad
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
@@ -92,10 +138,29 @@ class WristYawCommandGroup(SimpleCommandGroup):
 
 
 class GripperCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_robotis):
-        SimpleCommandGroup.__init__(self, 'joint_gripper_finger_left', None, acceptable_joint_error=1.0)
+    def __init__(self, range_robotis=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_gripper_finger_left', range_robotis, acceptable_joint_error=1.0, node=node)
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
         self.gripper_conversion = GripperConversion()
+        self.update_joint_range(range_robotis)
+
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(joint_range[0]),
+                                    self.gripper_conversion.robotis_to_aperture(joint_range[1]))
+            self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(joint_range[0]),
+                                    self.gripper_conversion.robotis_to_finger(joint_range[1]))
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_ticks = node.robot.end_of_arm.motors['stretch_gripper'].params['range_t']
+        range_rad = (node.robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[0]),
+                     node.robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[1]))
+        range_robotis = (node.robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[0]),
+                         node.robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[1]))
+        self.range = range_robotis
         self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(range_robotis[0]),
                                  self.gripper_conversion.robotis_to_aperture(range_robotis[1]))
         self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(range_robotis[0]),
@@ -181,13 +246,25 @@ class GripperCommandGroup(SimpleCommandGroup):
 
 
 class ArmCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_m, calibrated_retracted_offset_m=0.0):
-        SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008)
+    def __init__(self, range_m=None, calibrated_retracted_offset_m=None, node=None):
+        SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008, node=node)
         self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
         self.is_telescoping = False
+        if calibrated_retracted_offset_m is None:
+            calibrated_retracted_offset_m = node.controller_parameters['arm_retracted_offset']
         self.retracted_offset_m = calibrated_retracted_offset_m
         self.retracted = False
 
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_m = tuple(node.robot.arm.params['range_m'])
+        self.range = range_m
+
     def get_num_valid_commands(self):
         if self.active and self.is_telescoping:
             return len(self.telescoping_joints)
@@ -297,8 +374,18 @@ class ArmCommandGroup(SimpleCommandGroup):
 
 
 class LiftCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_m):
-        SimpleCommandGroup.__init__(self, 'joint_lift', range_m)
+    def __init__(self, range_m=None, node=None):
+        SimpleCommandGroup.__init__(self, 'joint_lift', range_m, node=node)
+
+    def update_joint_range(self, joint_range, node=None):
+        if joint_range is not None:
+            self.range = joint_range
+            return
+
+        if node is None:
+            return # cannot calculate range without Stretch Body handle
+        range_m = tuple(node.robot.lift.params['range_m'])
+        self.range = range_m
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
@@ -327,9 +414,9 @@ class LiftCommandGroup(SimpleCommandGroup):
 
 
 class MobileBaseCommandGroup(SimpleCommandGroup):
-    def __init__(self, virtual_range_m=(-0.5, 0.5)):
+    def __init__(self, virtual_range_m=(-0.5, 0.5), node=None):
         SimpleCommandGroup.__init__(self, 'joint_mobile_base_translation', virtual_range_m,
-                                    acceptable_joint_error=0.005)
+                                    acceptable_joint_error=0.005, node=node)
         self.incrementing_joint_names = ['translate_mobile_base', 'rotate_mobile_base']
         self.active_translate_mobile_base = False
         self.active_rotate_mobile_base = False
@@ -340,6 +427,14 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
         self.min_m_per_s = 0.002
         self.min_rad_per_s = np.radians(1.0)
 
+    def update_joint_range(self, virtual_joint_range, node=None):
+        if virtual_joint_range is not None:
+            self.range = virtual_joint_range
+            return
+
+        virtual_range_m = (-0.5, 0.5)
+        self.range = virtual_range_m
+
     def get_num_valid_commands(self):
         if self.active:
             num_inc = self.active_translate_mobile_base + self.active_rotate_mobile_base

stretch_core/nodes/joint_trajectory_server.py

@@ -24,37 +24,27 @@ class JointTrajectoryAction:
         self.feedback = FollowJointTrajectoryFeedback()
         self.result = FollowJointTrajectoryResult()
 
-        r = self.node.robot
-        head_pan_range_ticks = r.head.motors['head_pan'].params['range_t']
-        head_pan_range_rad = (r.head.motors['head_pan'].ticks_to_world_rad(head_pan_range_ticks[1]),
-                              r.head.motors['head_pan'].ticks_to_world_rad(head_pan_range_ticks[0]))
-        head_tilt_range_ticks = r.head.motors['head_tilt'].params['range_t']
-        head_tilt_range_rad = (r.head.motors['head_tilt'].ticks_to_world_rad(head_tilt_range_ticks[1]),
-                               r.head.motors['head_tilt'].ticks_to_world_rad(head_tilt_range_ticks[0]))
-        wrist_yaw_range_ticks = r.end_of_arm.motors['wrist_yaw'].params['range_t']
-        wrist_yaw_range_rad = (r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(wrist_yaw_range_ticks[1]),
-                               r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(wrist_yaw_range_ticks[0]))
-        gripper_range_ticks = r.end_of_arm.motors['stretch_gripper'].params['range_t']
-        gripper_range_rad = (r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(gripper_range_ticks[0]),
-                             r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(gripper_range_ticks[1]))
-        gripper_range_robotis = (r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[0]),
-                                 r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(gripper_range_rad[1]))
-
-        self.head_pan_cg = HeadPanCommandGroup(head_pan_range_rad,
-                                               self.node.controller_parameters['pan_angle_offset'],
-                                               self.node.controller_parameters['pan_looked_left_offset'])
-        self.head_tilt_cg = HeadTiltCommandGroup(head_tilt_range_rad,
-                                                 self.node.controller_parameters['tilt_angle_offset'],
-                                                 self.node.controller_parameters['tilt_looking_up_offset'],
-                                                 self.node.controller_parameters['tilt_angle_backlash_transition'])
-        self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad)
-        self.gripper_cg = GripperCommandGroup(gripper_range_robotis)
-        self.arm_cg = ArmCommandGroup(tuple(r.arm.params['range_m']),
-                                                      self.node.controller_parameters['arm_retracted_offset'])
-        self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m']))
-        self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5))
+        self.head_pan_cg = HeadPanCommandGroup(node=self.node) \
+            if 'head_pan' in self.node.robot.head.joints else None
+        self.head_tilt_cg = HeadTiltCommandGroup(node=self.node) \
+            if 'head_tilt' in self.node.robot.head.joints else None
+        self.wrist_yaw_cg = WristYawCommandGroup(node=self.node) \
+            if 'wrist_yaw' in self.node.robot.end_of_arm.joints else None
+        self.gripper_cg = GripperCommandGroup(node=self.node) \
+            if 'stretch_gripper' in self.node.robot.end_of_arm.joints else None
+        self.arm_cg = ArmCommandGroup(node=self.node)
+        self.lift_cg = LiftCommandGroup(node=self.node)
+        self.mobile_base_cg = MobileBaseCommandGroup(node=self.node)
         self.command_groups = [self.arm_cg, self.lift_cg, self.mobile_base_cg, self.head_pan_cg,
                                self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg]
+        self.command_groups = [cg for cg in self.command_groups if cg is not None]
+
+        for joint in self.node.robot.end_of_arm.joints:
+            module_name = self.node.robot.end_of_arm.params['devices'][joint].get('ros_py_module_name')
+            class_name = self.node.robot.end_of_arm.params['devices'][joint].get('ros_py_class_name')
+            if module_name and class_name:
+                endofarm_cg = getattr(importlib.import_module(module_name), class_name)(node=self.node)
+                self.command_groups.append(endofarm_cg)
 
     def execute_cb(self, goal):
         with self.node.robot_stop_lock:

stretch_core/nodes/stretch_driver

@@ -165,13 +165,14 @@ class StretchBodyNode:
 
         # add gripper joints to joint state
         gripper_cg = self.joint_trajectory_action.gripper_cg
-        missing_gripper_joint_names = list(set(gripper_cg.gripper_joint_names) - set(joint_state.name))
-        for j in missing_gripper_joint_names:
-            pos, vel, effort = gripper_cg.joint_state(robot_status, joint_name=j)
-            joint_state.name.append(j)
-            joint_state.position.append(pos)
-            joint_state.velocity.append(vel)
-            joint_state.effort.append(effort)
+        if gripper_cg is not None:
+            missing_gripper_joint_names = list(set(gripper_cg.gripper_joint_names) - set(joint_state.name))
+            for j in missing_gripper_joint_names:
+                pos, vel, effort = gripper_cg.joint_state(robot_status, joint_name=j)
+                joint_state.name.append(j)
+                joint_state.position.append(pos)
+                joint_state.velocity.append(vel)
+                joint_state.effort.append(effort)
 
         self.joint_state_pub.publish(joint_state)