Feature/concatenate trajs

src/prpy/util.py

@@ -2129,3 +2129,50 @@ def GetPointFrom(focus):
         raise ValueError('Focus of the point is an unknown object')
 
     return coord
+
+def concatenateTrajectories(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')
+
+    base_traj = traj_list[0]
+    base_cspec = base_traj.GetConfigurationSpecification()
+    base_robot = base_cspec.ExtractUsedBodies(base_traj.GetEnv())[0]
+    traj_indices = GetTrajectoryIndices(base_traj)
+
+    offset = base_traj.GetNumWaypoints()
+    for i in xrange(1, len(traj_list)):
+        traj_i = traj_list[i]
+        cspec_i = traj_i.GetConfigurationSpecification()
+        robot_i = cspec_i.ExtractUsedBodies(traj_i.GetEnv())[0]
+        if robot_i != base_robot:
+            raise ValueError('Trajectories are not on the same robot')
+
+        sub = numpy.subtract(GetTrajectoryIndices(traj_i), traj_indices)
+        if sub.sum() != 0:
+            raise ValueError('Trajectories are not on the same manipulator.')
+
+        tags_i = GetTrajectoryTags(traj_i)
+        # If one segment is not smooth, the entire trajectory is not (?)
+        if 'smooth' in tags_i and not tags_i['smooth']:
+            SetTrajectoryTags(base_traj, {TAGS.SMOOTH: True}, append=True)
+
+        # The entire trajectory gets tagged as constrained if one of 
+        # the segments is constrained.
+        if 'constrained' in tags_i and tags_i['constrained']:
+            SetTrajectoryTags(base_traj, {TAGS.CONSTRAINED: True}, append=True)
+
+        # Add trajectory in by inserting each waypoint
+        for j in xrange(traj_i.GetNumWaypoints()):
+            waypoint = traj_i.GetWaypoint(j)
+            base_traj.Insert(offset, waypoint)
+            offset += 1
+    return base_traj