Removed obsolete use_lift flag

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]

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)