Only use the devices/joints that body makes available

stretch_core/nodes/command_groups.py

@@ -182,7 +182,7 @@ class SimpleCommandGroup:
 
 class HeadPanCommandGroup(SimpleCommandGroup):
     def __init__(self, range_rad, head_pan_calibrated_offset_rad, head_pan_calibrated_looked_left_offset_rad, robot=None):
-        if range_rad is None and robot is not None:
+        if range_rad is None and robot is not None and robot.head is not None:
             range_ticks = robot.head.motors['head_pan'].params['range_t']
             range_rad = (robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[1]),
                          robot.head.motors['head_pan'].ticks_to_world_rad(range_ticks[0]))
@@ -220,7 +220,7 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
     def __init__(self, range_rad, head_tilt_calibrated_offset_rad,
                  head_tilt_calibrated_looking_up_offset_rad,
                  head_tilt_backlash_transition_angle_rad, robot=None):
-        if range_rad is None and robot is not None:
+        if range_rad is None and robot is not None and robot.head is not None:
             range_ticks = robot.head.motors['head_tilt'].params['range_t']
             range_rad = (robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[1]),
                          robot.head.motors['head_tilt'].ticks_to_world_rad(range_ticks[0]))
@@ -257,7 +257,8 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
 
 class WristYawCommandGroup(SimpleCommandGroup):
     def __init__(self, range_rad, robot=None):
-        if range_rad is None and robot is not None:
+        if (range_rad is None and robot is not None and robot.end_of_arm is not None
+            and robot.end_of_arm.is_tool_present('WristYaw')):
             range_ticks = robot.end_of_arm.motors['wrist_yaw'].params['range_t']
             range_rad = (robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[1]),
                          robot.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(range_ticks[0]))
@@ -285,19 +286,20 @@ class WristYawCommandGroup(SimpleCommandGroup):
 
 class GripperCommandGroup(SimpleCommandGroup):
     def __init__(self, range_robotis, robot=None):
-        if range_robotis is None and robot is not None:
+        if (range_robotis is None and robot is not None and robot.end_of_arm is not None
+            and robot.end_of_arm.is_tool_present('StretchGripper')):
             range_ticks = robot.end_of_arm.motors['stretch_gripper'].params['range_t']
             range_rad = (robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[0]),
                          robot.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(range_ticks[1]))
             range_robotis = (robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[0]),
                              robot.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(range_rad[1]))
+            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]),
+                                    self.gripper_conversion.robotis_to_finger(range_robotis[1]))
         SimpleCommandGroup.__init__(self, None, None, acceptable_joint_error=1.0)
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
         self.gripper_conversion = GripperConversion()
-        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]),
-                                 self.gripper_conversion.robotis_to_finger(range_robotis[1]))
 
     def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
         self.active = False
@@ -375,7 +377,7 @@ class GripperCommandGroup(SimpleCommandGroup):
 
 class ArmCommandGroup(SimpleCommandGroup):
     def __init__(self, range_m, wrist_extension_calibrated_retracted_offset_m, robot=None):
-        if range_m is None and robot is not None:
+        if range_m is None and robot is not None and robot.arm is not None:
             range_m = tuple(robot.arm.params['range_m'])
         SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008)
         self.calibrated_retracted_offset_m = wrist_extension_calibrated_retracted_offset_m
@@ -493,7 +495,7 @@ class ArmCommandGroup(SimpleCommandGroup):
 
 class LiftCommandGroup(SimpleCommandGroup):
     def __init__(self, range_m, robot=None):
-        if range_m is None and robot is not None:
+        if range_m is None and robot is not None and robot.lift is not None:
             range_m = tuple(robot.lift.params['range_m'])
         SimpleCommandGroup.__init__(self, 'joint_lift', range_m)
 

stretch_core/nodes/joint_trajectory_server.py

@@ -12,7 +12,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint
 
 from command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \
                            WristYawCommandGroup, GripperCommandGroup, \
-                           TelescopingCommandGroup, LiftCommandGroup, \
+                           ArmCommandGroup, LiftCommandGroup, \
                            MobileBaseCommandGroup
 
 
@@ -27,7 +27,8 @@ class JointTrajectoryAction:
         self.feedback = FollowJointTrajectoryFeedback()
         self.result = FollowJointTrajectoryResult()
 
-        # Setup main command groups
+        # Default command groups
+        self.command_groups = []
         self.head_pan_cg = HeadPanCommandGroup(None, self.node.head_pan_calibrated_offset_rad,
                                                self.node.head_pan_calibrated_looked_left_offset_rad,
                                                self.node.robot)
@@ -36,25 +37,35 @@ class JointTrajectoryAction:
                                                  self.node.head_tilt_calibrated_looking_up_offset_rad,
                                                  self.node.head_tilt_backlash_transition_angle_rad,
                                                  self.node.robot)
-        self.telescoping_cg = TelescopingCommandGroup(None,
-                                                      self.node.wrist_extension_calibrated_retracted_offset_m,
-                                                      self.node.robot)
+        self.arm_cg = ArmCommandGroup(None, self.node.wrist_extension_calibrated_retracted_offset_m,
+                                      self.node.robot)
         self.lift_cg = LiftCommandGroup(None, self.node.robot)
         self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5))
-        self.command_groups = [self.head_pan_cg, self.head_tilt_cg, self.telescoping_cg,
-                               self.lift_cg, self.mobile_base_cg]
+        self.wrist_yaw_cg = WristYawCommandGroup(None, self.node.robot)
+        self.gripper_cg = GripperCommandGroup(None, self.node.robot)
+        device_cmdgroup_map = {"head_pan": self.head_pan_cg,
+                               "head_tilt": self.head_tilt_cg,
+                               "arm": self.arm_cg,
+                               "lift": self.lift_cg,
+                               "base": self.mobile_base_cg,
+                               "wrist_yaw": self.wrist_yaw_cg,
+                               "stretch_gripper": self.gripper_cg}
+
+        # Setup main command groups
+        active_devices = [k for k in self.node.robot.devices.keys() if self.node.robot.devices.get(k) is not None]
+        if self.node.robot.head is not None:
+            active_devices.extend(self.node.robot.head.joints)
+        for device in active_devices:
+            if device_cmdgroup_map.get(device) is not None:
+                self.command_groups.append(device_cmdgroup_map.get(device))
 
         # Setup end of arm
-        for j in self.node.robot.end_of_arm.joints:
-            if j == "stretch_gripper":
-                self.gripper_cg = GripperCommandGroup(None, self.node.robot)
-                self.command_groups.append(self.gripper_cg)
-            elif j == "wrist_yaw":
-                self.wrist_yaw_cg = WristYawCommandGroup(None, self.node.robot)
-                self.command_groups.append(self.wrist_yaw_cg)
+        for joint in self.node.robot.end_of_arm.joints:
+            if joint in device_cmdgroup_map.keys():
+                self.command_groups.append(device_cmdgroup_map.get(joint))
             else:
-                module_name = self.node.robot.end_of_arm.params['devices'][j].get('ros_py_module_name')
-                class_name = self.node.robot.end_of_arm.params['devices'][j].get('ros_py_class_name')
+                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_device = getattr(importlib.import_module(module_name), class_name)(None, self.node.robot)
                     self.command_groups.append(endofarm_device)