Updates Limb.py for Joint Trajectory Mode

Updates the Limb interface to utilize the
new TRAJECTORY_MODE in intera_core_msgs. This
new mode is a reworked RAW_POSITION_MODE but
requires Position, Velocity, and Acceleration
commands to execute a Trajectory.

The other modes (Position, Velocity, Torque)
now require commands to be populated in their
respective fields within the JointCommand msg.

Additionally, slight optimization in the JTAS
to execute commands strictly in the mode commanded.

Depends on
RethinkRobotics/intera_common#7

Author: IanTheEngineer

intera_interface/src/intera_interface/limb.py

@@ -336,7 +336,31 @@ def set_joint_position_speed(self, speed):
         """
         self._pub_speed_ratio.publish(Float64(speed))
 
-    def set_joint_positions(self, positions, raw=False):
+    def set_joint_trajectory(self, names, positions, velocities, accelerations):
+        """
+        Commands the joints of this limb to the specified positions.
+
+        B{IMPORTANT:} 'raw' joint position control mode allows for commanding
+        joint positions, without modification, directly to the JCBs
+        (Joint Controller Boards). While this results in more unaffected
+        motions, 'raw' joint position control mode bypasses the safety system
+        modifications (e.g. collision avoidance).
+        Please use with caution.
+
+        @type positions: dict({str:float})
+        @param positions: joint_name:angle command
+        @type raw: bool
+        @param raw: advanced, direct position control mode
+        """
+        self._command_msg.names = names
+        self._command_msg.position = positions
+        self._command_msg.velocity = velocities
+        self._command_msg.acceleration = accelerations
+        self._command_msg.mode = JointCommand.TRAJECTORY_MODE
+        self._command_msg.header.stamp = rospy.Time.now()
+        self._pub_joint_cmd.publish(self._command_msg)
+
+    def set_joint_positions(self, positions):
         """
         Commands the joints of this limb to the specified positions.
 
@@ -353,11 +377,9 @@ def set_joint_positions(self, positions, raw=False):
         @param raw: advanced, direct position control mode
         """
         self._command_msg.names = positions.keys()
-        self._command_msg.command = positions.values()
-        if raw:
-            self._command_msg.mode = JointCommand.RAW_POSITION_MODE
-        else:
-            self._command_msg.mode = JointCommand.POSITION_MODE
+        self._command_msg.position = positions.values()
+        self._command_msg.mode = JointCommand.POSITION_MODE
+        self._command_msg.header.stamp = rospy.Time.now()
         self._pub_joint_cmd.publish(self._command_msg)
 
     def set_joint_velocities(self, velocities):
@@ -373,8 +395,9 @@ def set_joint_velocities(self, velocities):
         @param velocities: joint_name:velocity command
         """
         self._command_msg.names = velocities.keys()
-        self._command_msg.command = velocities.values()
+        self._command_msg.velocity = velocities.values()
         self._command_msg.mode = JointCommand.VELOCITY_MODE
+        self._command_msg.header.stamp = rospy.Time.now()
         self._pub_joint_cmd.publish(self._command_msg)
 
     def set_joint_torques(self, torques):
@@ -390,8 +413,9 @@ def set_joint_torques(self, torques):
         @param torques: joint_name:torque command
         """
         self._command_msg.names = torques.keys()
-        self._command_msg.command = torques.values()
+        self._command_msg.effort = torques.values()
         self._command_msg.mode = JointCommand.TORQUE_MODE
+        self._command_msg.header.stamp = rospy.Time.now()
         self._pub_joint_cmd.publish(self._command_msg)
 
     def move_to_neutral(self, timeout=15.0):

intera_interface/src/joint_trajectory_action/joint_trajectory_action.py

@@ -237,22 +237,25 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
                     break
                 rospy.sleep(1.0 / self._control_rate)
         elif self._mode == 'position' or self._mode == 'position_w_id':
-            raw_pos_mode = (self._mode == 'position_w_id')
-            if raw_pos_mode:
-                pnt = JointTrajectoryPoint()
-                pnt.positions = self._get_current_position(joint_names)
+            pnt = JointTrajectoryPoint()
+            pnt.positions = self._get_current_position(joint_names)
+            if self._mode == 'position_w_id':
                 if dimensions_dict['velocities']:
                     pnt.velocities = [0.0] * len(joint_names)
                 if dimensions_dict['accelerations']:
                     pnt.accelerations = [0.0] * len(joint_names)
+            ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
             while (not self._server.is_new_goal_available() and self._alive
                    and self.robot_is_enabled()):
-                self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
                 # zero inverse dynamics feedforward command
-                if self._mode == 'position_w_id':
-                    pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
-                    ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
-                    self._pub_ff_cmd.publish(ff_pnt)
+                if self._mode == 'position' and self._alive:
+                    self._limb.set_joint_positions(dict(zip(joint_names, point.positions)))
+                if self._mode == 'position_w_id' and self._alive:
+                    self._limb.set_joint_trajectory(joint_names,
+                                            ff_pnt.positions,
+                                            ff_pnt.velocities,
+                                            ff_pnt.accelerations)
+
                 if self._cuff_state:
                     self._limb.exit_control_mode()
                     break
@@ -277,17 +280,16 @@ def _command_joints(self, joint_names, point, start_time, dimensions_dict):
                 return False
             if self._mode == 'velocity':
                 velocities.append(self._pid[delta[0]].compute_output(delta[1]))
-        if ((self._mode == 'position' or self._mode == 'position_w_id')
-              and self._alive):
-            cmd = dict(zip(joint_names, point.positions))
-            raw_pos_mode = (self._mode == 'position_w_id')
-            self._limb.set_joint_positions(cmd, raw=raw_pos_mode)
-            if raw_pos_mode:
-                ff_pnt = self._reorder_joints_ff_cmd(joint_names, point)
-                self._pub_ff_cmd.publish(ff_pnt)
+
+        if self._mode == 'position' and self._alive:
+            self._limb.set_joint_positions(dict(zip(joint_names, point.positions)))
+        if self._mode == 'position_w_id' and self._alive:
+            self._limb.set_joint_trajectory(joint_names,
+                                            point.positions,
+                                            point.velocities,
+                                            point.accelerations)
         elif self._alive:
-            cmd = dict(zip(joint_names, velocities))
-            self._limb.set_joint_velocities(cmd)
+            self._limb.set_joint_velocities(dict(zip(joint_names, velocities)))
         return True
 
     def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):