diff --git a/hello_helpers/src/hello_helpers/simple_command_group.py b/hello_helpers/src/hello_helpers/simple_command_group.py index 9bf6a03..2f2c23b 100644 --- a/hello_helpers/src/hello_helpers/simple_command_group.py +++ b/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 diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index 82bb842..c7ec7be 100644 --- a/stretch_core/nodes/command_groups.py +++ b/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 diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 39b5e9f..8235cf2 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/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: diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 85fceef..dbe6fb7 100755 --- a/stretch_core/nodes/stretch_driver +++ b/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)