Merge pull request #2 from trajectory-invariants/example_code_screw_i…

…nvariants Example code screw invariants
trajectory-invariants · Jul 5, 2024 · 396be5f · 396be5f
2 parents 35c5940 + 46090b5
commit 396be5f
Show file tree

Hide file tree

Showing 21 changed files with 845 additions and 103 deletions.
diff --git a/.gitignore b/.gitignore
@@ -14,4 +14,5 @@ generation_*
 codegen/
 __pycache__/
 casadi_codegen.obj
-invariants_py/data/sine_wave_invariants.csv
+invariants_py/data/sine_wave_invariants.csv
+venv/
diff --git a/examples/calculate_invariants_position.py b/examples/calculate_invariants_position.py
@@ -10,7 +10,7 @@
 
 # Load the trajectory data from the file
 # todo format of data
-trajectory, time = dh.read_pose_trajectory_from_txt(path_data)
+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)

diff --git a/examples/calculate_screw_invariants_pose.py b/examples/calculate_screw_invariants_pose.py
@@ -0,0 +1,29 @@
+# Calculate the invariants of a pose trajectory
+
+# Import necessary modules
+from invariants_py import data_handler as dh
+import invariants_py.plotting_functions.plotters as plotters
+from invariants_py import invariants_handler
+import numpy as np
+from invariants_py.reparameterization import interpT
+
+# Find the path to the data file
+path_data = dh.find_data_path("pouring_segmentation.csv") 
+
+# Load the trajectory data from the file
+T, timestamps = dh.read_pose_trajectory_from_data(path_data, scalar_last = False, dtype = 'csv')
+
+# Define resampling interval (50Hz)
+dt = 0.02  # [s]
+
+# Compute the number of new samples
+N = int(1 + np.floor(timestamps[-1] / dt))
+
+# Generate new equidistant time vector
+time_new = np.linspace(0, timestamps[-1], N)
+
+# Interpolate pose matrices to new time vector
+T = interpT(timestamps, T, time_new)
+
+
+
diff --git a/examples/fatrop-tests/trajectory_generation_fullpose_coupled_rockit.py b/examples/fatrop-tests/trajectory_generation_fullpose_coupled_rockit.py
@@ -24,7 +24,7 @@
 
 
 data_location = dh.find_data_path('beer_1.txt')
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 home_pos = [0,0,0] # Use this if not considering the robot

diff --git a/examples/fatrop-tests/trajectory_generation_fullpose_rockit.py b/examples/fatrop-tests/trajectory_generation_fullpose_rockit.py
@@ -29,7 +29,7 @@
 
 #%%
 data_location = dh.find_data_path('beer_1.txt')
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 trajectory_position = pose[:,:3,3]

diff --git a/examples/fatrop-tests/trajectory_generation_position_rockit.py b/examples/fatrop-tests/trajectory_generation_position_rockit.py
@@ -22,7 +22,7 @@
 
 # Read trajectory data
 data_location = dh.find_data_path('beer_1.txt')
-trajectory, time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory, time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 pose, time_profile, arclength, nb_samples, stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength / arclength[-1]
 trajectory = pose[:, 0:3, 3]

diff --git a/examples/fatrop-tests/trajectory_generation_rotation_rockit.py b/examples/fatrop-tests/trajectory_generation_rotation_rockit.py
@@ -24,7 +24,7 @@
 
 #%%
 data_location = dh.find_data_path('beer_1.txt')
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 trajectory_position = pose[:,:3,3]

diff --git a/...es/invariants_calculation/other/other_OCP_formulations/offline_calculation_movingframe.py b/...es/invariants_calculation/other/other_OCP_formulations/offline_calculation_movingframe.py
@@ -17,7 +17,7 @@
 """
 
 # load data
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 
 # reparameterization
 trajectory_geom,arclength,arclength_n,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)

diff --git a/examples/trajectory_generation/online_generation.py b/examples/trajectory_generation/online_generation.py
@@ -12,7 +12,7 @@
 """Input data"""
 
 data_location = dh.find_data_path("sine_wave.txt")
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location,dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 trajectory = pose[:,0:3,3]

diff --git a/examples/trajectory_generation/trajectory_generation_fullpose.py b/examples/trajectory_generation/trajectory_generation_fullpose.py
@@ -19,7 +19,7 @@
 data_location = dh.find_data_path('beer_1.txt')
 opener_location =  dh.find_data_path('opener.stl')
 
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location,dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 trajectory_position = pose[:,:3,3]

diff --git a/examples/trajectory_generation/trajectory_generation_position.py b/examples/trajectory_generation/trajectory_generation_position.py
@@ -11,7 +11,7 @@
 from IPython.display import clear_output
 
 data_location = dh.find_data_path('beer_1.txt')
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 trajectory = pose[:,0:3,3]

diff --git a/examples/trajectory_generation/trajectory_generation_rotation.py b/examples/trajectory_generation/trajectory_generation_rotation.py
@@ -17,7 +17,7 @@
 #%%
 data_location = dh.find_data_path('beer_1.txt')
 opener_location = dh.find_data_path('opener.stl')
-trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
 pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
 arclength_n = arclength/arclength[-1]
 trajectory_position = pose[:,:3,3]