Merge pull request #52 from hello-robot/feature/automatic_jointstates

Calculate joint states automatically using command groups

hello_helpers/src/hello_helpers/simple_command_group.py

@@ -0,0 +1,175 @@
+import hello_helpers.hello_misc as hm
+
+
+class SimpleCommandGroup:
+    def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015):
+        """Simple command group to extend
+
+        Attributes
+        ----------
+        name: str
+            joint name
+        range: tuple(float)
+            acceptable joint bounds
+        active: bool
+            whether joint is active
+        index: int
+            index of joint's goal in point
+        goal: dict
+            components of the goal
+        error: float
+            the error between actual and desired
+        acceptable_joint_error: float
+            how close to zero the error must reach
+        """
+        self.name = joint_name
+        self.range = joint_range
+        self.active = False
+        self.index = None
+        self.goal = {"position": None}
+        self.error = None
+        self.acceptable_joint_error = acceptable_joint_error
+
+    def get_num_valid_commands(self):
+        """Returns number of active joints in the group
+
+        Returns
+        -------
+        int
+            the number of active joints within this group
+        """
+        if self.active:
+            return 1
+
+        return 0
+
+    def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
+        """Activates joints in the group
+
+        Checks commanded joints to activate the command
+        group and validates joints used correctly.
+
+        Parameters
+        ----------
+        commanded_joint_names: list(str)
+            list of commanded joints in the trajectory
+        invalid_joints_callback: func
+            error callback for misuse of joints in trajectory
+
+        Returns
+        -------
+        bool
+            False if commanded joints invalid, else True
+        """
+        self.active = False
+        self.index = None
+        if self.name in commanded_joint_names:
+            self.index = commanded_joint_names.index(self.name)
+            self.active = True
+
+        return True
+
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
+        """Sets goal for the joint group
+
+        Sets and validates the goal point for the joints
+        in this command group.
+
+        Parameters
+        ----------
+        point: trajectory_msgs.JointTrajectoryPoint
+            the target point for all joints
+        invalid_goal_callback: func
+            error callback for invalid goal
+        fail_out_of_range_goal: bool
+            whether to bound out-of-range goals to range or fail
+
+        Returns
+        -------
+        bool
+            False if commanded goal invalid, else True
+        """
+        self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
+        if self.active:
+            goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
+            if goal_pos is None:
+                err_str = ("Received goal point with positions array length={0}. "
+                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
+                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
+                invalid_goal_callback(err_str)
+                return False
+
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
+            self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
+            self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
+            self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
+            if self.goal['position'] is None:
+                err_str = ("Received {0} goal point that is out of bounds. "
+                            "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
+                invalid_goal_callback(err_str)
+                return False
+
+        return True
+
+    def init_execution(self, robot, robot_status, **kwargs):
+        """Starts execution of the point
+
+        Uses Stretch's Python API to begin moving to the
+        target point.
+
+        Parameters
+        ----------
+        robot: stretch_body.robot.Robot
+            top-level interface to Python API
+        robot_status: dict
+            robot's current status
+        """
+        raise NotImplementedError
+
+    def update_execution(self, robot_status, **kwargs):
+        """Monitors progress of joint group
+
+        Checks against robot's status to track progress
+        towards the target point.
+
+        This method must set self.error.
+
+        Parameters
+        ----------
+        robot_status: dict
+            robot's current status
+
+        Returns
+        -------
+        float/None
+            error value if group active, else None
+        """
+        raise NotImplementedError
+
+    def goal_reached(self):
+        """Returns whether reached target point
+
+        Returns
+        -------
+        bool
+            if active, whether reached target point, else True
+        """
+        if self.active:
+            return (abs(self.error) < self.acceptable_joint_error)
+
+        return True
+
+    def joint_state(self, robot_status, **kwargs):
+        """Returns state of the joint group
+
+        Parameters
+        ----------
+        robot_status: dict
+            whole robot's current status
+
+        Returns
+        -------
+        (float, float, float)
+            Current position, velocity, and effort
+        """
+        raise NotImplementedError

stretch_core/nodes/command_groups.py

@@ -3,233 +3,70 @@ from __future__ import print_function
 
 import numpy as np
 import hello_helpers.hello_misc as hm
+from hello_helpers.simple_command_group import SimpleCommandGroup
 from hello_helpers.gripper_conversion import GripperConversion
 
 
-class SimpleCommandGroup:
-    def __init__(self, joint_name, joint_range, acceptable_joint_error=0.015):
-        """Simple command group to extend
-
-        Attributes
-        ----------
-        name: str
-            joint name
-        range: tuple(float)
-            acceptable joint bounds
-        active: bool
-            whether joint is active
-        index: int
-            index of joint's goal in point
-        goal: dict
-            components of the goal
-        error: float
-            the error between actual and desired
-        acceptable_joint_error: float
-            how close to zero the error must reach
-        """
-        self.name = joint_name
-        self.range = joint_range
-        self.active = False
-        self.index = None
-        self.goal = {"position": None}
-        self.error = None
-        self.acceptable_joint_error = acceptable_joint_error
-
-    def get_num_valid_commands(self):
-        """Returns number of active joints in the group
-
-        Returns
-        -------
-        int
-            the number of active joints within this group
-        """
-        if self.active:
-            return 1
-
-        return 0
-
-    def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
-        """Activates joints in the group
-
-        Checks commanded joints to activate the command
-        group and validates joints used correctly.
-
-        Parameters
-        ----------
-        commanded_joint_names: list(str)
-            list of commanded joints in the trajectory
-        invalid_joints_callback: func
-            error callback for misuse of joints in trajectory
-
-        Returns
-        -------
-        bool
-            False if commanded joints invalid, else True
-        """
-        self.active = False
-        self.index = None
-        if self.name in commanded_joint_names:
-            self.index = commanded_joint_names.index(self.name)
-            self.active = True
-
-        return True
-
-    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
-        """Sets goal for the joint group
-
-        Sets and validates the goal point for the joints
-        in this command group.
-
-        Parameters
-        ----------
-        point: trajectory_msgs.JointTrajectoryPoint
-            the target point for all joints
-        invalid_goal_callback: func
-            error callback for invalid goal
-        fail_out_of_range_goal: bool
-            whether to bound out-of-range goals to range or fail
-
-        Returns
-        -------
-        bool
-            False if commanded goal invalid, else True
-        """
-        self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
-        if self.active:
-            goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
-            if goal_pos is None:
-                err_str = ("Received goal point with positions array length={0}. "
-                           "This joint ({1})'s index is {2}. Length of array must cover all joints listed "
-                           "in commanded_joint_names.").format(len(point.positions), self.name, self.index)
-                invalid_goal_callback(err_str)
-                return False
-
-            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
-            self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
-            self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
-            self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
-            if self.goal['position'] is None:
-                err_str = ("Received {0} goal point that is out of bounds. "
-                            "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
-                invalid_goal_callback(err_str)
-                return False
-
-        return True
-
-    def init_execution(self, robot, robot_status, **kwargs):
-        """Starts execution of the point
-
-        Uses Stretch's Python API to begin moving to the
-        target point.
-
-        Parameters
-        ----------
-        robot: stretch_body.robot.Robot
-            top-level interface to Python API
-        robot_status: dict
-            robot's current status
-        """
-        raise NotImplementedError
-
-    def update_execution(self, robot_status, **kwargs):
-        """Monitors progress of joint group
-
-        Checks against robot's status to track progress
-        towards the target point.
-
-        This method must set self.error.
-
-        Parameters
-        ----------
-        robot_status: dict
-            robot's current status
-
-        Returns
-        -------
-        float/None
-            error value if group active, else None
-        """
-        raise NotImplementedError
-
-    def goal_reached(self):
-        """Returns whether reached target point
-
-        Returns
-        -------
-        bool
-            if active, whether reached target point, else True
-        """
-        if self.active:
-            return (abs(self.error) < self.acceptable_joint_error)
-
-        return True
-
-
 class HeadPanCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_rad, head_pan_calibrated_offset, head_pan_calibrated_looked_left_offset):
+    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)
-        self.head_pan_calibrated_offset = head_pan_calibrated_offset
-        self.head_pan_calibrated_looked_left_offset = head_pan_calibrated_looked_left_offset
+        self.calibrated_offset_rad = calibrated_offset_rad
+        self.looked_left_offset_rad = calibrated_looked_left_offset_rad
+        self.looked_left = False
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
-            _, pan_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            _, pan_error = self.update_execution(robot_status)
             robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
-            if pan_error > 0.0:
-                kwargs['backlash_state']['head_pan_looked_left'] = True
-            else:
-                kwargs['backlash_state']['head_pan_looked_left'] = False
+            self.looked_left = pan_error > 0.0
 
     def update_execution(self, robot_status, **kwargs):
         self.error = None
-        backlash_state = kwargs['backlash_state']
         if self.active:
-            if backlash_state['head_pan_looked_left']:
-                pan_backlash_correction = self.head_pan_calibrated_looked_left_offset
-            else:
-                pan_backlash_correction = 0.0
-            pan_current = robot_status['head']['head_pan']['pos'] + \
-                          self.head_pan_calibrated_offset + pan_backlash_correction
+            pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0
+            pan_current = robot_status['head']['head_pan']['pos'] + self.calibrated_offset_rad + pan_backlash_correction
             self.error = self.goal['position'] - pan_current
             return self.name, self.error
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        pan_status = robot_status['head']['head_pan']
+        pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0
+        pos = pan_status['pos'] + self.calibrated_offset_rad + pan_backlash_correction
+        return (pos, pan_status['vel'], pan_status['effort'])
+
 
 class HeadTiltCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_rad, head_tilt_calibrated_offset,
-                 head_tilt_calibrated_looking_up_offset,
-                 head_tilt_backlash_transition_angle):
+    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)
-        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
+        self.calibrated_offset_rad = calibrated_offset_rad
+        self.looking_up_offset_rad = calibrated_looking_up_offset_rad
+        self.backlash_transition_angle_rad = backlash_transition_angle_rad
+        self.looking_up = False
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
-            _, tilt_error = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            _, tilt_error = self.update_execution(robot_status)
             robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
-            #if tilt_error > (self.head_tilt_backlash_transition_angle + self.head_tilt_calibrated_offset):
-            if self.goal['position'] > (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
+            self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad)
 
     def update_execution(self, robot_status, **kwargs):
         self.error = None
-        backlash_state = kwargs['backlash_state']
         if self.active:
-            if backlash_state['head_tilt_looking_up']:
-                tilt_backlash_correction = self.head_tilt_calibrated_looking_up_offset
-            else:
-                tilt_backlash_correction = 0.0
-            tilt_current = robot_status['head']['head_tilt']['pos'] + \
-                           self.head_tilt_calibrated_offset + tilt_backlash_correction
+            tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0
+            tilt_current = robot_status['head']['head_tilt']['pos'] + self.calibrated_offset_rad + tilt_backlash_correction
             self.error = self.goal['position'] - tilt_current
             return self.name, self.error
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        tilt_status = robot_status['head']['head_tilt']
+        tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0
+        pos = tilt_status['pos'] + self.calibrated_offset_rad + tilt_backlash_correction
+        return (pos, tilt_status['vel'], tilt_status['effort'])
+
 
 class WristYawCommandGroup(SimpleCommandGroup):
     def __init__(self, range_rad):
@@ -250,10 +87,14 @@ class WristYawCommandGroup(SimpleCommandGroup):
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        yaw_status = robot_status['end_of_arm']['wrist_yaw']
+        return (yaw_status['pos'], yaw_status['vel'], yaw_status['effort'])
+
 
 class GripperCommandGroup(SimpleCommandGroup):
     def __init__(self, range_robotis):
-        SimpleCommandGroup.__init__(self, None, None, acceptable_joint_error=1.0)
+        SimpleCommandGroup.__init__(self, 'joint_gripper_finger_left', None, acceptable_joint_error=1.0)
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
         self.gripper_conversion = GripperConversion()
         self.range_aperture_m = (self.gripper_conversion.robotis_to_aperture(range_robotis[0]),
@@ -330,14 +171,23 @@ class GripperCommandGroup(SimpleCommandGroup):
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        joint_name = kwargs['joint_name'] if 'joint_name' in kwargs.keys() else self.name
+        gripper_status = robot_status['end_of_arm']['stretch_gripper']
+        pos_aperture_m, pos_rad, effort, vel = self.gripper_conversion.status_to_all(gripper_status)
+        if (joint_name == 'gripper_aperture'):
+            return (pos_aperture_m, vel, effort)
+        elif (joint_name == 'joint_gripper_finger_left') or (joint_name == 'joint_gripper_finger_right'):
+            return (pos_rad, vel, effort)
+
 
-class TelescopingCommandGroup(SimpleCommandGroup):
-    def __init__(self, range_m, wrist_extension_calibrated_retracted_offset):
-        #SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.005)
+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)
-        self.wrist_extension_calibrated_retracted_offset = wrist_extension_calibrated_retracted_offset
         self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
         self.is_telescoping = False
+        self.retracted_offset_m = calibrated_retracted_offset_m
+        self.retracted = False
 
     def get_num_valid_commands(self):
         if self.active and self.is_telescoping:
@@ -417,36 +267,35 @@ class TelescopingCommandGroup(SimpleCommandGroup):
 
     def init_execution(self, robot, robot_status, **kwargs):
         if self.active:
-            _, extension_error_m = self.update_execution(robot_status, backlash_state=kwargs['backlash_state'])
+            _, extension_error_m = self.update_execution(robot_status)
             robot.arm.move_by(extension_error_m,
                               v_m=self.goal['velocity'],
                               a_m=self.goal['acceleration'],
                               contact_thresh_pos_N=self.goal['contact_threshold'],
                               contact_thresh_neg_N=-self.goal['contact_threshold'] \
                                                    if self.goal['contact_threshold'] is not None else None)
-            if extension_error_m < 0.0:
-                kwargs['backlash_state']['wrist_extension_retracted'] = True
-            else:
-                kwargs['backlash_state']['wrist_extension_retracted'] = False
+            self.retracted = extension_error_m < 0.0
 
     def update_execution(self, robot_status, **kwargs):
-        backlash_state = kwargs['backlash_state']
         success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
         self.error = None
         if self.active:
             if success_callback and robot_status['arm']['motor']['in_guarded_event']:
                 success_callback("{0} contact detected.".format(self.name))
                 return True
-            if backlash_state['wrist_extension_retracted']:
-                arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset
-            else:
-                arm_backlash_correction = 0.0
+            arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
             extension_current = robot_status['arm']['pos'] + arm_backlash_correction
             self.error = self.goal['position'] - extension_current
             return (self.telescoping_joints, self.error) if self.is_telescoping else (self.name, self.error)
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        arm_status = robot_status['arm']
+        arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
+        pos = arm_status['pos'] + arm_backlash_correction
+        return (pos, arm_status['vel'], arm_status['motor']['effort'])
+
 
 class LiftCommandGroup(SimpleCommandGroup):
     def __init__(self, range_m):
@@ -473,6 +322,10 @@ class LiftCommandGroup(SimpleCommandGroup):
 
         return None
 
+    def joint_state(self, robot_status, **kwargs):
+        lift_status = robot_status['lift']
+        return (lift_status['pos'], lift_status['vel'], lift_status['motor']['effort'])
+
 
 class MobileBaseCommandGroup(SimpleCommandGroup):
     def __init__(self, virtual_range_m=(-0.5, 0.5)):
@@ -689,3 +542,12 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                 return (abs(self.error) < self.acceptable_joint_error)
 
         return True
+
+    def joint_state(self, robot_status, **kwargs):
+        robot_mode = kwargs['robot_mode']
+        manipulation_origin = kwargs['manipulation_origin']
+        base_status = robot_status['base']
+        if robot_mode == "manipulation":
+            pos = base_status['x'] - manipulation_origin['x']
+            return (pos, base_status['x_vel'], base_status['effort'][0])
+        return (0.0, 0.0, 0.0)

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
 
 
@@ -42,18 +42,20 @@ class JointTrajectoryAction:
                                  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.head_pan_calibrated_offset_rad,
-                                               self.node.head_pan_calibrated_looked_left_offset_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.head_tilt_calibrated_offset_rad,
-                                                 self.node.head_tilt_calibrated_looking_up_offset_rad,
-                                                 self.node.head_tilt_backlash_transition_angle_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.telescoping_cg = TelescopingCommandGroup(tuple(r.arm.params['range_m']),
-                                                      self.node.wrist_extension_calibrated_retracted_offset_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.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):
         with self.node.robot_stop_lock:
@@ -68,11 +70,9 @@ class JointTrajectoryAction:
 
         ###################################################
         # Decide what to do based on the commanded joints.
-        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]
+                   for c in self.command_groups]
         if not all(updates):
             # The joint names violated at least one of the command
             # group's requirements. The command group should have
@@ -80,7 +80,7 @@ class JointTrajectoryAction:
             self.node.robot_mode_rwlock.release_read()
             return
 
-        num_valid_points = sum([c.get_num_valid_commands() for c in command_groups])
+        num_valid_points = sum([c.get_num_valid_commands() for c in self.command_groups])
         if num_valid_points <= 0:
             err_str = ("Received a command without any valid joint names."
                        "Received joint names = {0}").format(commanded_joint_names)
@@ -103,7 +103,7 @@ class JointTrajectoryAction:
 
             valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal,
                                       manipulation_origin=self.node.mobile_base_manipulation_origin)
-                           for c in command_groups]
+                           for c in self.command_groups]
             if not all(valid_goals):
                 # At least one of the goals violated the requirements
                 # of a command group. Any violations should have been
@@ -112,11 +112,11 @@ class JointTrajectoryAction:
                 return
 
             robot_status = self.node.robot.get_status() # uses lock held by robot
-            [c.init_execution(self.node.robot, robot_status, backlash_state=self.node.backlash_state)
-             for c in command_groups]
+            for c in self.command_groups:
+                c.init_execution(self.node.robot, robot_status)
             self.node.robot.push_command()
 
-            goals_reached = [c.goal_reached() for c in command_groups]
+            goals_reached = [c.goal_reached() for c in self.command_groups]
             update_rate = rospy.Rate(15.0)
             goal_start_time = rospy.Time.now()
 
@@ -140,9 +140,8 @@ class JointTrajectoryAction:
                         return
 
                 robot_status = self.node.robot.get_status()
-                named_errors = [c.update_execution(robot_status, success_callback=self.success_callback,
-                                                   backlash_state=self.node.backlash_state)
-                                for c in command_groups]
+                named_errors = [c.update_execution(robot_status, success_callback=self.success_callback)
+                                for c in self.command_groups]
                 # It's not clear how this could ever happen. The
                 # groups in command_groups.py seem to return
                 # (self.name, self.error) or None, rather than True.
@@ -151,7 +150,7 @@ class JointTrajectoryAction:
                     return
 
                 self.feedback_callback(commanded_joint_names, point, named_errors)
-                goals_reached = [c.goal_reached() for c in command_groups]
+                goals_reached = [c.goal_reached() for c in self.command_groups]
                 update_rate.sleep()
 
             rospy.logdebug("{0} joint_traj action: Achieved target point.".format(self.node.node_name))

stretch_core/nodes/stretch_driver

@@ -27,13 +27,8 @@ from nav_msgs.msg import Odometry
 from sensor_msgs.msg import JointState, Imu, MagneticField
 from std_msgs.msg import Header
 
-import hello_helpers.hello_misc as hm
-from hello_helpers.gripper_conversion import GripperConversion
 from joint_trajectory_server import JointTrajectoryAction
 
-GRIPPER_DEBUG = False
-BACKLASH_DEBUG = False
-
 
 class StretchBodyNode:
 
@@ -44,23 +39,6 @@ class StretchBodyNode:
         self.default_goal_timeout_s = 10.0
         self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s)
 
-        # Initialize calibration offsets
-        self.head_tilt_calibrated_offset_rad = 0.0
-        self.head_pan_calibrated_offset_rad = 0.0
-
-        # Initialize backlash state
-        self.backlash_state = {'head_tilt_looking_up' : False,
-                               'head_pan_looked_left' : False,
-                               'wrist_extension_retracted' : False}
-
-        # Initialize backlash offsets
-        self.head_pan_calibrated_looked_left_offset_rad = 0.0
-        self.head_tilt_calibrated_looking_up_offset_rad = 0.0
-        self.wrist_extension_calibrated_retracted_offset_m = 0.0
-        self.head_tilt_backlash_transition_angle_rad = -0.4
-
-        self.gripper_conversion = GripperConversion()
-
         self.mobile_base_manipulation_origin = None #{'x':None, 'y':None, 'theta':None}
 
         self.robot_stop_lock = threading.Lock()
@@ -85,47 +63,33 @@ class StretchBodyNode:
     def command_mobile_base_velocity_and_publish_state(self):
         self.robot_mode_rwlock.acquire_read()
 
-        if BACKLASH_DEBUG:
-            print('***')
-            print('self.backlash_state =', self.backlash_state)
-
-        # set new mobile base velocities, if appropriate
-        # check on thread safety for this with callback that sets velocity command values
+        # set new mobile base velocities if available
         if self.robot_mode == 'navigation': 
             time_since_last_twist = rospy.get_time() - self.last_twist_time
             if time_since_last_twist < self.timeout:
                 self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps)
                 self.robot.push_command()
             else:
-                # Too much information in general, although it could be blocked, since it's just INFO.
+                # Watchdog timer stops motion if no communication within timeout
                 self.robot.base.set_velocity(0.0, 0.0)
                 self.robot.push_command()
 
-        # get copy of the current robot status (uses lock held by the robot)
-        robot_status = self.robot.get_status()
-
-
-        # In the future, consider using time stamps from the robot's
+        # TODO: In the future, consider using time stamps from the robot's
         # motor control boards and other boards. These would need to
         # be synchronized with the rospy clock.
         #robot_time = robot_status['timestamp_pc']
-        #rospy.loginfo('robot_time =', robot_time)        
         #current_time = rospy.Time.from_sec(robot_time)
-
         current_time = rospy.Time.now()
+        robot_status = self.robot.get_status()
 
         # obtain odometry
-        # assign relevant base status to variables
         base_status = robot_status['base']
         x = base_status['x']
-        x_raw = x
         y = base_status['y']
         theta = base_status['theta']
+        q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta)
         x_vel = base_status['x_vel']
-        x_vel_raw = x_vel
-        y_vel = base_status['y_vel']
         x_effort = base_status['effort'][0]
-        x_effort_raw = x_effort
         theta_vel = base_status['theta_vel']
         pose_time_s = base_status['pose_time_s']
 
@@ -134,71 +98,7 @@ class StretchBodyNode:
             x_vel = 0.0
             x_effort = 0.0
 
-        # assign relevant arm status to variables
-        arm_status = robot_status['arm']
-        if self.backlash_state['wrist_extension_retracted']:
-            arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset_m
-        else: 
-            arm_backlash_correction = 0.0
-
-        if BACKLASH_DEBUG: 
-            print('arm_backlash_correction =', arm_backlash_correction)
-        pos_out = arm_status['pos'] + arm_backlash_correction
-        vel_out = arm_status['vel']
-        eff_out = arm_status['motor']['effort']
-
-        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
-            wrist_status = robot_status['end_of_arm']['wrist_yaw']
-            wrist_rad = wrist_status['pos']
-            wrist_vel = wrist_status['vel']
-            wrist_effort = wrist_status['effort']
-
-            # assign relevant gripper status to variables
-            gripper_status = robot_status['end_of_arm']['stretch_gripper']
-            if GRIPPER_DEBUG: 
-                print('-----------------------')
-                print('gripper_status[\'pos\'] =', gripper_status['pos'])
-                print('gripper_status[\'pos_pct\'] =', gripper_status['pos_pct'])
-            gripper_aperture_m, gripper_finger_rad, gripper_finger_effort, gripper_finger_vel = self.gripper_conversion.status_to_all(gripper_status)
-            if GRIPPER_DEBUG: 
-                print('gripper_aperture_m =', gripper_aperture_m)
-                print('gripper_finger_rad =', gripper_finger_rad)
-                print('-----------------------')
-
-        if self.use_robotis_head: 
-            # assign relevant head pan status to variables
-            head_pan_status = robot_status['head']['head_pan']
-            if self.backlash_state['head_pan_looked_left']:
-                pan_backlash_correction = self.head_pan_calibrated_looked_left_offset_rad 
-            else:
-                pan_backlash_correction = 0.0
-            if BACKLASH_DEBUG: 
-                print('pan_backlash_correction =', pan_backlash_correction)
-            head_pan_rad = head_pan_status['pos'] + self.head_pan_calibrated_offset_rad + pan_backlash_correction
-            head_pan_vel = head_pan_status['vel']
-            head_pan_effort = head_pan_status['effort']
-
-            # assign relevant head tilt status to variables
-            head_tilt_status = robot_status['head']['head_tilt']
-            if self.backlash_state['head_tilt_looking_up']:
-                tilt_backlash_correction = self.head_tilt_calibrated_looking_up_offset_rad
-            else:
-                tilt_backlash_correction = 0.0
-            if BACKLASH_DEBUG: 
-                print('tilt_backlash_correction =', tilt_backlash_correction)
-            head_tilt_rad = head_tilt_status['pos'] + self.head_tilt_calibrated_offset_rad + tilt_backlash_correction
-            head_tilt_vel = head_tilt_status['vel']
-            head_tilt_effort = head_tilt_status['effort']
-
-        q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta)
-
-        if self.broadcast_odom_tf: 
+        if self.broadcast_odom_tf:
             # publish odometry via TF
             t = TransformStamped()
             t.header.stamp = current_time
@@ -213,7 +113,7 @@ class StretchBodyNode:
             t.transform.rotation.w = q[3]
             self.tf_broadcaster.sendTransform(t)
 
-        # publish odometry via the odom topic
+        # publish odometry
         odom = Odometry()
         odom.header.stamp = current_time
         odom.header.frame_id = self.odom_frame_id
@@ -228,80 +128,36 @@ class StretchBodyNode:
         odom.twist.twist.angular.z = theta_vel
         self.odom_pub.publish(odom)
 
-        # publish joint state for the arm
+        # publish joint state
         joint_state = JointState()
         joint_state.header.stamp = current_time
-        # joint_arm_l3 is the most proximal and joint_arm_l0 is the
-        # most distal joint of the telescoping arm model. The joints
-        # are connected in series such that moving the most proximal
-        # joint moves all the other joints in the global frame.
-        joint_state.name = ['wrist_extension', 'joint_lift', 'joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
-
-        # set positions of the telescoping joints
-        positions = [pos_out/4.0 for i in xrange(4)]
-        # set lift position
-        positions.insert(0, pos_up)
-        # set wrist_extension position
-        positions.insert(0, pos_out)
-
-        # set velocities of the telescoping joints
-        velocities = [vel_out/4.0 for i in xrange(4)]
-        # set lift velocity
-        velocities.insert(0, vel_up)
-        # set wrist_extension velocity
-        velocities.insert(0, vel_out)
-
-        # set efforts of the telescoping joints
-        efforts = [eff_out for i in xrange(4)]
-        # set lift effort
-        efforts.insert(0, eff_up)
-        # set wrist_extension effort
-        efforts.insert(0, eff_out)
-
-        if self.use_robotis_head:
-            head_joint_names = ['joint_head_pan', 'joint_head_tilt']
-            joint_state.name.extend(head_joint_names)
-
-            positions.append(head_pan_rad)
-            velocities.append(head_pan_vel)
-            efforts.append(head_pan_effort)
-
-            positions.append(head_tilt_rad)
-            velocities.append(head_tilt_vel)
-            efforts.append(head_tilt_effort)
-
-        if self.use_robotis_end_of_arm: 
-            end_of_arm_joint_names = ['joint_wrist_yaw', 'joint_gripper_finger_left', 'joint_gripper_finger_right']
-            joint_state.name.extend(end_of_arm_joint_names)
-
-            positions.append(wrist_rad)
-            velocities.append(wrist_vel)
-            efforts.append(wrist_effort)
-
-            positions.append(gripper_finger_rad)
-            velocities.append(gripper_finger_vel)
-            efforts.append(gripper_finger_effort)
-
-            positions.append(gripper_finger_rad)
-            velocities.append(gripper_finger_vel)
-            efforts.append(gripper_finger_effort)
-
-        # set virtual joint for mobile base translation
-        joint_state.name.append('joint_mobile_base_translation')
-        if self.robot_mode == 'manipulation':
-            manipulation_translation = x_raw - self.mobile_base_manipulation_origin['x']
-            positions.append(manipulation_translation)
-            velocities.append(x_vel_raw)
-            efforts.append(x_effort_raw)
-        else:
-            positions.append(0.0)
-            velocities.append(0.0)
-            efforts.append(0.0)
-
-        # set joint_state
-        joint_state.position = positions
-        joint_state.velocity = velocities
-        joint_state.effort = efforts
+        for cg in self.joint_trajectory_action.command_groups:
+            pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode,
+                               manipulation_origin=self.mobile_base_manipulation_origin)
+            joint_state.name.append(cg.name)
+            joint_state.position.append(pos)
+            joint_state.velocity.append(vel)
+            joint_state.effort.append(effort)
+
+        # add telescoping joints to joint state
+        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
+        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)
+
         self.joint_state_pub.publish(joint_state)
 
         ##################################################
@@ -323,7 +179,6 @@ class StretchBodyNode:
         i.angular_velocity.x = gx
         i.angular_velocity.y = gy
         i.angular_velocity.z = gz
-
         i.linear_acceleration.x = ax
         i.linear_acceleration.y = ay
         i.linear_acceleration.z = az
@@ -508,47 +363,40 @@ class StretchBodyNode:
         if self.broadcast_odom_tf:
             self.tf_broadcaster = tf2_ros.TransformBroadcaster()
 
-        large_ang = 45.0 * np.pi/180.0
-
+        large_ang = np.radians(45.0)
         filename = rospy.get_param('~controller_calibration_file')
         rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename))
         with open(filename, 'r') as fid:
-            controller_parameters = yaml.safe_load(fid)
-
-            rospy.loginfo('controller parameters loaded = {0}'.format(controller_parameters))
-
-            self.head_tilt_calibrated_offset_rad = controller_parameters['tilt_angle_offset']
-            ang = self.head_tilt_calibrated_offset_rad
-            if (abs(ang) > large_ang):
-                rospy.loginfo('WARNING: self.head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
-            rospy.loginfo('self.head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_offset_rad)))
-
-            self.head_pan_calibrated_offset_rad = controller_parameters['pan_angle_offset']
-            ang = self.head_pan_calibrated_offset_rad
-            if (abs(ang) > large_ang):
-                rospy.loginfo('WARNING: self.head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
-            rospy.loginfo('self.head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_offset_rad)))
-
-            self.head_pan_calibrated_looked_left_offset_rad = controller_parameters['pan_looked_left_offset']
-            ang = self.head_pan_calibrated_looked_left_offset_rad
-            if (abs(ang) > large_ang):
-                rospy.loginfo('WARNING: self.head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
-            rospy.loginfo('self.head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_looked_left_offset_rad)))
-
-            self.head_tilt_backlash_transition_angle_rad = controller_parameters['tilt_angle_backlash_transition']
-            rospy.loginfo('self.head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(self.head_tilt_backlash_transition_angle_rad)))
-
-            self.head_tilt_calibrated_looking_up_offset_rad = controller_parameters['tilt_looking_up_offset']
-            ang = self.head_tilt_calibrated_looking_up_offset_rad
-            if (abs(ang) > large_ang):
-                rospy.loginfo('WARNING: self.head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
-            rospy.loginfo('self.head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_looking_up_offset_rad)))
-
-            self.wrist_extension_calibrated_retracted_offset_m = controller_parameters['arm_retracted_offset']
-            m = self.wrist_extension_calibrated_retracted_offset_m
-            if (abs(m) > 0.05):
-                rospy.loginfo('WARNING: self.wrist_extension_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE')
-            rospy.loginfo('self.wrist_extension_calibrated_retracted_offset_m in meters = {0}'.format(self.wrist_extension_calibrated_retracted_offset_m))
+            self.controller_parameters = yaml.safe_load(fid)
+            rospy.loginfo('controller parameters loaded = {0}'.format(self.controller_parameters))
+
+            head_tilt_calibrated_offset_rad = self.controller_parameters['tilt_angle_offset']
+            if (abs(head_tilt_calibrated_offset_rad) > large_ang):
+                rospy.logwarn('WARNING: head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
+            rospy.loginfo('head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(head_tilt_calibrated_offset_rad)))
+
+            head_pan_calibrated_offset_rad = self.controller_parameters['pan_angle_offset']
+            if (abs(head_pan_calibrated_offset_rad) > large_ang):
+                rospy.logwarn('WARNING: head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
+            rospy.loginfo('head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(head_pan_calibrated_offset_rad)))
+
+            head_pan_calibrated_looked_left_offset_rad = self.controller_parameters['pan_looked_left_offset']
+            if (abs(head_pan_calibrated_looked_left_offset_rad) > large_ang):
+                rospy.logwarn('WARNING: head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
+            rospy.loginfo('head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(head_pan_calibrated_looked_left_offset_rad)))
+
+            head_tilt_backlash_transition_angle_rad = self.controller_parameters['tilt_angle_backlash_transition']
+            rospy.loginfo('head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(head_tilt_backlash_transition_angle_rad)))
+
+            head_tilt_calibrated_looking_up_offset_rad = self.controller_parameters['tilt_looking_up_offset']
+            if (abs(head_tilt_calibrated_looking_up_offset_rad) > large_ang):
+                rospy.logwarn('WARNING: head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
+            rospy.loginfo('head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(head_tilt_calibrated_looking_up_offset_rad)))
+
+            arm_calibrated_retracted_offset_m = self.controller_parameters['arm_retracted_offset']
+            if (abs(arm_calibrated_retracted_offset_m) > 0.05):
+                rospy.logwarn('WARNING: arm_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE')
+            rospy.loginfo('arm_calibrated_retracted_offset_m in meters = {0}'.format(arm_calibrated_retracted_offset_m))
 
         self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103)
         self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)