Feature/concatenate trajs

src/prpy/util.py

@@ -2129,3 +2129,61 @@ 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()
+
+    smoothFlag = True
+    constrainedFlag = False
+    deterministicTrajectoryFlag = True
+    deterministicEndPointFlag = False
+
+    offset = base_traj.GetNumWaypoints()
+    for i in xrange(1, len(traj_list)):
+        traj_i = traj_list[i]
+
+        cspec_i = traj_i.GetConfigurationSpecification()
+        if base_cspec != cspec_i:
+            raise ValueError('The configuration specifications of the trajectory do not match')
+
+        tags_i = GetTrajectoryTags(traj_i)
+        # Only True if all trajectories have this set
+        if 'smooth' in tags_i and not tags_i['smooth']:
+            smoothFlag = False
+
+        # True if any trajectory has this set
+        if 'constrained' in tags_i and tags_i['constrained']:
+            constrainedFlag = True
+
+         # Only True if all trajectories have this set
+        if 'deterministic' in tags_i and not tags_i['deterministic']:
+            deterministcTrajectoryFlag = False
+
+        # True if the last trajectory has this set
+        if i == len(traj_list)-1 and 'deterministic_endpoint' in tags_i and tags_i['determinstic_endpoint']:
+             deterministicEndPointFlag = 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
+
+    flagSettings = {TAGS.SMOOTH: smoothFlag, TAGS.CONSTRAINED: constrainedFlag, 
+                    TAGS.DETERMINISTIC_TRAJECTORY: deterministicTrajectoryFlag,
+                    TAGS.DETERMINISTIC_ENDPOINT: deterministicEndPointFlag}
+    SetTrajectoryTags(base_traj, flagSettings, append=True)
+    return base_traj