add minjerk interpolation method

scripts/joint_trajectory_action_server.py

@@ -52,7 +52,7 @@
 )
 
 
-def start_server(limb, rate, mode):
+def start_server(limb, rate, mode, interpolation):
     print("Initializing node... ")
     rospy.init_node("rsdk_%s_joint_trajectory_action_server%s" %
                     (mode, "" if limb == 'both' else "_" + limb,))
@@ -70,11 +70,11 @@ def start_server(limb, rate, mode):
     jtas = []
     if limb == 'both':
         jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
-                                                rate, mode))
+                                                rate, mode, interpolation))
         jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
-                                                rate, mode))
+                                                rate, mode, interpolation))
     else:
-        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
+        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode, interpolation))
 
     def cleanup():
         for j in jtas:
@@ -102,8 +102,13 @@ def main():
         choices=['position_w_id', 'position', 'velocity'],
         help="control mode for trajectory execution"
     )
+    parser.add_argument(
+        "-i", "--interpolation", default='bezier',
+        choices=['bezier', 'minjerk'],
+        help="interpolation method for trajectory generation"
+    )
     args = parser.parse_args(rospy.myargv()[1:])
-    start_server(args.limb, args.rate, args.mode)
+    start_server(args.limb, args.rate, args.mode, args.interpolation)
 
 
 if __name__ == "__main__":

src/joint_trajectory_action/joint_trajectory_action.py

@@ -35,6 +35,7 @@
 import numpy as np
 
 import bezier
+import minjerk
 
 import rospy
 
@@ -59,7 +60,7 @@
 
 class JointTrajectoryActionServer(object):
     def __init__(self, limb, reconfig_server, rate=100.0,
-                 mode='position_w_id'):
+                 mode='position_w_id', interpolation='bezier'):
         self._dyn = reconfig_server
         self._ns = 'robot/limb/' + limb
         self._fjt_ns = self._ns + '/follow_joint_trajectory'
@@ -72,6 +73,7 @@ def __init__(self, limb, reconfig_server, rate=100.0,
         self._limb = baxter_interface.Limb(limb)
         self._enable = baxter_interface.RobotEnable()
         self._name = limb
+        self._interpolation = interpolation
         self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb,))
         self._cuff.state_changed.connect(self._cuff_cb)
         # Verify joint control mode
@@ -332,6 +334,47 @@ def _compute_bezier_coeff(self, joint_names, trajectory_points, dimensions_dict)
             b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts)
         return b_matrix
 
+    def _get_minjerk_point(self, m_matrix, idx, t, cmd_time, dimensions_dict):
+        pnt = JointTrajectoryPoint()
+        pnt.time_from_start = rospy.Duration(cmd_time)
+        num_joints = m_matrix.shape[0]
+        pnt.positions = [0.0] * num_joints
+        if dimensions_dict['velocities']:
+            pnt.velocities = [0.0] * num_joints
+        if dimensions_dict['accelerations']:
+            pnt.accelerations = [0.0] * num_joints
+        for jnt in range(num_joints):
+            m_point = minjerk.minjerk_point(m_matrix[jnt, :, :, :], idx, t)
+            # Positions at specified time
+            pnt.positions[jnt] = m_point[0]
+            # Velocities at specified time
+            if dimensions_dict['velocities']:
+                pnt.velocities[jnt] = m_point[1]
+            # Accelerations at specified time
+            if dimensions_dict['accelerations']:
+                pnt.accelerations[jnt] = m_point[-1]
+        return pnt
+
+    def _compute_minjerk_coeff(self, joint_names, trajectory_points, point_duration, dimensions_dict):
+        # Compute Full Minimum Jerk Curve
+        num_joints = len(joint_names)
+        num_traj_pts = len(trajectory_points)
+        num_traj_dim = sum(dimensions_dict.values())
+        num_m_values = len(['a0', 'a1', 'a2', 'a3', 'a4', 'a5', 'tm'])
+        m_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_m_values))
+        for jnt in xrange(num_joints):
+            traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
+            for idx, point in enumerate(trajectory_points):
+                current_point = list()
+                current_point.append(point.positions[jnt])
+                if dimensions_dict['velocities']:
+                    current_point.append(point.velocities[jnt])
+                if dimensions_dict['accelerations']:
+                    current_point.append(point.accelerations[jnt])
+                traj_array[idx, :] = current_point
+            m_matrix[jnt, :, :, :] = minjerk.minjerk_coefficients(traj_array, point_duration)
+        return m_matrix
+
     def _determine_dimensions(self, trajectory_points):
         # Determine dimensions supplied
         position_flag = True
@@ -388,9 +431,16 @@ def _on_trajectory_action(self, goal):
         # Compute Full Bezier Curve Coefficients for all 7 joints
         pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
         try:
-            b_matrix = self._compute_bezier_coeff(joint_names,
-                                                  trajectory_points,
-                                                  dimensions_dict)
+            if self._interpolation == 'minjerk':
+                point_duration = [pnt_times[i+1] - pnt_times[i] for i in range(len(pnt_times)-1)]
+                m_matrix = self._compute_minjerk_coeff(joint_names,
+                                                       trajectory_points,
+                                                       point_duration,
+                                                       dimensions_dict)
+            else:
+                b_matrix = self._compute_bezier_coeff(joint_names,
+                                                      trajectory_points,
+                                                      dimensions_dict)
         except Exception as ex:
             rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
                          " arm with error \"{2}: {3}\"").format(
@@ -429,9 +479,14 @@ def _on_trajectory_action(self, goal):
                 cmd_time = 0
                 t = 0
 
-	    point = self._get_bezier_point(b_matrix, idx,
-                                           t, cmd_time,
-				           dimensions_dict)
+            if self._interpolation == 'minjerk':
+                point = self._get_minjerk_point(m_matrix, idx,
+                                                t, cmd_time,
+                                                dimensions_dict)
+            else:
+                point = self._get_bezier_point(b_matrix, idx,
+                                               t, cmd_time,
+                                               dimensions_dict)
 
             # Command Joint Position, Velocity, Acceleration
             command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)