add reconstruction to example position

.gitignore

@@ -16,3 +16,5 @@ __pycache__/
 casadi_codegen.obj
 invariants_py/data/sine_wave_invariants.csv
 venv/
+debug_fatrop_*
+nlp_hess_l.casadi

examples/calculate_invariants_position.py

@@ -13,11 +13,18 @@
 trajectory, time = dh.read_pose_trajectory_from_data(path_data,dtype = 'txt')
 
 # Calculate the invariants of the translation trajectory
-invariants, progress, calc_trajectory, movingframes = invariants_handler.calculate_invariants_translation(trajectory)
+invariants, progress, calc_trajectory, movingframes, progress_n = invariants_handler.calculate_invariants_translation(trajectory)
+
+# (Optional) Reconstruction of the trajectory from the invariants
+reconstructed_trajectory, recon_mf = invariants_handler.reconstruct_trajectory(invariants, progress_n, p_init=calc_trajectory[0,:], mf_init=movingframes[0,:,:])
+
+print(recon_mf[1,:,:])
+print(movingframes[1,:,:])
 
 # Plotting the results
 plotters.plot_invariants_new2(invariants, progress) # calculated invariants
 plotters.plot_trajectory(calc_trajectory) # calculated trajectory corresponding to invariants
+plotters.plot_trajectory(reconstructed_trajectory) # reconstructed trajectory corresponding to invariants
 plotters.plot_moving_frames(calc_trajectory, movingframes) # calculated moving frames along trajectory
 plotters.animate_moving_frames(calc_trajectory, movingframes) # animated moving frames along trajectory
 

invariants_py/invariants_handler.py

@@ -3,14 +3,18 @@
 """
 
 import invariants_py.reparameterization as reparam
+import invariants_py.dynamics_vector_invariants as dyn
 import invariants_py.calculate_invariants.opti_calculate_vector_invariants_position_mf as FS_calculation
+import numpy as np
 
 def calculate_invariants_translation(trajectory, progress_definition="arclength"):
 
     # different progress definition: {time, default: arclength, arcangle, screwbased}
     # Reparameterize the trajectory based on arclength
     trajectory_geom, arclength, arclength_n, nb_samples, stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 
+    print(stepsize)
+
     # Create an instance of the OCP_calc_pos class
     FS_calculation_problem = FS_calculation.OCP_calc_pos(window_len=nb_samples, geometric=True)
 
@@ -20,5 +24,31 @@ def calculate_invariants_translation(trajectory, progress_definition="arclength"
     invariants = result[0]
     trajectory = result[1]
     movingframes = result[2]
-    return invariants, arclength, trajectory, movingframes
+    return invariants, arclength, trajectory, movingframes, arclength_n
+
+
+def reconstruct_trajectory(invariants, progress, p_init=np.zeros((3,1)), mf_init=np.eye(3)):
+
+    N = len(progress)
+
+    positions = np.zeros((N,3))
+    R_frames = np.zeros((N,3,3))
+
+    stepsize = 1/N
+    print(stepsize)
+
+    positions[0,:] = p_init
+    R_frames[0,:,:] = mf_init
 
+    # Use integrator to find the other initial states
+    for k in range(N-1):
+
+        [R_plus1,p_plus1] = dyn.integrate_vector_invariants_position(mf_init, p_init, invariants[k,:], stepsize)
+
+        positions[k+1,:] = np.array(p_plus1).T
+        R_frames[k+1,:,:] = np.array(R_plus1)     
+
+        p_init = p_plus1
+        mf_init = R_plus1
+
+    return positions, R_frames

pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "invariants-py"
-version = "0.3.7" # note that it is possible for the build backend to manage versions in a dynamic way
+version = "0.3.8" # note that it is possible for the build backend to manage versions in a dynamic way
 description = "Calculate invariant trajectory representations from trajectory data and generate new trajectories from invariant representations"
 authors = [ {name = "Maxim Vochten"}, 
     {name = "Riccardo Burlizzi"},