diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index fb9754f..a8d14cc 100644 --- a/stretch_core/nodes/command_groups.py +++ b/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) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 95b6825..97abed7 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/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)