diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index b347bbe..6aaef62 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -52,8 +52,7 @@ class JointTrajectoryAction: self.gripper_cg = GripperCommandGroup(gripper_range_robotis) self.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']), self.node.wrist_extension_calibrated_retracted_offset_m) - if self.node.use_lift: - self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m'])) + self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m'])) self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5, 0.5)) def execute_cb(self, goal): @@ -69,13 +68,8 @@ class JointTrajectoryAction: ################################################### # Decide what to do based on the commanded joints. - if self.node.use_lift: - command_groups = [self.telescoping_cg, self.lift_cg, self.mobile_base_cg, - self.head_pan_cg, self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] - else: - command_groups = [self.telescoping_cg, self.mobile_base_cg, self.head_pan_cg, - self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] - + command_groups = [self.telescoping_cg, self.lift_cg, self.mobile_base_cg, self.head_pan_cg, + self.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg] updates = [c.update(commanded_joint_names, self.invalid_joints_callback, robot_mode=self.node.robot_mode) for c in command_groups] diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 1911e4d..e6e587a 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -37,10 +37,8 @@ BACKLASH_DEBUG = False class StretchBodyNode: def __init__(self): - self.use_robotis_head = True self.use_robotis_end_of_arm = True - self.use_robotis_trajectories = False self.default_goal_timeout_s = 10.0 self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s) @@ -64,12 +62,6 @@ class StretchBodyNode: self.mobile_base_manipulation_origin = None #{'x':None, 'y':None, 'theta':None} - self.use_lift = True - - # Clip the command, instead of invalidating it, if it is close - # enough to the valid ranges. - self.trajectory_debug = True - self.robot_stop_lock = threading.Lock() self.stop_the_robot = False @@ -154,16 +146,10 @@ class StretchBodyNode: vel_out = arm_status['vel'] eff_out = arm_status['motor']['effort'] - if self.use_lift: - # assign relevant lift status to variables - lift_status = robot_status['lift'] - pos_up = lift_status['pos'] - vel_up = lift_status['vel'] - eff_up = lift_status['motor']['effort'] - else: - pos_up = 0.242 - vel_up = 0.0 - eff_up = 0.0 + lift_status = robot_status['lift'] + pos_up = lift_status['pos'] + vel_up = lift_status['vel'] + eff_up = lift_status['motor']['effort'] if self.use_robotis_end_of_arm: # assign relevant wrist status to variables @@ -418,8 +404,7 @@ class StretchBodyNode: def calibrate(self): def code_to_run(): - if self.use_lift: - self.robot.lift.home() + self.robot.lift.home() self.robot.arm.home() self.change_mode('calibration', code_to_run)