preparing to validate the discrete formulas which are used as initial…

…ization for the ocp

examples/calculate_invariants_position_longtrial.py

@@ -36,19 +36,29 @@
 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}
 progress = "time" # options: {time, arclength}. This is the progress parameter with which trajectory/invariants evolve.
 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}
 

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')