Fixed Bezier lib to prevent errors on 2 points

This commit partially resolves baxter_interface issue (#53 and #54),
where the JTAS would error out if too few points were passed into it.

There was a bug in the Bezier library which did not properly check for
two points, which this resolves. We can safely remove the additional
two point check in the joint trajectory action server based on this.

Trac ticket #11669

src/joint_trajectory_action/bezier.py

@@ -135,19 +135,25 @@ def de_boor_control_pts(points_array, d0=None,
     # Construct de Boor Control Points from A matrix
     d_pts = np.zeros((N+3, k))
     for col in range(0, k):
-        x = np.zeros((N-1, 1))
-        # Compute start / end conditions
-        if natural:
-            x[N-2, 0] = 6*points_array[-2, col] - points_array[-1, col]
-            x[0, 0] = 6*points_array[1, col] - points_array[0, col]
-        else:
-            x[N-2, 0] = 6*points_array[-2, col] - 1.5*dN[0, col]
-            x[0, 0] = 6*points_array[1, col] - 1.5*d0[0, col]
-        x[range(1, N-3+1), 0] = 6*points_array[range(2, N-2+1), col]
-        # Solve bezier interpolation
+        x = np.zeros((max(N-1, 1), 1))
         if N > 2:
+            # Compute start / end conditions
+            if natural:
+                x[N-2, 0] = 6*points_array[-2, col] - points_array[-1, col]
+                x[0, 0] = 6*points_array[1, col] - points_array[0, col]
+            else:
+                x[N-2, 0] = 6*points_array[-2, col] - 1.5*dN[0, col]
+                x[0, 0] = 6*points_array[1, col] - 1.5*d0[0, col]
+            x[range(1, N-3+1), 0] = 6*points_array[range(2, N-2+1), col]
+            # Solve bezier interpolation
             d_pts[2:N+1, col] = np.linalg.solve(A, x).T
         else:
+            # Compute start / end conditions
+            if natural:
+                x[0, 0] = 6*points_array[1, col] - points_array[0, col]
+            else:
+                x[0, 0] = 6*points_array[1, col] - 1.5*d0[col]
+            # Solve bezier interpolation
             d_pts[2, col] = x / A
     # Store off start and end positions
     d_pts[0, :] = points_array[0, :]

src/joint_trajectory_action/joint_trajectory_action.py

@@ -367,18 +367,6 @@ def _on_trajectory_action(self, goal):
             trajectory_points.insert( 0, first_trajectory_point )
             num_points = len(trajectory_points)
 
-        if num_points == 2:
-            # Add midpoint of 2 trajectory points
-            mid_trajectory_point = JointTrajectoryPoint()
-            mid_trajectory_point.positions = [(p1 + p2)/2 for (p1, p2) in zip(trajectory_points[0].positions, trajectory_points[1].positions)]
-            if dimensions_dict['velocities']:
-                mid_trajectory_point.velocities = [(v1 + v2)/2 for (v1, v2) in zip(trajectory_points[0].velocities, trajectory_points[1].velocities)]
-            if dimensions_dict['accelerations']:
-                mid_trajectory_point.accelerations = [(a1 + a2)/2 for (a1, a2) in zip(trajectory_points[0].accelerations, trajectory_points[1].accelerations)]
-            mid_trajectory_point.time_from_start = (trajectory_points[0].time_from_start + trajectory_points[1].time_from_start)/2
-            trajectory_points.insert( 1, mid_trajectory_point )
-            num_points = len(trajectory_points)
-
 	# Force Velocites/Accelerations to zero at the final timestep 
         # if they exist in the trajectory
 	# Remove this behavior if you are stringing together trajectories,