Simplify joint state calculation by using command groups

hello_helpers/src/hello_helpers/simple_command_group.py

@@ -158,3 +158,18 @@ class SimpleCommandGroup:
             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

@@ -29,6 +29,12 @@ class HeadPanCommandGroup(SimpleCommandGroup):
 
         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, calibrated_offset_rad=0.0, calibrated_looking_up_offset_rad=0.0, backlash_transition_angle_rad=-0.4):
@@ -54,6 +60,12 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
 
         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):
@@ -74,10 +86,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]),
@@ -154,6 +170,15 @@ 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, calibrated_retracted_offset_m=0.0):
@@ -265,6 +290,12 @@ class TelescopingCommandGroup(SimpleCommandGroup):
 
         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):
@@ -291,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)):

stretch_core/nodes/joint_trajectory_server.py

@@ -53,6 +53,8 @@ class JointTrajectoryAction:
                                                       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.head_tilt_cg, self.wrist_yaw_cg, self.gripper_cg]
 
     def execute_cb(self, goal):
         with self.node.robot_stop_lock:
@@ -67,11 +69,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
@@ -79,7 +79,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)
@@ -100,13 +100,8 @@ class JointTrajectoryAction:
             rospy.logdebug(("{0} joint_traj action: "
                             "target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
 
-            #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]
-
             valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal)
-                           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
@@ -115,11 +110,11 @@ class JointTrajectoryAction:
                 return
 
             robot_status = self.node.robot.get_status() # uses lock held by robot
-            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()
 
@@ -144,7 +139,7 @@ class JointTrajectoryAction:
 
                 robot_status = self.node.robot.get_status()
                 named_errors = [c.update_execution(robot_status, success_callback=self.success_callback)
-                                for c in command_groups]
+                                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.
@@ -153,7 +148,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

@@ -26,13 +26,9 @@ from nav_msgs.msg import Odometry
 from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField
 from std_msgs.msg import Bool, Header, String
 
-import hello_helpers.hello_misc as hm
-from hello_helpers.gripper_conversion import GripperConversion
 from joint_trajectory_server import JointTrajectoryAction
 from stretch_diagnostics import StretchDiagnostics
 
-GRIPPER_DEBUG = False
-
 
 class StretchBodyNode:
 
@@ -43,8 +39,6 @@ class StretchBodyNode:
         self.default_goal_timeout_s = 10.0
         self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s)
 
-        self.gripper_conversion = GripperConversion()
-
         self.robot_stop_lock = threading.Lock()
         self.stop_the_robot = False
 
@@ -67,97 +61,37 @@ class StretchBodyNode:
     def command_mobile_base_velocity_and_publish_state(self):
         self.robot_mode_rwlock.acquire_read()
 
-        # 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']
 
-        # assign relevant arm status to variables
-        arm_status = robot_status['arm']
-        arm_backlash_correction = 0.0
-        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']
-            pan_backlash_correction = 0.0
-            head_pan_calibrated_offset_rad = 0.0
-            head_pan_rad = head_pan_status['pos'] + 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']
-            tilt_backlash_correction = 0.0
-            head_tilt_calibrated_offset_rad = 0.0
-            head_tilt_rad = head_tilt_status['pos'] + 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
@@ -172,7 +106,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
@@ -209,68 +143,36 @@ class StretchBodyNode:
         mode_msg.data = self.robot_mode
         self.mode_pub.publish(mode_msg)
 
-        # 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 range(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 range(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 range(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 joint_state
-        joint_state.position = positions
-        joint_state.velocity = velocities
-        joint_state.effort = efforts
+        cgs = list(set(self.joint_trajectory_action.command_groups) - set([self.joint_trajectory_action.mobile_base_cg]))
+        for cg in cgs:
+            pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode)
+            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
+        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))
+            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)
 
         ##################################################
@@ -292,7 +194,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