From 5169bdd832c70c62930d2cd2d8d727e798c65923 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Wed, 20 Nov 2024 15:19:12 +0100 Subject: [PATCH] preparing to validate the discrete formulas which are used as initialization for the ocp --- .../calculate_invariants_position_longtrial.py | 16 +++++++++++++--- invariants_py/invariants_handler.py | 2 +- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/examples/calculate_invariants_position_longtrial.py b/examples/calculate_invariants_position_longtrial.py index c3589b6..9fbde89 100644 --- a/examples/calculate_invariants_position_longtrial.py +++ b/examples/calculate_invariants_position_longtrial.py @@ -36,11 +36,21 @@ trajectory = np.column_stack((df['x'], df['y'], df['z'])) timestamps = df['timestamp'].values #downsampled_indices = np.linspace(0, len(trajectory) - 1, nb_samples, dtype=int) -trajectory = trajectory[0:200]*10 # correction: the time step should be around 0.1s -timestamps = timestamps[0:200] +trajectory = trajectory[0:200] # correction: the time step should be around 0.1s +timestamps = timestamps[0:200]*10 print(timestamps) print(trajectory) +#/*********************************************************************************************************************/ +#/* Option 1: Calculate invariants using discretized analytical formulas +#/*********************************************************************************************************************/ + + + +#/*********************************************************************************************************************/ +#/* Option 2: Calculate invariants using optimal control problem (OCP) method */ +#/*********************************************************************************************************************/ + # Options choice_invariants = "vector_invariants" # options: {vector_invariants, screw_invariants} trajectory_type = "position" # options: {position, pose} @@ -48,7 +58,7 @@ normalized_progress = False # scale progress to be between 0 and 1 scale_invariance = False # scale trajectory to unit length, where length is defined by the progress parameter (e.g. arclength) ocp_implementation = "rockit" # options: {rockit, optistack} -solver = "fatrop" # options: {ipopt, fatrop} +solver = "ipopt" # options: {ipopt, fatrop} rms_error_tolerance = 1e-1 solver_options = {"max_iter": 200} diff --git a/invariants_py/invariants_handler.py b/invariants_py/invariants_handler.py index 082e315..227a6b9 100644 --- a/invariants_py/invariants_handler.py +++ b/invariants_py/invariants_handler.py @@ -70,7 +70,7 @@ def calculate_invariants_translation(self, time_array, trajectory_meas): fatrop_solver = True else: fatrop_solver = False - FS_calculation_problem = FS_calculation_rockit.OCP_calc_pos(window_len=nb_samples, geometric=constant_invariant, fatrop_solver=fatrop_solver, rms_error_traj=self.rms_error_tolerance) + FS_calculation_problem = FS_calculation_rockit.OCP_calc_pos(window_len=nb_samples, geometric=constant_invariant, fatrop_solver=fatrop_solver, rms_error_traj=self.rms_error_tolerance, solver_options=self.solver_options) elif self.ocp_implementation == "optistack": print('Calculating with optistack and ipopt')