From d167f22260d3525c27237fdc8d124d0cffabaf54 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 31 Dec 2021 02:38:24 -0800 Subject: [PATCH] Rename arm command group --- stretch_core/nodes/command_groups.py | 3 +-- stretch_core/nodes/joint_trajectory_server.py | 6 +++--- stretch_core/nodes/stretch_driver | 12 ++++++------ 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index aa58e5c..d395619 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -181,9 +181,8 @@ class GripperCommandGroup(SimpleCommandGroup): return (pos_rad, vel, effort) -class TelescopingCommandGroup(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.005) SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008) self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0'] self.is_telescoping = False diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index bc060b7..5b93a00 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -10,7 +10,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint from command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \ WristYawCommandGroup, GripperCommandGroup, \ - TelescopingCommandGroup, LiftCommandGroup, \ + ArmCommandGroup, LiftCommandGroup, \ MobileBaseCommandGroup @@ -50,11 +50,11 @@ class JointTrajectoryAction: 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.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']), + 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.command_groups = [self.telescoping_cg, self.lift_cg, self.mobile_base_cg, self.head_pan_cg, + 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] def execute_cb(self, goal): diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 8bc7391..54f9d2d 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -140,12 +140,12 @@ class StretchBodyNode: joint_state.effort.append(effort) # add telescoping joints to joint state - telescoping_cg = self.joint_trajectory_action.telescoping_cg - joint_state.name.extend(telescoping_cg.telescoping_joints) - pos, vel, effort = telescoping_cg.joint_state(robot_status) - for _ in range(len(telescoping_cg.telescoping_joints)): - joint_state.position.append(pos / len(cg.telescoping_joints)) - joint_state.velocity.append(vel / len(cg.telescoping_joints)) + arm_cg = self.joint_trajectory_action.arm_cg + joint_state.name.extend(arm_cg.telescoping_joints) + pos, vel, effort = arm_cg.joint_state(robot_status) + for _ in range(len(arm_cg.telescoping_joints)): + joint_state.position.append(pos / len(arm_cg.telescoping_joints)) + joint_state.velocity.append(vel / len(arm_cg.telescoping_joints)) joint_state.effort.append(effort) # add gripper joints to joint state