Merge remote-tracking branch 'origin/release-1.1.1'

Author: IanTheEngineer

CHANGELOG.rst

@@ -1,3 +1,7 @@
+1.1.1 (2015-5-15)
+---------------------------------
+-  Fixed a bug that caused the JTAS to error with a path of one or two points is supplied as a trajectory
+
 1.1.0 (2015-1-2)
 ---------------------------------
 - Updates baxter_interface to ROS Indigo

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>baxter_interface</name>
-  <version>1.1.0</version>
+  <version>1.1.1</version>
   <description>
     Convenient python interface classes for control
     of the Baxter Research Robot from Rethink Robotics.

src/baxter_interface/settings.py

@@ -29,11 +29,11 @@
 HEAD_PAN_ANGLE_TOLERANCE = 0.1396263401
 
 ## Versioning
-SDK_VERSION = '1.1.0'
+SDK_VERSION = '1.1.1'
 CHECK_VERSION = True
 # Version Compatibility Maps - {current: compatible}
-VERSIONS_SDK2ROBOT = {'1.1.0': ['1.1.0']}
-VERSIONS_SDK2GRIPPER = {'1.1.0':
+VERSIONS_SDK2ROBOT = {'1.1.1': ['1.1.1', '1.1.0']}
+VERSIONS_SDK2GRIPPER = {'1.1.1':
                           {
                            'warn': '2014/5/20 00:00:00',  # Version 1.0.0
                            'fail': '2013/10/15 00:00:00', # Version 0.6.2

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

@@ -354,9 +354,24 @@ def _on_trajectory_action(self, goal):
         control_rate = rospy.Rate(self._control_rate)
 
         dimensions_dict = self._determine_dimensions(trajectory_points)
-	# Force Velocites/Accelerations to zero at the final timestep 
+
+        if num_points == 1:
+            # Add current position as trajectory point
+            first_trajectory_point = JointTrajectoryPoint()
+            first_trajectory_point.positions = self._get_current_position(joint_names)
+            # To preserve desired velocities and accelerations, copy them to the first
+            # trajectory point if the trajectory is only 1 point.
+            if dimensions_dict['velocities']:
+                first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities)
+            if dimensions_dict['accelerations']:
+                first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations)
+            first_trajectory_point.time_from_start = rospy.Duration(0)
+            trajectory_points.insert(0, first_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,
+        # Remove this behavior if you are stringing together trajectories,
         # and want continuous, non-zero velocities/accelerations between
         # trajectories
         if dimensions_dict['velocities']:
@@ -366,9 +381,18 @@ 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]
-        b_matrix = self._compute_bezier_coeff(joint_names,
-                                              trajectory_points,
-                                              dimensions_dict)
+        try:
+            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(
+                                                  self._action_name,
+                                                  self._name,
+                                                  type(ex).__name__, ex))
+            self._server.set_aborted()
+            return
         # Wait for the specified execution time, if not provided use now
         start_time = goal.trajectory.header.stamp.to_sec()
         if start_time == 0.0: