Populated command groups' init_execution method

hello_helpers/src/hello_helpers/gripper_conversion.py

@@ -51,6 +51,11 @@ class GripperConversion:
         robotis_out = self.aperture_to_robotis(aperture_m)
         return robotis_out
 
+    def robotis_to_finger(self, robotis_pct):
+        aperture_m = self.robotis_to_aperture(robotis_pct)
+        finger_rad = self.aperture_to_finger_rad(aperture_m)
+        return finger_rad
+
     def status_to_all(self, gripper_status):
         aperture_m = self.robotis_to_aperture(gripper_status['pos_pct'])
         finger_rad = self.aperture_to_finger_rad(aperture_m)

stretch_core/nodes/command_groups.py

@@ -41,7 +41,7 @@ class SimpleCommandGroup:
     def ros_to_mechaduino(self, joint_ros):
         return joint_ros
 
-    def init_execution(self, **kwargs):
+    def init_execution(self, robot, robot_status, **kwargs):
         pass
 
     def update_execution(self, robot_status, **kwargs):
@@ -61,6 +61,15 @@ class HeadPanCommandGroup(SimpleCommandGroup):
         self.head_pan_calibrated_offset = head_pan_calibrated_offset
         self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset
 
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.joint_goal:
+            pan_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            robot.head.move_by('head_pan', pan_error)
+            if pan_error > 0.0:
+                kwargs['backlash_state']['head_pan_looked_left'] = True
+            else:
+                kwargs['backlash_state']['head_pan_looked_left'] = False
+
     def update_execution(self, robot_status, **kwargs):
         backlash_state = kwargs['backlash_state']
         if self.joint_goal:
@@ -77,10 +86,20 @@ class HeadPanCommandGroup(SimpleCommandGroup):
 
 
 class HeadTiltCommandGroup(SimpleCommandGroup):
-    def __init__(self, head_tilt_calibrated_offset, head_tilt_calibrated_looking_up_offset):
+    def __init__(self, head_tilt_calibrated_offset, head_tilt_calibrated_looking_up_offset, head_tilt_backlash_transition_angle):
         SimpleCommandGroup.__init__(self, 'joint_head_tilt', acceptable_joint_error=0.52, clip_ros_tolerance=0.001)
         self.head_tilt_calibrated_offset = head_tilt_calibrated_offset
         self.head_tilt_calibrated_looking_up_offset = head_tilt_calibrated_looking_up_offset
+        self.head_tilt_backlash_transition_angle = head_tilt_backlash_transition_angle
+
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.joint_goal:
+            tilt_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            robot.head.move_by('head_tilt', tilt_error)
+            if tilt_error > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset):
+                kwargs['backlash_state']['head_tilt_looking_up'] = True
+            else:
+                kwargs['backlash_state']['head_tilt_looking_up'] = False
 
     def update_execution(self, robot_status, **kwargs):
         backlash_state = kwargs['backlash_state']
@@ -101,6 +120,10 @@ class WristYawCommandGroup(SimpleCommandGroup):
     def __init__(self):
         SimpleCommandGroup.__init__(self, 'joint_wrist_yaw', acceptable_joint_error=0.015, clip_ros_tolerance=0.001)
 
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.joint_goal:
+            robot.end_of_arm.move_by('wrist_yaw', self.update_execution(robot_status))
+
     def update_execution(self, robot_status, **kwargs):
         if self.joint_goal:
             self.current_joint_hello = robot_status['end_of_arm']['wrist_yaw']['pos']
@@ -117,6 +140,7 @@ class GripperCommandGroup:
         self.acceptable_joint_error = acceptable_joint_error
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
         self.gripper_conversion = GripperConversion()
+        self.gripper_joint_goal_valid = False
 
     def update(self, joint_names, invalid_joints_error_func, **kwargs):
         self.use_gripper_joint = False
@@ -145,6 +169,7 @@ class GripperCommandGroup:
             return 0
 
     def set_goal(self, point, invalid_goal_error_func, **kwargs):
+        self.gripper_joint_goal_valid = False
         self.gripper_joint_goal = False
         self.goal_gripper_joint = None
         goal = None
@@ -152,9 +177,9 @@ class GripperCommandGroup:
             name = self.gripper_joint_command_name
             goal = point.positions[self.gripper_joint_command_index]
             if ((name == 'joint_gripper_finger_left') or (name == 'joint_gripper_finger_right')):
-                self.goal_gripper_joint = self.gripper_conversion.finger_to_robotis(goal)
+                self.goal_gripper_joint = goal
             if (name == 'gripper_aperture'):
-                self.goal_gripper_joint = self.gripper_conversion.aperture_to_robotis(goal)
+                self.goal_gripper_joint = goal
             self.gripper_joint_goal = True
 
             # Check that the goal is valid.
@@ -171,16 +196,33 @@ class GripperCommandGroup:
     def ros_to_mechaduino(self, joint_ros):
         return joint_ros
 
-    def init_execution(self, **kwargs):
-        pass
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.gripper_joint_goal_valid:
+            name = self.gripper_joint_command_name
+            delta = self.update_execution(robot_status)
+            if (name == 'joint_gripper_finger_left') or (name == 'joint_gripper_finger_right'):
+                robotis_delta = self.gripper_conversion.finger_to_robotis(delta)
+            if (name == 'gripper_aperture'):
+                robotis_delta = self.gripper_conversion.aperture_to_robotis(delta)
+            robot.end_of_arm.move_by('stretch_gripper', robotis_delta)
 
     def update_execution(self, robot_status, **kwargs):
-        pass
+        if self.gripper_joint_goal_valid:
+            name = self.gripper_joint_command_name
+            robotis_pct = robot_status['end_of_arm']['stretch_gripper']['pos_pct']
+            if (name == 'joint_gripper_finger_left') or (name == 'joint_gripper_finger_right'):
+                self.current_gripper_pos = self.gripper_conversion.robotis_to_finger(robotis_pct)
+            if name == 'gripper_aperture':
+                self.current_gripper_pos = self.gripper_conversion.robotis_to_aperture(robotis_pct)
+
+            self.gripper_error = self.goal_gripper_joint - self.current_gripper_pos
+            return self.gripper_error
+        else:
+            return None
 
     def goal_reached(self):
-        # TODO: check the gripper state
-        if self.use_gripper_joint:
-            return True
+        if self.gripper_joint_goal_valid:
+            return (abs(self.gripper_error) < self.acceptable_joint_error)
         else:
             return True
 
@@ -240,8 +282,14 @@ class TelescopingCommandGroup:
     def ros_to_mechaduino(self, wrist_extension_ros):
         return wrist_extension_ros
 
-    def init_execution(self, **kwargs):
-        pass
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.extension_goal:
+            extension_error_m = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            robot.arm.move_by(extension_error_m)
+            if extension_error_m < 0.0:
+                kwargs['backlash_state']['wrist_extension_retracted'] = True
+            else:
+                kwargs['backlash_state']['wrist_extension_retracted'] = False
 
     def update_execution(self, robot_status, **kwargs):
         backlash_state = kwargs['backlash_state']
@@ -300,8 +348,9 @@ class LiftCommandGroup:
     def ros_to_mechaduino(self, lift_ros):
         return lift_ros
 
-    def init_execution(self, **kwargs):
-        pass
+    def init_execution(self, robot, robot_status, **kwargs):
+        if self.lift_goal:
+            robot.lift.move_by(self.update_execution(robot_status))
 
     def update_execution(self, robot_status, **kwargs):
         if self.lift_goal:
@@ -326,16 +375,14 @@ class MobileBaseCommandGroup:
         self.acceptable_mobile_base_error_rad = (np.pi/180.0) * 6.0
         self.excellent_mobile_base_error_rad = (np.pi/180.0) * 0.6
 
-    def update(self, joint_names, invalid_joints_error_func, **kwargs):
+    def update(self, joint_names, invalid_joints_callback, **kwargs):
         robot_mode = kwargs['robot_mode']
-        self.use_mobile_base_virtual_joint = False
-        self.use_mobile_base_inc = False
         self.use_mobile_base_inc_rot = ('rotate_mobile_base' in joint_names)
         self.use_mobile_base_inc_trans = ('translate_mobile_base' in joint_names)
-        self.use_mobile_base_inc = (self.use_mobile_base_inc_rot or self.use_mobile_base_inc_trans)
         self.use_mobile_base_virtual_joint = ('joint_mobile_base_translation' in joint_names)
 
-        if self.use_mobile_base_inc and self.use_mobile_base_virtual_joint:
+        if (self.use_mobile_base_inc_trans or self.use_mobile_base_inc_rot) \
+            and self.use_mobile_base_virtual_joint:
             # Commands that attempt to control the mobile base's
             # virtual joint together with mobile base incremental
             # commands are invalid.
@@ -344,31 +391,36 @@ class MobileBaseCommandGroup:
                 command_string += ' rotate_mobile_base '
             if self.use_mobile_base_inc_trans:
                 command_string += ' translate_mobile_base '
-            error_string = 'received a command for the mobile base virtual joint (joint_mobile_base_translation) and mobile base incremental motions ({0}). These are mutually exclusive options. The joint names in the received command = {1}'.format(command_string, joint_names)
-            invalid_joints_error_func(error_string)
+            err_str = 'Received a command for the mobile base virtual joint (joint_mobile_base_translation) \
+                       and mobile base incremental motions ({0}). These are mutually exclusive options. \
+                       The joint names in the received command = {1}'.format(command_string, joint_names)
+            invalid_joints_callback(err_str)
             return False
 
         if self.use_mobile_base_virtual_joint:
-            # Check if a mobile base command was received.
             if robot_mode != 'manipulation':
-                error_string = 'must be in manipulation mode to receive a command for the joint_mobile_base_translation joint. Current mode = {0}.'.format(robot_mode)
-                invalid_joints_error_func(error_string)
+                err_str = 'Must be in manipulation mode to receive a command for the \
+                           joint_mobile_base_translation joint. Current mode = {0}.'.format(robot_mode)
+                invalid_joints_callback(err_str)
                 return False
             self.virtual_joint_mobile_base_index = joint_names.index('joint_mobile_base_translation')
 
         if self.use_mobile_base_inc_rot:
             if robot_mode != 'position':
-                error_string = 'must be in position mode to receive a rotate_mobile_base command. Current mode = {0}.'.format(robot_mode)
-                invalid_joints_error_func(error_string)
+                err_str = 'Must be in position mode to receive a rotate_mobile_base command. \
+                           Current mode = {0}.'.format(robot_mode)
+                invalid_joints_callback(err_str)
                 return False
             self.rotate_mobile_base_index = joint_names.index('rotate_mobile_base')
 
         if self.use_mobile_base_inc_trans:
             if robot_mode != 'position':
-                error_string = 'must be in position mode to receive a translate_mobile_base command. Current mode = {0}.'.format(robot_mode)
-                invalid_joints_error_func(error_string)
+                err_str = 'Must be in position mode to receive a translate_mobile_base command. \
+                           Current mode = {0}.'.format(robot_mode)
+                invalid_joints_callback(err_str)
                 return False
             self.translate_mobile_base_index = joint_names.index('translate_mobile_base')
+
         return True
 
     def get_num_valid_commands(self):
@@ -381,7 +433,7 @@ class MobileBaseCommandGroup:
             number_of_valid_joints += 1
         return number_of_valid_joints
 
-    def set_goal(self, point, invalid_goal_error_func, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, **kwargs):
         self.rotate_mobile_base_goal = False
         self.translate_mobile_base_goal = False
         self.virtual_joint_mobile_base_goal = False
@@ -400,8 +452,11 @@ class MobileBaseCommandGroup:
             self.translate_mobile_base_goal = True
 
         if self.rotate_mobile_base_goal and self.translate_mobile_base_goal:
-            error_string = 'received a goal point with simultaneous rotation and translation mobile base goals. This is not allowed. Only one is allowed to be sent for a given goal point. rotate_mobile_base = {0} and translate_mobile_base = {1}'.format(self.goal_rotate_mobile_base_ros, self.goal_translate_mobile_base_ros)
-            invalid_goal_error_func(error_string)
+            err_str = 'Received a goal point with simultaneous rotation and translation mobile base goals. \
+                       This is not allowed. Only one is allowed to be sent for a given goal point. \
+                       rotate_mobile_base = {0} and translate_mobile_base = \
+                       {1}'.format(self.goal_rotate_mobile_base_mecha, self.goal_translate_mobile_base_mecha)
+            invalid_goal_callback(err_str)
             return False
 
         return True
@@ -431,13 +486,18 @@ class MobileBaseCommandGroup:
         else:
             return (manipulation_origin['x'] + ros_pos)
 
-    def init_execution(self, **kwargs):
-        robot_status = kwargs['robot_status']
+    def init_execution(self, robot, robot_status, **kwargs):
         self.initial_mobile_base_translation_mecha_x = robot_status['base']['x']
         self.initial_mobile_base_translation_mecha_y = robot_status['base']['y']
         self.initial_mobile_base_rotation_mecha = robot_status['base']['theta']
         b = robot_status['base']
 
+        mobile_base_error_m, mobile_base_error_rad = self.update_execution(robot_status)
+        if mobile_base_error_m is not None:
+            robot.base.translate_by(mobile_base_error_m)
+        if mobile_base_error_rad is not None:
+            robot.base.rotate_by(mobile_base_error_rad)
+
     def update_execution(self, robot_status, **kwargs):
         current_mobile_base_translation_mecha_x = robot_status['base']['x']
         current_mobile_base_translation_mecha_y = robot_status['base']['y']

stretch_core/nodes/joint_trajectory_server.py

@@ -31,7 +31,8 @@ class JointTrajectoryAction:
         self.head_pan_cg = HeadPanCommandGroup(self.node.head_pan_calibrated_offset_rad,
                                                self.node.head_pan_calibrated_looked_left_offset_rad)
         self.head_tilt_cg = HeadTiltCommandGroup(self.node.head_tilt_calibrated_offset_rad,
-                                                 self.node.head_tilt_calibrated_looking_up_offset_rad)
+                                                 self.node.head_tilt_calibrated_looking_up_offset_rad,
+                                                 self.node.head_tilt_backlash_transition_angle_rad)
         self.wrist_yaw_cg = WristYawCommandGroup()
         self.gripper_cg = GripperCommandGroup()
 
@@ -99,9 +100,11 @@ class JointTrajectoryAction:
                 return
 
             robot_status = self.node.robot.get_status() # uses lock held by robot
-            [c.init_execution(robot_status=robot_status) for c in command_groups]
+            [c.init_execution(self.node.robot, robot_status, backlash_state=self.node.backlash_state)
+             for c in command_groups]
+            self.node.robot.push_command()
+
             goals_reached = [c.goal_reached() for c in command_groups]
-            incremental_commands_executed = False
             update_rate = rospy.Rate(15.0)
             goal_start_time = rospy.Time.now()
 
@@ -137,57 +140,6 @@ class JointTrajectoryAction:
                         self.node.robot_mode_rwlock.release_read()
                         return
 
-                if not incremental_commands_executed:
-                    translate = (mobile_base_error_m is not None)
-                    rotate = (mobile_base_error_rad is not None)
-                    if translate and rotate:
-                        err_str = 'Simultaneous translation and rotation of the mobile base requested. \
-                                   This is not allowed.'
-                        self.invalid_goal_callback(err_str)
-                        self.node.robot_mode_rwlock.release_read()
-                        return
-                    if translate:
-                        self.node.robot.base.translate_by(mobile_base_error_m)
-                    if rotate:
-                        self.node.robot.base.rotate_by(mobile_base_error_rad)
-
-                    if self.telescoping_cg.extension_goal:
-                        self.node.robot.arm.move_by(extension_error_m)
-                        if extension_error_m < 0.0:
-                            self.node.backlash_state['wrist_extension_retracted'] = True
-                        else:
-                            self.node.backlash_state['wrist_extension_retracted'] = False
-
-                    if self.node.use_lift:
-                        if self.lift_cg.lift_goal:
-                            self.node.robot.lift.move_by(lift_error_m)
-
-                    if self.head_pan_cg.joint_goal:
-                        self.node.robot.head.move_by('head_pan', self.head_pan_cg.joint_error)
-                        if self.head_pan_cg.joint_error > 0.0:
-                            self.node.backlash_state['head_pan_looked_left'] = True
-                        else:
-                            self.node.backlash_state['head_pan_looked_left'] = False
-
-                    if self.head_tilt_cg.joint_goal:
-                        self.node.robot.head.move_by('head_tilt', self.head_tilt_cg.joint_error)
-                        if self.head_tilt_cg.joint_target > (self.node.head_tilt_backlash_transition_angle_rad + self.node.head_tilt_calibrated_offset_rad):
-                            self.node.backlash_state['head_tilt_looking_up'] = True
-                        else:
-                            self.node.backlash_state['head_tilt_looking_up'] = False
-
-                    if self.wrist_yaw_cg.joint_goal:
-                        self.node.robot.end_of_arm.move_to('wrist_yaw', self.wrist_yaw_cg.joint_target)
-
-                    if self.gripper_cg.gripper_joint_goal:
-                        gripper_command = self.gripper_cg.goal_gripper_joint
-                        rospy.logdebug('{0} gripper debug: move_to stretch_gripper = \
-                                        {1}'.format(self.node.node_name, gripper_command))
-                        self.node.robot.end_of_arm.move_to('stretch_gripper', gripper_command)
-
-                    self.node.robot.push_command()
-                    incremental_commands_executed = True
-
                 goals_reached = [c.goal_reached() for c in command_groups]
                 update_rate.sleep()