track full trajectories for every lcp frame

least_changed_path/least_changed_path.py

@@ -5,6 +5,7 @@ class LeastChangedPath(object):
     def __init__(self, steps=None):
         self.steps = steps
         self._result = paths.Trajectory()
+        self.frame_to_full_traj = {}
         self.n = self.find_first_n(steps)
         self.make_results()
 
@@ -50,6 +51,8 @@ class LeastChangedPath(object):
     def add_sub_trajectory(self, steps=None, old_step=None, new_step=None):
         if new_step is None:
             sub_traj = paths.Trajectory([self.sp(old_step)])
+            first_traj = self.steps[0].active[0].trajectory
+            self.frame_to_full_traj[sub_traj[0]] = first_traj
         else:
             full_traj = new_step.active[0].trajectory
             new_sp = self.sp(new_step)
@@ -63,7 +66,8 @@ class LeastChangedPath(object):
                 sub_traj = full_traj[i_old_sp:i_new_sp:-1]
             else:
                 raise ValueError("shooting point indexes are equal")
-
+            for frame in sub_traj:
+                self.frame_to_full_traj[frame] = full_traj
         self._result += sub_traj
 
     @property