Feature/concatenate trajs

src/prpy/util.py

@@ -2129,3 +2129,75 @@ def GetPointFrom(focus):
         raise ValueError('Focus of the point is an unknown object')
 
     return coord
+
+def ConcatenateTrajectories(robot, traj_list):
+    """
+    Given a list of trajectories for a single manipulator,
+    concatenate them into a single trajectory
+
+    @param traj_list List of consective trajectories for a given manipulator
+    @return base_traj Combined trajectory
+    """
+    from planning.base import Tags
+
+    if len(traj_list) == 0:
+        raise ValueError('Trajectory list is empty')
+
+    env = robot.GetEnv()
+
+    # Create an empty trajectory to populate
+    base_traj = openravepy.RaveCreateTrajectory(env, '')
+    base_cspec = robot.GetActiveConfigurationSpecification('linear')
+    base_traj.Init(base_cspec)
+    #base_cspec = base_traj.GetConfigurationSpecification()
+
+    smoothFlag = True
+    constrainedFlag = False
+    deterministicTrajectoryFlag = True
+    deterministicEndPointFlag = False
+
+    offset = 0
+    for traj in traj_list:
+        cspec_i = traj.GetConfigurationSpecification()
+        if base_cspec != cspec_i:
+            raise ValueError('The configuration specifications of the trajectory do not match')
+
+        tags = GetTrajectoryTags(traj)
+        # Only True if all trajectories have this set
+        if 'smooth' in tags and not tags['smooth']:
+            smoothFlag = False
+
+        # True if any trajectory has this set
+        if 'constrained' in tags and tags['constrained']:
+            constrainedFlag = True
+
+         # Only True if all trajectories have this set
+        if 'deterministic' in tags and not tags['deterministic']:
+            deterministcTrajectoryFlag = False
+
+        # Check that trajectory leds from previous,
+        # or if the first, start the trajectory
+        epsilon = 0.001
+        if offset == 0:
+            base_traj.Insert(offset, traj.GetWaypoint(0))
+            offset += 1
+        else:
+            if sum(numpy.subtract(traj.GetWaypoint(0), base_traj.GetWaypoint(offset-1))) > epsilon:
+                raise ValueError('The trajectories are not consecutive.')
+
+        # Add trajectory in by inserting each waypoint
+        for j in xrange(1, traj.GetNumWaypoints()):
+            waypoint = traj.GetWaypoint(j)
+            base_traj.Insert(offset, waypoint)
+            offset += 1
+
+    # True if the last trajectory has this set
+    last_tags = GetTrajectoryTags(traj_list[-1])
+    if 'deterministic_endpoint' in last_tags and last_tags['determinstic_endpoint']:
+        deterministicEndPointFlag = True
+
+    flagSettings = {Tags.SMOOTH: smoothFlag, Tags.CONSTRAINED: constrainedFlag, 
+                    Tags.DETERMINISTIC_TRAJECTORY: deterministicTrajectoryFlag,
+                    Tags.DETERMINISTIC_ENDPOINT: deterministicEndPointFlag}
+    SetTrajectoryTags(base_traj, flagSettings, append=True)
+    return base_traj

tests/test_util.py

@@ -1088,6 +1088,26 @@ def test_GetMinDistanceBetweenTransformAndWorkspaceTraj(self):
         numpy.testing.assert_array_almost_equal(T_loc, expected_T_loc, decimal=7, \
                                                 err_msg=error, verbose=True)
 
+    def test_ConcatenateTrajectories(self):
+        q0 = self.robot.GetDOFValues(self.active_dof_indices).tolist()
+        q1 = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0]
+        q2 = [0.50, 0.30, 0.02, 0.01, 0.01, 0.01, 0.0]
+        traj1 = self.CreateTrajectory(q0, q1)
+        traj2 = self.CreateTrajectory(q1, q2)
+        combined_traj = prpy.util.ConcatenateTrajectories(self.robot, [traj1, traj2])
+
+        combined_waypoints = numpy.zeros((combined_traj.GetNumWaypoints(), 7))
+        for j in xrange(combined_traj.GetNumWaypoints()):
+            combined_waypoints[j] = combined_traj.GetWaypoint(j)
+        all_waypoints = [q0, q1, q2]
+
+        error = 'The waypoints on the trajectory do not match the individual waypoints'
+        numpy.testing.assert_array_almost_equal(combined_waypoints,
+                                                all_waypoints,
+                                                decimal=4,
+                                                err_msg=error,
+                                                verbose=True)
+
 
 class Test_GetPointFrom(unittest.TestCase):
     """