diff --git a/src/joint_trajectory_action/bezier.py b/src/joint_trajectory_action/bezier.py
index d11830e..37c2026 100644
--- a/src/joint_trajectory_action/bezier.py
+++ b/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, :]
diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py
index d010e38..1a7c26f 100644
--- a/src/joint_trajectory_action/joint_trajectory_action.py
+++ b/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,