Merge branch 'main' of https://gitlab.kuleuven.be/robotgenskill/publi…

…c_code/invariants_py

.gitignore

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

README.md

@@ -3,6 +3,10 @@
 ![GitHub code size in bytes](https://img.shields.io/github/languages/code-size/trajectory-invariants/invariants_py)
 ![GitHub issues](https://img.shields.io/github/issues/trajectory-invariants/invariants_py)
 
+<p align="center">
+  <img src="https://trajectory-invariants.github.io/images/logo-trajectory-invariants.png" alt="Logo Trajectory Invariants" title="Trajectory Invariants" width="750">
+</p>
+
 `invariants-py` is a Python library to robustly calculate coordinate-invariant trajectory representations using geometric optimal control. 
 It also supports trajectory generation under user-specified trajectory constraints starting from invariant representations.
 

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, recon_vel = invariants_handler.reconstruct_trajectory(invariants, position_init=calc_trajectory[0,:], movingframe_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/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py

@@ -215,7 +215,7 @@ def calculate_invariants(self, measured_positions, stepsize, use_previous_soluti
             else:  
 
                 # Initialize states and controls using measurements
-                self.values_variables = initialization.initialize_VI_pos2(measured_positions)
+                self.values_variables = initialization.initialize_VI_pos2(measured_positions,stepsize)
                 self.first_time = False
 
         # Solve the optimization problem for the given measurements starting from previous solution

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,46 @@ 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, position_init=np.zeros((3,1)), movingframe_init=np.eye(3)):
+    """
+    Reconstructs a position trajectory from its invariant representation starting from an initial position and moving frame.
+    
+    The initial position is the starting position of the trajectory. The initial moving frame encodes the starting direction (tangent and normal) of the trajectory.
+    
+    Parameters:
+    - invariants (numpy array of shape (N,3)): Array of vector invariants.
+    - position_init (numpy array of shape (3,1), optional): Initial position. Defaults to a 3x1 zero array.
+    - movingframe_init (numpy array of shape (3,3), optional): Initial frame matrix. Defaults to a 3x3 identity matrix.
+    
+    Returns:
+    - positions (numpy array of shape (N,3)): Array of reconstructed positions.
+    - R_frames (numpy array of shape (N,3,3)): Array of reconstructed moving frames.
+    """
+
+    N = np.size(invariants, 0)
+    stepsize = 1/N
+
+    positions = np.zeros((N,3))
+    velocities = np.zeros((N,3))
+    R_frames = np.zeros((N,3,3))
+
+    positions[0,:] = position_init
+    R_frames[0,:,:] = movingframe_init
+
+    # Use integrator to find the other initial states
+    for k in range(N-1):
+        [R_plus1, p_plus1] = dyn.integrate_vector_invariants_position(movingframe_init, position_init, invariants[k, :], stepsize)
+
+        positions[k+1,:] = np.array(p_plus1).T
+        R_frames[k+1,:,:] = np.array(R_plus1)
+
+        position_init = p_plus1
+        movingframe_init = R_plus1
+
+    for k in range(N):
+        velocities[k,:] = invariants[k, 0]*R_frames[k,:,0]
 
+    return positions, R_frames, velocities

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"},