From 59cb37ca8a14bcfec62b5ed7600ef22ce5cda83d Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Wed, 27 Nov 2024 17:03:48 +0100 Subject: [PATCH 01/11] more fatrop options --- .../rockit_calculate_vector_invariants_position.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 71f7091..856ed28 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -91,7 +91,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b running_ek = ocp.state() # running sum of squared error ocp.subject_to(ocp.at_t0(running_ek == 0)) ocp.set_next(running_ek, running_ek + ek) # sum over the control grid - ocp.subject_to(ocp.at_tf( (running_ek + ek) < N*rms_error_traj**2 )) + ocp.subject_to(ocp.at_tf( (running_ek + ek)/(N*rms_error_traj**2) < 1 )) # TODO this is still needed because last sample is not included in the sum now #total_ek = ocp.state() # total sum of squared error @@ -143,7 +143,10 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b ocp._method.set_option("tol",tolerance) ocp._method.set_option("print_level",print_level) ocp._method.set_option("max_iter",max_iter) - ocp._method.set_option("linsol_lu_fact_tol",1e-12) + ocp._method.set_option("linsol_lu_fact_tol",1e-6) + ocp._method.set_option("linsol_perturbed_mode","no") + ocp._method.set_option("mu_init",1e5) + else: ocp.method(rockit.MultipleShooting(N=N-1)) ocp.solver('ipopt', {'expand':True, 'print_time':False, 'ipopt.tol':tolerance, 'ipopt.print_info_string':'yes', 'ipopt.max_iter':max_iter, 'ipopt.print_level':print_level, 'ipopt.ma57_automatic_scaling':'no', 'ipopt.linear_solver':'mumps'}) @@ -216,6 +219,11 @@ def calculate_invariants(self, measured_positions, stepsize, use_previous_soluti self.first_time = False # Solve the optimization problem for the given measurements starting from previous solution + + # print(measured_positions.T) + # print(stepsize) + # print(*self.values_variables) + self.values_variables = self.ocp_function(measured_positions.T, stepsize, *self.values_variables) # Return the results From 30f8d3843caa19c82bf9b86cfeef91f0d063e0d8 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Wed, 27 Nov 2024 22:18:08 +0100 Subject: [PATCH 02/11] revert constraint --- .../rockit_calculate_vector_invariants_position.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 856ed28..29aa624 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -85,13 +85,13 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b if not fatrop_solver: # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj total_ek = ocp.sum(ek,grid='control',include_last=True) - ocp.subject_to(total_ek < N*rms_error_traj**2) + ocp.subject_to(total_ek < (N*rms_error_traj**2)) else: # Fatrop does not support summing over grid points inside a constraint, so we implement the sum using a running cost to achieve the same result as above running_ek = ocp.state() # running sum of squared error ocp.subject_to(ocp.at_t0(running_ek == 0)) ocp.set_next(running_ek, running_ek + ek) # sum over the control grid - ocp.subject_to(ocp.at_tf( (running_ek + ek)/(N*rms_error_traj**2) < 1 )) + ocp.subject_to(ocp.at_tf( (running_ek + ek) < (N*rms_error_traj**2) )) # TODO this is still needed because last sample is not included in the sum now #total_ek = ocp.state() # total sum of squared error From 5c5633085b8bf4a9172011ee417798a79e727688 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Thu, 28 Nov 2024 13:26:02 +0100 Subject: [PATCH 03/11] test --- ...it_calculate_vector_invariants_position.py | 47 +++++++++++++------ .../discretized_vector_invariants.py | 12 ++--- invariants_py/ocp_initialization.py | 24 +++++----- 3 files changed, 51 insertions(+), 32 deletions(-) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 29aa624..3d93504 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -22,11 +22,12 @@ import rockit from invariants_py import ocp_helper, ocp_initialization from invariants_py.ocp_helper import check_solver -from invariants_py.dynamics_vector_invariants import integrate_vector_invariants_position +from invariants_py.dynamics_vector_invariants import integrate_vector_invariants_position, integrate_vector_invariants_position_seq +from invariants_py import discretized_vector_invariants as dvi class OCP_calc_pos: - def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, bool_unsigned_invariants=False, planar_task=False, solver_options = {}, geometric=False): + def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, bool_unsigned_invariants=False, planar_task=False, solver_options = {}, geometric=False, periodic=False): """ Initializes an instance of the RockitCalculateVectorInvariantsPosition class. It specifies the optimal control problem (OCP) for calculating the invariants of a trajectory in a symbolic way. @@ -65,7 +66,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # System dynamics (integrate current states + controls to obtain next states) # this relates the states/controls over the whole window - (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position(R_t, p_obj, invars, h) + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, invars, h) ocp.set_next(p_obj,p_obj_plus1) ocp.set_next(R_t,R_t_plus1) @@ -76,28 +77,38 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # Lower bounds on controls if bool_unsigned_invariants: - ocp.subject_to(invars[0,:]>=0) # velocity always positive - #ocp.subject_to(invars[1,:]>=0) # curvature rate always positive + ocp.subject_to(invars[0]>=0) # velocity always positive + #ocp.subject_to(invars[1]>=0) # curvature rate always positive + + #ocp.subject_to(-np.pi <= invars[1,:]*h) # curvature rate bounded by 1 + #ocp.subject_to(-np.pi <= invars[2,:]*h) # curvature rate bounded by 1 + #ocp.subject_to(invars[1,:]*h <= np.pi) # curvature rate bounded by 1 + #ocp.subject_to(invars[2,:]*h <= np.pi) # curvature rate bounded by 1 # Measurement fitting constraint # TODO: what about specifying the tolerance per sample instead of the sum? ek = cas.dot(p_obj - p_obj_m, p_obj - p_obj_m) # squared position error if not fatrop_solver: # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj - total_ek = ocp.sum(ek,grid='control',include_last=True) + total_ek = ocp.sum(ek,grid='control',include_last=True) ocp.subject_to(total_ek < (N*rms_error_traj**2)) else: # Fatrop does not support summing over grid points inside a constraint, so we implement the sum using a running cost to achieve the same result as above running_ek = ocp.state() # running sum of squared error ocp.subject_to(ocp.at_t0(running_ek == 0)) ocp.set_next(running_ek, running_ek + ek) # sum over the control grid - ocp.subject_to(ocp.at_tf( (running_ek + ek) < (N*rms_error_traj**2) )) + ocp.subject_to(ocp.at_tf( (running_ek + ek)/(N*rms_error_traj**2) < 1 )) + + #ocp.subject_to(ek < rms_error_traj**2) # squared position error should be less than the specified tolerance rms_error_traj - # TODO this is still needed because last sample is not included in the sum now - #total_ek = ocp.state() # total sum of squared error - #ocp.set_next(total_ek, total_ek) - #ocp.subject_to(ocp.at_tf(total_ek == running_ek + ek)) - # total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error + # # TODO this is still needed because last sample is not included in the sum now + # #total_ek = ocp.state() # total sum of squared error + # #ocp.set_next(total_ek, total_ek) + # #ocp.subject_to(ocp.at_tf(total_ek == running_ek + ek)) + # # total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error + + total_ek = ocp.sum(ek,grid='control',include_last=True) + ocp.add_objective(total_ek/N) #ocp.subject_to(total_ek/N < rms_error_traj**2) #total_ek_scaled = running_ek/N/rms_error_traj**2 # scaled total error @@ -118,6 +129,12 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b L = ocp.state() # introduce extra state L for the speed ocp.set_next(L, L) # enforce constant speed ocp.subject_to(invars[0] - L == 0, include_last=False) # relate to first invariant + + #if periodic: + # ocp.subject_to(ocp.at_tf(p_obj) - ocp.at_t0(p_obj) == 0) + # # Periodic boundary conditions (optional) + # ocp.subject_to(ocp.at_tf(R_t) == ocp.at_t0(R_t)) + """ Objective function """ @@ -144,8 +161,8 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b ocp._method.set_option("print_level",print_level) ocp._method.set_option("max_iter",max_iter) ocp._method.set_option("linsol_lu_fact_tol",1e-6) - ocp._method.set_option("linsol_perturbed_mode","no") - ocp._method.set_option("mu_init",1e5) + #ocp._method.set_option("linsol_perturbed_mode","no") + ocp._method.set_option("mu_init",1e2) else: ocp.method(rockit.MultipleShooting(N=N-1)) @@ -222,7 +239,7 @@ def calculate_invariants(self, measured_positions, stepsize, use_previous_soluti # print(measured_positions.T) # print(stepsize) - # print(*self.values_variables) + # print(*self.values_variables)False self.values_variables = self.ocp_function(measured_positions.T, stepsize, *self.values_variables) diff --git a/invariants_py/discretized_vector_invariants.py b/invariants_py/discretized_vector_invariants.py index 6bb7174..7779b85 100644 --- a/invariants_py/discretized_vector_invariants.py +++ b/invariants_py/discretized_vector_invariants.py @@ -141,15 +141,15 @@ def calculate_binormal(vector_traj,tangent, tolerance_singularity, reference_vec return binormal -def calculate_moving_frames(vector_traj, tolerance_singularity = 1e-2): +def calculate_moving_frames(vector_traj, tolerance_singularity_vel = 1e-2, tolerance_singularity_curv = 1e-2): N = np.size(vector_traj, 0) # Calculate first axis - e_tangent = calculate_tangent(vector_traj, tolerance_singularity) + e_tangent = calculate_tangent(vector_traj, tolerance_singularity_vel) # Calculate third axis - e_binormal = calculate_binormal(vector_traj,e_tangent,tolerance_singularity,reference_vector=np.array([0,0,1])) + e_binormal = calculate_binormal(vector_traj,e_tangent,tolerance_singularity_curv,reference_vector=np.array([0,0,1])) # Calculate second axis as cross product of third and first axis e_normal = np.array([ np.cross(e_binormal[i,:],e_tangent[i,:]) for i in range(N) ]) @@ -214,7 +214,7 @@ def calculate_vector_invariants(R_mf_traj,vector_traj,progress_step): return invariants -def calculate_discretized_invariants(measured_positions, progress, tolerance_singularity = 1e0): +def calculate_discretized_invariants(measured_positions, progress_step, tolerance_singularity_vel = 1e-1, tolerance_singularity_curv = 1e-2): """ Calculate the vector invariants of a measured position trajectory based on a discrete approximation of the moving frame. @@ -228,7 +228,7 @@ def calculate_discretized_invariants(measured_positions, progress, tolerance_sin mf: moving frame (Nx3x3) """ - progress_step = np.mean(np.diff(progress))#.reshape(-1,1) + #progress_step = np.mean(np.diff(progress))#.reshape(-1,1) # Calculate velocity vector in a discretized way pos_vector_differences = np.diff(measured_positions,axis=0) @@ -236,7 +236,7 @@ def calculate_discretized_invariants(measured_positions, progress, tolerance_sin #print(vel_vector) # Calculate the moving frame - R_mf_traj = calculate_moving_frames(vel_vector, tolerance_singularity) + R_mf_traj = calculate_moving_frames(vel_vector, tolerance_singularity_vel, tolerance_singularity_curv) #print(R_mf_traj) # Calculate the invariants diff --git a/invariants_py/ocp_initialization.py b/invariants_py/ocp_initialization.py index e74e4b2..6f5ce69 100644 --- a/invariants_py/ocp_initialization.py +++ b/invariants_py/ocp_initialization.py @@ -1,7 +1,7 @@ import numpy as np import invariants_py.kinematics.orientation_kinematics as SO3 from invariants_py.reparameterization import interpR -from invariants_py.discretized_vector_invariants import calculate_vector_invariants, calculate_velocity_from_discrete_rotations +from invariants_py.discretized_vector_invariants import calculate_discretized_invariants, calculate_velocity_from_discrete_rotations def initial_trajectory_movingframe_rotation(R_obj_start,R_obj_end,N=100): @@ -204,26 +204,28 @@ def estimate_initial_frames(vector_traj): def initialize_VI_pos2(measured_positions,stepsize): N = np.size(measured_positions,0) - Pdiff = np.diff(measured_positions, axis=0) - Pdiff = np.vstack((Pdiff, Pdiff[-1])) + invariants, meas_pos, mf = calculate_discretized_invariants(measured_positions,stepsize) + + #Pdiff = np.diff(measured_positions, axis=0) + #Pdiff = np.vstack((Pdiff, Pdiff[-1])) - [ex,ey,ez] = estimate_initial_frames(Pdiff) + #[ex,ey,ez] = estimate_initial_frames(Pdiff) - R_t_init2 = np.zeros((N,3,3)) - for i in range(N): - R_t_init2[i,:,:] = np.column_stack((ex[i,:],ey[i,:],ez[i,:])) + #R_t_init2 = np.zeros((N,3,3)) + #for i in range(N): + # R_t_init2[i,:,:] = np.column_stack((ex[i,:],ey[i,:],ez[i,:])) #print(R_t_init2) - invars = calculate_vector_invariants(R_t_init2,Pdiff,stepsize) + 1e-12*np.ones((N,3)) + #invars = calculate_vector_invariants(R_t_init2,Pdiff,stepsize) + 1e-12*np.ones((N,3)) #print(invars) R_t_init = np.zeros((9,N)) for i in range(N): - R_t_init[:,i] = np.hstack([ex[i,:],ey[i,:],ez[i,:]]) + R_t_init[:,i] = np.hstack([mf[i,0],mf[i,1],mf[i,2]]) - p_obj_sol = measured_positions.T + p_obj_sol = meas_pos.T #invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) - return [invars[:-1,:].T, p_obj_sol, R_t_init] + return [invariants[:-1,:].T, p_obj_sol, R_t_init] def initialize_VI_rot(input_trajectory): From f3d2bf99d4090f789d74b742af8437b9012e7fa8 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 10:42:00 +0100 Subject: [PATCH 04/11] new variant windowed OCP with constant invariants --- ...calculate_invariants_position_longtrial.py | 3 +- ...it_calculate_vector_invariants_position.py | 16 +- ...alculate_vector_invariants_position_cte.py | 372 ++++++++++++++++++ .../calculate_invariants/test_helix_fit.py | 58 +++ .../calculate_invariants/test_logm_expm.py | 0 .../discretized_vector_invariants.py | 4 +- 6 files changed, 447 insertions(+), 6 deletions(-) create mode 100644 invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py create mode 100644 invariants_py/calculate_invariants/test_helix_fit.py rename test_logm_expm.py => invariants_py/calculate_invariants/test_logm_expm.py (100%) diff --git a/examples/calculate_invariants_position_longtrial.py b/examples/calculate_invariants_position_longtrial.py index 3b20a65..6a7c5a8 100644 --- a/examples/calculate_invariants_position_longtrial.py +++ b/examples/calculate_invariants_position_longtrial.py @@ -49,8 +49,9 @@ #/* Option 1: Calculate invariants using discretized analytical formulas #/*********************************************************************************************************************/ +progress_step = np.mean(np.diff(timestamps))#.reshape(-1,1) from invariants_py import discretized_vector_invariants as dvi -invariants_init, trajectory_init, moving_frames_init = dvi.calculate_discretized_invariants(trajectory, timestamps) +invariants_init, trajectory_init, moving_frames_init = dvi.calculate_discretized_invariants(trajectory, progress_step) print(invariants_init) plotters.plot_moving_frames(trajectory, moving_frames_init) # calculated moving frames along trajectory diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 29aa624..b816760 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -86,6 +86,11 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj total_ek = ocp.sum(ek,grid='control',include_last=True) ocp.subject_to(total_ek < (N*rms_error_traj**2)) + + #ocp.subject_to(ek < 0.01**2) + #ocp.subject_to(p_obj - p_obj_m > -np.array((0.0001,0.0001,0.0001))) # squared position error at each sample should be less than the specified tolerance rms_error_traj + #ocp.subject_to(p_obj - p_obj_m < np.array((0.0001,0.0001,0.0001))) # squared position error at each sample should be less than the specified tolerance rms_error_traj + else: # Fatrop does not support summing over grid points inside a constraint, so we implement the sum using a running cost to achieve the same result as above running_ek = ocp.state() # running sum of squared error @@ -93,6 +98,11 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b ocp.set_next(running_ek, running_ek + ek) # sum over the control grid ocp.subject_to(ocp.at_tf( (running_ek + ek) < (N*rms_error_traj**2) )) + + #ocp.subject_to(ek < 0.001**2) # squared position error at each sample should be less than the specified tolerance rms_error_traj + #ocp.subject_to(p_obj - p_obj_m < np.array((0.0001,0.0001,0.0001))) # squared position error at each sample should be less than the specified tolerance rms_error_traj + + # TODO this is still needed because last sample is not included in the sum now #total_ek = ocp.state() # total sum of squared error #ocp.set_next(total_ek, total_ek) @@ -143,9 +153,9 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b ocp._method.set_option("tol",tolerance) ocp._method.set_option("print_level",print_level) ocp._method.set_option("max_iter",max_iter) - ocp._method.set_option("linsol_lu_fact_tol",1e-6) - ocp._method.set_option("linsol_perturbed_mode","no") - ocp._method.set_option("mu_init",1e5) + ocp._method.set_option("linsol_lu_fact_tol",1e-12) + #ocp._method.set_option("linsol_perturbed_mode","no") + #ocp._method.set_option("mu_init",1e5) else: ocp.method(rockit.MultipleShooting(N=N-1)) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py new file mode 100644 index 0000000..8567e98 --- /dev/null +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py @@ -0,0 +1,372 @@ +import numpy as np +import casadi as cas +import rockit +from invariants_py import ocp_helper, ocp_initialization +from invariants_py.ocp_helper import check_solver +from invariants_py.dynamics_vector_invariants import integrate_vector_invariants_position, integrate_vector_invariants_position_seq + +class OCP_calc_pos: + + def __init__(self, + window_len=100, + w_pos = 1, w_rot = 1, + w_deriv = (10**-6)*np.array([1.0, 1.0, 1.0]), + w_abs = (10**-10)*np.array([1.0, 1.0]), + bool_unsigned_invariants = False, + planar_task = False, + geometric = False, + fatrop_solver = True, + solver_options = {}): + # TODO change "planar_task" to "planar_trajectory" + # TODO change bool_unsigned_invariants to positive_velocity + + # Set solver options + tolerance = solver_options.get('tol',1e-4) # tolerance for the solver + max_iter = solver_options.get('max_iter',500) # maximum number of iterations + print_level = solver_options.get('print_level',5) # 5 prints info, 0 prints nothing + + # Use rockit to define the optimal control problem (OCP) + ocp = rockit.Ocp(T=1.0) + N = window_len + + """ Variables, parameters and dynamics """ + + # States and controls (= decision variables) + p_obj = ocp.state(3) # position trajectory (xyz-coordinates) + R_t_vec = ocp.state(9) # moving frame (Frenet-Serret frame) as a 9x1 vector + R_t = cas.reshape(R_t_vec,(3,3)) # reshape vector to 3x3 rotation matrix + invars = ocp.control(3) # invariants (velocity, curvature rate, torsion rate) + + #invars_deriv = ocp.control(3) # invariants derivatives + + # Parameters + p_obj_m = ocp.parameter(3,grid='control+') # measured position trajectory (xyz-coordinates) + #R_t_0 = ocp.parameter(3,3) # initial translational Frenet-Serret frame (for enforcing continuity of the moving frame) + h = ocp.parameter(1,1) # step-size (can be a discrete step in time or arclength) + + # System dynamics (integrate current states + controls to obtain next states) + # this relates the states/controls over the whole window + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, invars, h) + ocp.set_next(p_obj,p_obj_plus1) + ocp.set_next(R_t,R_t_plus1) + + """ Constraints """ + + # Orthonormality of rotation matrix (only needed for one timestep, property is propagated by integrator) + ocp.subject_to(ocp.at_t0(ocp_helper.tril_vec(R_t.T @ R_t - np.eye(3)) == 0.)) + + # Lower bounds on controls + if bool_unsigned_invariants: + ocp.subject_to(invars[0,:]>=0) # velocity always positive + #ocp.subject_to(invars[1,:]>=0) # curvature rate always positive + + # Measurement fitting constraint + # TODO: what about specifying the tolerance per sample instead of the sum? + # Create the first half of the weight vector (from 0 to 1) + + # create a weight vector that evolves linearly from 0 at 1, to 1 at N/2, and back to 0 at N + + + ek = cas.dot((p_obj - p_obj_m), (p_obj - p_obj_m)) # squared position error + #if not fatrop_solver: + # # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj + # total_ek = ocp.sum(ek,grid='control',include_last=True) + # ocp.subject_to(total_ek/N < rms_error_traj**2) + #else: + # # Fatrop does not support summing over grid points inside the constraint, so we implement it differently to achieve the same result + # running_ek = ocp.state() # running sum of squared error + # ocp.subject_to(ocp.at_t0(running_ek == 0)) + # ocp.set_next(running_ek, running_ek + ek) + # ocp.subject_to(ocp.at_tf(1000*running_ek/N < 1000*rms_error_traj**2)) # scaling to avoid numerical issues in fatrop + + # # TODO this is still needed because last sample is not included in the sum now + # total_ek = ocp.state() # total sum of squared error + # ocp.set_next(total_ek, total_ek) + # ocp.subject_to(ocp.at_tf(total_ek == running_ek + ek)) + # # total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error + # ocp.subject_to(1000*total_ek/N < 1000*rms_error_traj**2) + # #total_ek_scaled = running_ek/N/rms_error_traj**2 # scaled total error + # #ocp.subject_to(ocp.at_tf(total_ek_scaled < 1)) + + #ocp.subject_to(ek < rms_error_traj**2) + + # Boundary conditions (optional, but may help to avoid straight line fits) + #ocp.subject_to(ocp.at_t0(p_obj == p_obj_m)) # fix first position to measurement + #ocp.subject_to(ocp.at_tf(p_obj == p_obj_m)) # fix last position to measurement + + if planar_task: + # Constrain the binormal vector of the moving frame to point upwards + # Useful to prevent frame flips in case of planar tasks + # TODO make normal to plane a parameter instead of hardcoding the Z-axis + ocp.subject_to( cas.dot(R_t[:,2],np.array([0,0,1])) > 0) + + # Geometric invariants (optional), i.e. enforce constant speed + #if geometric: + # L = ocp.state() # introduce extra state L for the speed + # ocp.set_next(L, L) # enforce constant speed + # ocp.subject_to(invars[0] - L == 0, include_last=False) # relate to first invariant + + vel = ocp.state() + ocp.set_next(vel, vel) + kappa = ocp.state() + ocp.set_next(kappa, kappa) + tau = ocp.state() + ocp.set_next(tau, tau) + ocp.subject_to(invars[0] - vel ==0, include_last=False) + ocp.subject_to(invars[1] - kappa == 0, include_last=False) + ocp.subject_to(invars[2] - tau == 0, include_last=False) + + + """ Objective function """ + + # Minimize moving frame invariants to deal with singularities and noise + objective_reg = ocp.sum(cas.dot(invars[:3],invars[:3])) + objective = 10e-10*objective_reg/(N-1) + ocp.add_objective(objective) + + # objective_reg2 = ocp.sum(cas.dot(invars_deriv,invars_deriv)) + # objective_reg2_scaled = 0.000001*objective_reg2/(N-1) + # ocp.add_objective(objective_reg2_scaled) + + objective_fit = ocp.sum(ek, include_last=True) + ocp.add_objective(objective_fit) + + """ Solver definition """ + + if check_solver(fatrop_solver): + ocp.method(rockit.external_method('fatrop',N=N-1)) + ocp._method.set_name("/codegen/calculate_position") + ocp._method.set_expand(True) + else: + ocp.method(rockit.MultipleShooting(N=N-1)) + ocp.solver('ipopt', {'expand':True, 'ipopt.tol':tolerance,'ipopt.print_info_string':'yes', 'ipopt.max_iter':max_iter,'ipopt.print_level':print_level, 'ipopt.ma57_automatic_scaling':'no', 'ipopt.linear_solver':'mumps'}) + + """ Encapsulate solver in a casadi function so that it can be easily reused """ + + # Solve already once with dummy values so that Fatrop can do code generation (Q: can this step be avoided somehow?) + ocp.set_initial(R_t, np.eye(3)) + ocp.set_initial(invars, np.array([1,0.01,0.01])) + ocp.set_value(p_obj_m, np.vstack((np.linspace(0, 1, N), np.ones((2, N))))) + ocp.set_value(h, 0.01) + ocp.solve_limited() # code generation + + # Set Fatrop solver options (Q: why can this not be done before solving?) + if fatrop_solver: + ocp._method.set_option("tol",tolerance) + ocp._method.set_option("print_level",print_level) + ocp._method.set_option("max_iter",max_iter) + self.first_time = True + + # Encapsulate OCP specification in a casadi function after discretization (sampling) + p_obj_m_sampled = ocp.sample(p_obj_m, grid='control')[1] # sampled measured object positions + h_value = ocp.value(h) # value of stepsize + solution = [ocp.sample(invars, grid='control-')[1], # sampled invariants + ocp.sample(p_obj, grid='control')[1], # sampled object positions + ocp.sample(R_t_vec, grid='control')[1]] # sampled FS frame + + self.ocp = ocp # this line avoids a problem in case of multiple OCP's (e.g. position and rotation invariants are calculated separately) + self.ocp_function = self.ocp.to_function('ocp_function', + [p_obj_m_sampled,h_value,*solution], # inputs + [*solution], # outputs + ["p_obj_m","h","invars_in","p_obj_in","R_t_in"], # (optional) input labels for debugging + ["invars_out","p_obj_out","R_t_out"]) # (optional) output labels for debugging + + # Set variables as class properties (only necessary for calculate_invariants_OLD function) + self.R_t = R_t_vec + self.p_obj = p_obj + self.U = invars + self.p_obj_m = p_obj_m + self.window_len = window_len + self.ocp = ocp + self.first_window = True + self.h = h + + def calculate_invariants(self, measured_positions, stepsize, use_previous_solution=True, init_values=None): + """ + Calculate the invariants for the given measurements. + + Args: + - measured_positions (numpy.ndarray of shape N x 3): measured positions. + - stepsize (float): the discrete time step or arclength step between measurements + - use_previous_solution (bool, optional): If True, the previous solution is used as the initial guess for the optimization problem. Defaults to True. + + Returns: + - invariants (numpy.ndarray of shape (N, 3)): calculated invariants + - calculated_trajectory (numpy.ndarray of shape (N, 3)): fitted trajectory corresponding to invariants + - calculated_movingframes (numpy.ndarray of shape (N, 3, 3)): moving frames corresponding to invariants + """ + + # Check if this is the first function call + if not use_previous_solution or self.first_time: + + if init_values is not None: + + temp0 = init_values[0][:-1,:].T + temp1 = init_values[1].T + temp2 = init_values[2] + N = np.size(temp2,0) + R_t_init = np.zeros((9,np.size(temp2,0))) + for i in range(N): + R_t_init[:,i] = temp2[i,:,:].reshape(-1) + + self.values_variables = [temp0,temp1,R_t_init] + else: + + # Initialize states and controls using measurements + self.values_variables = ocp_initialization.initialize_VI_pos2(measured_positions,stepsize) + self.first_time = False + + # Solve the optimization problem for the given measurements starting from previous solution + self.values_variables = self.ocp_function(measured_positions.T, stepsize, *self.values_variables) + + # Return the results + invars_sol, p_obj_sol, R_t_sol = self.values_variables # unpack the results + invariants = np.array(invars_sol).T # make a N-1 x 3 array + invariants = np.vstack((invariants, invariants[-1, :])) # make a N x 3 array by repeating last row + calculated_trajectory = np.array(p_obj_sol).T # make a N x 3 array + calculated_movingframe = np.transpose(np.reshape(R_t_sol.T, (-1, 3, 3)), (0, 2, 1)) # make a N x 3 x 3 array + return invariants, calculated_trajectory, calculated_movingframe + + def calculate_invariants_OLD(self, measured_positions, stepsize): + """ + Calculate the invariants for the given measurements. + + Note: This function is not recommended for repeated use due to overhead caused by sampling the solution. + + Parameters: + - measured_positions (numpy.ndarray of shape (N, 3)): measured positions + - stepsize (float): the discrete time step or arclength step between measurements + + Returns: + - invariants (numpy.ndarray of shape (N, 3)): calculated invariants + - calculated_trajectory (numpy.ndarray of shape (N, 3)): fitted trajectory corresponding to invariants + - calculated_movingframes (numpy.ndarray of shape (N, 3, 3)): moving frames corresponding to invariants + """ + + N = self.window_len + + # Estimate moving frames from measurements TODO make this a function + Pdiff = np.diff(measured_positions, axis=0) + ex = Pdiff / np.linalg.norm(Pdiff, axis=1).reshape(N-1, 1) + ex = np.vstack((ex, [ex[-1, :]])) + ez = np.tile(np.array((0, 0, 1)), (N, 1)) + ey = np.array([np.cross(ez[i, :], ex[i, :]) for i in range(N)]) + + R_obj_init = np.zeros((9, N)) + for i in range(N): + R_obj_init[:, i] = np.hstack([ex[i, :], ey[i, :], ez[i, :]]) + + # Initialize states + self.ocp.set_initial(self.R_t, R_obj_init) + self.ocp.set_initial(self.p_obj, measured_positions.T) + + # Initialize controls + self.ocp.set_initial(self.U, [0.01, 1e-12, 1e-12]) + + # Set values parameters + self.ocp.set_value(self.p_obj_m, measured_positions.T) + self.ocp.set_value(self.h, stepsize) + + # Solve the OCP + sol = self.ocp.solve_limited() + + # Extract the solved variables + invariants = np.array([sol.sample(self.U[i], grid='control')[1] for i in range(3)]).T + trajectory = sol.sample(self.p_obj, grid='control')[1] + moving_frames = sol.sample(self.R_t, grid='control')[1] + + return invariants, trajectory, moving_frames + + def calculate_invariants_new(self, measured_positions, stepsize): + """ + Calculate the invariants for the given measurements. + + Note: This function is not recommended for repeated use due to overhead caused by sampling the solution. + + Parameters: + - measured_positions (numpy.ndarray of shape (N, 3)): measured positions + - stepsize (float): the discrete time step or arclength step between measurements + + Returns: + - invariants (numpy.ndarray of shape (N, 3)): calculated invariants + - calculated_trajectory (numpy.ndarray of shape (N, 3)): fitted trajectory corresponding to invariants + - calculated_movingframes (numpy.ndarray of shape (N, 3, 3)): moving frames corresponding to invariants + """ + + N = self.window_len + + # Estimate moving frames from measurements TODO make this a function + Pdiff = np.diff(measured_positions, axis=0) + ex = Pdiff / np.linalg.norm(Pdiff, axis=1).reshape(N-1, 1) + ex = np.vstack((ex, [ex[-1, :]])) + ez = np.tile(np.array((0, 0, 1)), (N, 1)) + ey = np.array([np.cross(ez[i, :], ex[i, :]) for i in range(N)]) + + ex = np.array([1,0,0]) + ez = np.array([0,0,1]) + ey = np.array([0,1,0]) + + #R_obj_init = np.zeros((9, N)) + #for i in range(N): + R_obj_init = np.eye(3) + + # Initialize states + self.ocp.set_initial(self.R_t, R_obj_init) + self.ocp.set_initial(self.p_obj, measured_positions.T) + + # Initialize controls + self.ocp.set_initial(self.U, [1e-12, 1e-12, 1e-12]) + + # Set values parameters + self.ocp.set_value(self.p_obj_m, measured_positions.T) + self.ocp.set_value(self.h, stepsize) + + # Solve the OCP + sol = self.ocp.solve_limited() + + # Extract the solved variables + invariants = np.array([sol.sample(self.U[i], grid='control')[1] for i in range(3)]).T + trajectory = sol.sample(self.p_obj, grid='control')[1] + moving_frames = sol.sample(self.R_t, grid='control')[1] + + return invariants, trajectory, moving_frames + + +if __name__ == "__main__": + import matplotlib.pyplot as plt + + # Example data for measured positions and stepsize + N = 100 + t = np.linspace(0, 4, N) + measured_positions = np.column_stack((1 * np.cos(t), 1 * np.sin(t), 0.1 * t)) + np.random.normal(scale=0.01, size=(N, 3)) + stepsize = t[1]-t[0] + + # Test the functionalities of the class + OCP = OCP_calc_pos(window_len=N, fatrop_solver=True) + + # Call the calculate_invariants function and measure the elapsed time + #start_time = time.time() + calc_invariants, calc_trajectory, calc_movingframes = OCP.calculate_invariants(measured_positions, stepsize) + #elapsed_time = time.time() - start_time + + #ocp_helper.solution_check_pos(measured_positions,calc_trajectory,rms = 10**-3) + + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ax.plot(measured_positions[:, 0], measured_positions[:, 1], measured_positions[:, 2],'b.-') + ax.plot(calc_trajectory[:, 0], calc_trajectory[:, 1], calc_trajectory[:, 2],'r--') + + fig = plt.figure() + plt.plot(calc_invariants) + plt.show() + + #plt.plot(calc_trajectory) + #plt.show() + # # Print the results and elapsed time + # print("Calculated invariants:") + # print(calc_invariants) + # print("Calculated Moving Frame:") + # print(calc_movingframes) + # print("Calculated Trajectory:") + # print(calc_trajectory) + # print("Elapsed Time:", elapsed_time, "seconds") \ No newline at end of file diff --git a/invariants_py/calculate_invariants/test_helix_fit.py b/invariants_py/calculate_invariants/test_helix_fit.py new file mode 100644 index 0000000..ccb770a --- /dev/null +++ b/invariants_py/calculate_invariants/test_helix_fit.py @@ -0,0 +1,58 @@ +import numpy as np +from scipy.optimize import minimize +from scipy.spatial.transform import Rotation as R +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +def fit_helix(points, regularization=0.1): + def helix(t, a, b, c, d, e, f): + x = a * np.cos(b * t + c) + y = a * np.sin(b * t + c) + z = d * t + e + return np.array([x, y, z]).T + + def residual(params, points): + t = np.linspace(0, 1, len(points)) + a, b, c, d, e, f, rx, ry, rz, tx, ty, tz = params + fitted_points = helix(t, a, b, c, d, e, f) + rotation = R.from_euler('xyz', [rx, ry, rz]).as_matrix() + rotated_points = fitted_points @ rotation.T + translated_points = rotated_points + np.array([tx, ty, tz]) + return np.sum((points - translated_points) ** 2) + regularization * np.sum(params**2) + + initial_guess = [1, 2 * np.pi, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0] + result = minimize(residual, initial_guess, args=(points,)) + return result.x + +def generate_helix(params, num_points=100): + t = np.linspace(0, 1, num_points) + a, b, c, d, e, f, rx, ry, rz, tx, ty, tz = params + x = a * np.cos(b * t + c) + y = a * np.sin(b * t + c) + z = d * t + e + helix_points = np.array([x, y, z]).T + rotation = R.from_euler('xyz', [rx, ry, rz]).as_matrix() + rotated_points = helix_points @ rotation.T + translated_points = rotated_points + np.array([tx, ty, tz]) + return translated_points + +# Example usage with noisy data: +np.random.seed(0) +points = np.array([ + [1.0 + 2.0*np.cos(np.pi/4), 2.0*np.sin(np.pi/4), 1.0], + [0.7071 + 2.0*np.cos(np.pi/4), 0.7071 + 2.0*np.sin(np.pi/4), 2.0], + [0.0 + 2.0*np.cos(np.pi/4), 1.0 + 2.0*np.sin(np.pi/4), 3.0], + [-0.7071 + 2.0*np.cos(np.pi/4), 0.7071 + 2.0*np.sin(np.pi/4), 4.0], + [-1.0 + 2.0*np.cos(np.pi/4), 2.0*np.sin(np.pi/4), 5.0], +]) + np.random.normal(scale=0.1, size=(5, 3)) + +params = fit_helix(points, regularization=0) +fitted_points = generate_helix(params) + +# Plot the results +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.plot(points[:, 0], points[:, 1], points[:, 2], 'ro', label='Noisy Points') +ax.plot(fitted_points[:, 0], fitted_points[:, 1], fitted_points[:, 2], 'b-', label='Fitted Helix') +ax.legend() +plt.show() \ No newline at end of file diff --git a/test_logm_expm.py b/invariants_py/calculate_invariants/test_logm_expm.py similarity index 100% rename from test_logm_expm.py rename to invariants_py/calculate_invariants/test_logm_expm.py diff --git a/invariants_py/discretized_vector_invariants.py b/invariants_py/discretized_vector_invariants.py index 6bb7174..360834b 100644 --- a/invariants_py/discretized_vector_invariants.py +++ b/invariants_py/discretized_vector_invariants.py @@ -214,7 +214,7 @@ def calculate_vector_invariants(R_mf_traj,vector_traj,progress_step): return invariants -def calculate_discretized_invariants(measured_positions, progress, tolerance_singularity = 1e0): +def calculate_discretized_invariants(measured_positions, progress_step, tolerance_singularity = 1e0): """ Calculate the vector invariants of a measured position trajectory based on a discrete approximation of the moving frame. @@ -228,7 +228,7 @@ def calculate_discretized_invariants(measured_positions, progress, tolerance_sin mf: moving frame (Nx3x3) """ - progress_step = np.mean(np.diff(progress))#.reshape(-1,1) + #progress_step = np.mean(np.diff(progress))#.reshape(-1,1) # Calculate velocity vector in a discretized way pos_vector_differences = np.diff(measured_positions,axis=0) From 65fc3d9b27ec5535ad94ec92d9e4e6f25b26c6d7 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 11:02:11 +0100 Subject: [PATCH 05/11] fix issue --- examples/calculate_invariants_position_longtrial.py | 3 +-- .../rockit_calculate_vector_invariants_position.py | 6 +++--- invariants_py/ocp_initialization.py | 7 +++++-- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/examples/calculate_invariants_position_longtrial.py b/examples/calculate_invariants_position_longtrial.py index 6a7c5a8..4568b78 100644 --- a/examples/calculate_invariants_position_longtrial.py +++ b/examples/calculate_invariants_position_longtrial.py @@ -48,8 +48,7 @@ #/*********************************************************************************************************************/ #/* Option 1: Calculate invariants using discretized analytical formulas #/*********************************************************************************************************************/ - -progress_step = np.mean(np.diff(timestamps))#.reshape(-1,1) +progress_step = np.mean(np.diff(timestamps)) from invariants_py import discretized_vector_invariants as dvi invariants_init, trajectory_init, moving_frames_init = dvi.calculate_discretized_invariants(trajectory, progress_step) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 6b253d4..f756854 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -66,7 +66,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # System dynamics (integrate current states + controls to obtain next states) # this relates the states/controls over the whole window - (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, invars, h) + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position(R_t, p_obj, invars, h) ocp.set_next(p_obj,p_obj_plus1) ocp.set_next(R_t,R_t_plus1) @@ -117,8 +117,8 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # #ocp.subject_to(ocp.at_tf(total_ek == running_ek + ek)) # # total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error - total_ek = ocp.sum(ek,grid='control',include_last=True) - ocp.add_objective(total_ek/N) + #total_ek = ocp.sum(ek,grid='control',include_last=True) + #ocp.add_objective(total_ek/N) #ocp.subject_to(total_ek/N < rms_error_traj**2) #total_ek_scaled = running_ek/N/rms_error_traj**2 # scaled total error diff --git a/invariants_py/ocp_initialization.py b/invariants_py/ocp_initialization.py index 6f5ce69..5a7941a 100644 --- a/invariants_py/ocp_initialization.py +++ b/invariants_py/ocp_initialization.py @@ -220,9 +220,12 @@ def initialize_VI_pos2(measured_positions,stepsize): R_t_init = np.zeros((9,N)) for i in range(N): - R_t_init[:,i] = np.hstack([mf[i,0],mf[i,1],mf[i,2]]) + R_t_init[:,i] = np.hstack([mf[i,:,0],mf[i,:,1],mf[i,:,2]]) + + #print(R_t_init) + + p_obj_sol = measured_positions.T - p_obj_sol = meas_pos.T #invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) return [invariants[:-1,:].T, p_obj_sol, R_t_init] From eec9025c67e1c4acccea67105a5b992482628f2c Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 11:12:48 +0100 Subject: [PATCH 06/11] fix issue --- .../opti_calculate_vector_invariants_rotation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py index db6de78..0f5454f 100644 --- a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py +++ b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_rotation.py @@ -99,8 +99,8 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, rms_error def calculate_invariants(self,trajectory_meas,stepsize, choice_initialization=2): - from invariants_py.ocp_initialization import calculate_velocity_from_discrete_rotations, calculate_vector_invariants - from invariants_py.discretized_vector_invariants import calculate_moving_frames + from invariants_py.ocp_initialization import calculate_velocity_from_discrete_rotations + from invariants_py.discretized_vector_invariants import calculate_moving_frames, calculate_vector_invariants from invariants_py.dynamics_vector_invariants import reconstruct_rotation_traj from invariants_py.kinematics.screw_kinematics import average_vector_orientation_frame From cfff0c48b8c51c01a881e9a34b41e476d7cfd77e Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 11:22:12 +0100 Subject: [PATCH 07/11] finish moving frame invariant regularization --- ...calculate_vector_invariants_position_mf.py | 91 ++++++------------- 1 file changed, 26 insertions(+), 65 deletions(-) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py index 8de95c7..22cfb32 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_mf.py @@ -1,22 +1,3 @@ -''' -This class is used to calculate the invariants of a measured position trajectory using the Rockit optimal control problem (OCP) framework. - -The invariants are calculated by finding the invariant control inputs that reconstruct a trajectory such that it lies within a specified tolerance from the measured trajectory, while minimizing the curvature and torsion rate of the trajectory to deal with noise and singularities. - -Usage: - # Example data (helix) - N = 100 - t = np.linspace(0, 4, N) - measured_positions = np.column_stack((1 * np.cos(t), 1 * np.sin(t), 0.1 * t)) - stepsize = t[1]-t[0] - - # Specify optimal control problem (OCP) - OCP = OCP_calc_pos(window_len=N, rms_error_traj=10**-3) - - # Calculate invariants - invariants,trajectory,moving-frames = OCP.calculate_invariants(measured_positions, stepsize) -''' - import numpy as np import casadi as cas import rockit @@ -26,18 +7,17 @@ class OCP_calc_pos: - def __init__(self, window_len=100, rms_error_traj=10**-2, fatrop_solver=False, bool_unsigned_invariants=False, planar_task=False, geometric = False, solver_options = {}): - """ - Initializes an instance of the RockitCalculateVectorInvariantsPosition class. - It specifies the optimal control problem (OCP) for calculating the invariants of a trajectory in a symbolic way. + def __init__(self, + window_len=100, + w_pos = 1, w_rot = 1, + w_deriv = (10**-6)*np.array([1.0, 1.0, 1.0]), + w_abs = (10**-10)*np.array([1.0, 1.0]), + fatrop_solver=False, + bool_unsigned_invariants=False, + planar_task=False, + geometric = False, + solver_options = {}): - Args: - window_len (int, optional): The length of the window of trajectory measurmeents in the optimization problem. Defaults to 100. - rms_error_traj (float, optional): The tolerance for the squared RMS error of the trajectory. Defaults to 10**-2. - fatrop_solver (bool, optional): Flag indicating whether to use the Fatrop solver. Defaults to False. - bool_unsigned_invariants (bool, optional): Flag indicating whether to enforce unsigned invariants. Defaults to False. - planar_task (bool, optional): Flag indicating whether the task is planar. Defaults to False. - """ # TODO change "planar_task" to "planar_trajectory" # TODO change bool_unsigned_invariants to positive_velocity @@ -84,29 +64,9 @@ def __init__(self, window_len=100, rms_error_traj=10**-2, fatrop_solver=False, b # Measurement fitting constraint # TODO: what about specifying the tolerance per sample instead of the sum? - ek = cas.dot(p_obj - p_obj_m, p_obj - p_obj_m) # squared position error - if not fatrop_solver: - # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj - total_ek = ocp.sum(ek,grid='control',include_last=True) - ocp.subject_to(total_ek/N < rms_error_traj**2) - #else: - # # Fatrop does not support summing over grid points inside the constraint, so we implement it differently to achieve the same result - # running_ek = ocp.state() # running sum of squared error - # ocp.subject_to(ocp.at_t0(running_ek == 0)) - # ocp.set_next(running_ek, running_ek + ek) - # ocp.subject_to(ocp.at_tf(1000*running_ek/N < 1000*rms_error_traj**2)) # scaling to avoid numerical issues in fatrop - - # # TODO this is still needed because last sample is not included in the sum now - # total_ek = ocp.state() # total sum of squared error - # ocp.set_next(total_ek, total_ek) - # ocp.subject_to(ocp.at_tf(total_ek == running_ek + ek)) - # # total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error - # ocp.subject_to(1000*total_ek/N < 1000*rms_error_traj**2) - # #total_ek_scaled = running_ek/N/rms_error_traj**2 # scaled total error - # #ocp.subject_to(ocp.at_tf(total_ek_scaled < 1)) - - #ocp.subject_to(ek < rms_error_traj**2) + + #total_ek = ocp.sum(ek,grid='control',include_last=True # Boundary conditions (optional, but may help to avoid straight line fits) #ocp.subject_to(ocp.at_t0(p_obj == p_obj_m)) # fix first position to measurement #ocp.subject_to(ocp.at_tf(p_obj == p_obj_m)) # fix last position to measurement @@ -126,16 +86,17 @@ def __init__(self, window_len=100, rms_error_traj=10**-2, fatrop_solver=False, b """ Objective function """ # Minimize moving frame invariants to deal with singularities and noise - objective_reg = ocp.sum(cas.dot(invars[1:3],invars[1:3])) - objective = 0.000001*objective_reg/(N-1) - ocp.add_objective(objective) - - objective_reg2 = ocp.sum(cas.dot(invars_deriv,invars_deriv)) - objective_reg2_scaled = 0.000001*objective_reg2/(N-1) - ocp.add_objective(objective_reg2_scaled) + deriv_invariants_weighted = w_deriv**(0.5)*invars_deriv + cost_deriv_invariants = ocp.sum(cas.dot(deriv_invariants_weighted,deriv_invariants_weighted))/(N-1) + ocp.add_objective(cost_deriv_invariants) + + abs_invariants_weighted = w_abs**(0.5)*invars[1:3] + cost_absolute_invariants = ocp.sum(cas.dot(abs_invariants_weighted,abs_invariants_weighted))/(N-1) + ocp.add_objective(cost_absolute_invariants) - objective_fit = ocp.sum(ek, include_last=True) - ocp.add_objective(objective_fit) + pos_error_weighted = w_pos**(0.5)*(p_obj_m - p_obj) + cost_fitting = ocp.sum(cas.dot(pos_error_weighted, pos_error_weighted),grid='control',include_last=True)/N + ocp.add_objective(cost_fitting) """ Solver definition """ @@ -289,18 +250,18 @@ def calculate_invariants_OLD(self, measured_positions, stepsize): # Example data for measured positions and stepsize N = 100 t = np.linspace(0, 4, N) - measured_positions = np.column_stack((1 * np.cos(t), 1 * np.sin(t), 0.1 * t)) + measured_positions = np.column_stack((1 * np.cos(t), 1 * np.sin(t), 0.1 * t)) + np.random.normal(0, 0.001, (N, 3)) stepsize = t[1]-t[0] # Test the functionalities of the class - OCP = OCP_calc_pos(window_len=N, rms_error_traj=10**-3, fatrop_solver=True) + OCP = OCP_calc_pos(window_len=N, fatrop_solver=True) # Call the calculate_invariants function and measure the elapsed time #start_time = time.time() - calc_invariants, calc_trajectory, calc_movingframes = OCP.calculate_invariants_OLD(measured_positions, stepsize) + calc_invariants, calc_trajectory, calc_movingframes = OCP.calculate_invariants(measured_positions, stepsize) #elapsed_time = time.time() - start_time - ocp_helper.solution_check_pos(measured_positions,calc_trajectory,rms = 10**-3) + #ocp_helper.solution_check_pos(measured_positions,calc_trajectory,rms = 10**-3) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') From 412184d772a1ff5abacfe2b473ea95e61db35637 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 11:23:47 +0100 Subject: [PATCH 08/11] constant invariant variant finished --- ...ckit_calculate_vector_invariants_position_cte.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py index 8567e98..60ee08b 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position_cte.py @@ -9,14 +9,11 @@ class OCP_calc_pos: def __init__(self, window_len=100, - w_pos = 1, w_rot = 1, - w_deriv = (10**-6)*np.array([1.0, 1.0, 1.0]), - w_abs = (10**-10)*np.array([1.0, 1.0]), bool_unsigned_invariants = False, planar_task = False, - geometric = False, fatrop_solver = True, solver_options = {}): + # TODO change "planar_task" to "planar_trajectory" # TODO change bool_unsigned_invariants to positive_velocity @@ -46,7 +43,7 @@ def __init__(self, # System dynamics (integrate current states + controls to obtain next states) # this relates the states/controls over the whole window - (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, invars, h) + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position(R_t, p_obj, invars, h) ocp.set_next(p_obj,p_obj_plus1) ocp.set_next(R_t,R_t_plus1) @@ -120,9 +117,9 @@ def __init__(self, """ Objective function """ # Minimize moving frame invariants to deal with singularities and noise - objective_reg = ocp.sum(cas.dot(invars[:3],invars[:3])) - objective = 10e-10*objective_reg/(N-1) - ocp.add_objective(objective) + # objective_reg = ocp.sum(cas.dot(invars[:3],invars[:3])) + # objective = 10e-10*objective_reg/(N-1) + # ocp.add_objective(objective) # objective_reg2 = ocp.sum(cas.dot(invars_deriv,invars_deriv)) # objective_reg2_scaled = 0.000001*objective_reg2/(N-1) From 5203d634be073722363817c45f5413eb9c013070 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 16:01:16 +0100 Subject: [PATCH 09/11] sequential integrator for opti implementations --- ...ti_calculate_vector_invariants_position.py | 8 ++-- ...it_calculate_vector_invariants_position.py | 48 +++++++++++-------- invariants_py/dynamics_vector_invariants.py | 19 ++++++++ invariants_py/ocp_initialization.py | 8 ++-- tests/test_compare_ipopt_fatrop_position.py | 2 +- 5 files changed, 56 insertions(+), 29 deletions(-) diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py index 7d10551..bf0b5a6 100644 --- a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py @@ -13,7 +13,7 @@ def __init__(self, window_len = 100, solver_options = {}): # Set solver options - tolerance = solver_options.get('tol',1e-4) # tolerance for the solver + tolerance = solver_options.get('tol',1e-10) # tolerance for the solver max_iter = solver_options.get('max_iter',500) # maximum number of iterations print_level = solver_options.get('print_level',5) # 5 prints info, 0 prints nothing @@ -40,7 +40,7 @@ def __init__(self, window_len = 100, opti.subject_to(ocp_helper.tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0) # Dynamics constraints (Multiple shooting) - integrator = dynamics.define_integrator_invariants_position(h) + integrator = dynamics.define_integrator_invariants_position_seq(h) for k in range(window_len-1): # Integrate current state to obtain next state (next rotation and position) Xk_end = integrator(X[k],U[:,k],h) @@ -73,7 +73,7 @@ def __init__(self, window_len = 100, trajectory_error = trajectory_error + cas.dot(err_pos,err_pos) # r.24 in paper [Maxim 2023] - opti.subject_to(trajectory_error < window_len*rms_error_traj**2) + opti.subject_to(trajectory_error/(window_len*rms_error_traj**2) < 1) # Boundary constraints #opti.subject_to(self.p_obj[0] == self.p_obj_m[0]) # Fix first measurement @@ -95,7 +95,7 @@ def __init__(self, window_len = 100, opti.minimize(objective) opti.solver('ipopt',{"print_time":False,"expand":True},{ #'gamma_theta':1e-12, - 'max_iter':max_iter,'tol':tolerance,'print_level':print_level,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes'}) + 'max_iter':max_iter,'tol':tolerance,'print_level':print_level,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes','mu_init':1e5}) # Save variables self.R_t = R_t diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index f756854..6fb4532 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -43,7 +43,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # TODO change bool_unsigned_invariants to positive_velocity # Set solver options - tolerance = solver_options.get('tol',1e-4) # tolerance for the solver + tolerance = solver_options.get('tol',1e-10) # tolerance for the solver max_iter = solver_options.get('max_iter',500) # maximum number of iterations print_level = solver_options.get('print_level',5) # 5 prints info, 0 prints nothing @@ -66,7 +66,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # System dynamics (integrate current states + controls to obtain next states) # this relates the states/controls over the whole window - (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position(R_t, p_obj, invars, h) + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, invars, h) ocp.set_next(p_obj,p_obj_plus1) ocp.set_next(R_t,R_t_plus1) @@ -91,7 +91,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b if not fatrop_solver: # sum of squared position errors in the window should be less than the specified tolerance rms_error_traj total_ek = ocp.sum(ek,grid='control',include_last=True) - ocp.subject_to(total_ek < (N*rms_error_traj**2)) + ocp.subject_to(total_ek/(N*rms_error_traj**2) < 1) #ocp.subject_to(ek < 0.01**2) #ocp.subject_to(p_obj - p_obj_m > -np.array((0.0001,0.0001,0.0001))) # squared position error at each sample should be less than the specified tolerance rms_error_traj @@ -102,9 +102,9 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b running_ek = ocp.state() # running sum of squared error ocp.subject_to(ocp.at_t0(running_ek == 0)) ocp.set_next(running_ek, running_ek + ek) # sum over the control grid - ocp.subject_to(ocp.at_tf( (running_ek + ek) < (N*rms_error_traj**2) )) + ocp.subject_to(ocp.at_tf( (running_ek + ek)/(N*rms_error_traj**2) < 1 )) - + # Variant of the above constraint per sample #ocp.subject_to(ek < 0.001**2) # squared position error at each sample should be less than the specified tolerance rms_error_traj #ocp.subject_to(p_obj - p_obj_m < np.array((0.0001,0.0001,0.0001))) # squared position error at each sample should be less than the specified tolerance rms_error_traj @@ -118,7 +118,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # # total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error #total_ek = ocp.sum(ek,grid='control',include_last=True) - #ocp.add_objective(total_ek/N) + #ocp.add_objective(10e-8*total_ek/N) #ocp.subject_to(total_ek/N < rms_error_traj**2) #total_ek_scaled = running_ek/N/rms_error_traj**2 # scaled total error @@ -156,7 +156,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b """ Dummy values """ # Solve already once with dummy values so that Fatrop can do code generation (Q: can this step be avoided somehow?) ocp.set_initial(R_t, np.eye(3)) - ocp.set_initial(invars, np.array([1,0.2,0.2])) + ocp.set_initial(invars, np.array([1,0,0])) ocp.set_initial(p_obj, np.vstack((np.linspace(1, 2, N), np.ones((2, N))))) ocp.set_value(p_obj_m, np.vstack((np.linspace(1, 2, N), np.ones((2, N))))) ocp.set_value(h, 0.01) @@ -170,13 +170,13 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b ocp._method.set_option("tol",tolerance) ocp._method.set_option("print_level",print_level) ocp._method.set_option("max_iter",max_iter) - ocp._method.set_option("linsol_lu_fact_tol",1e-12) + #ocp._method.set_option("linsol_lu_fact_tol",1e-12) #ocp._method.set_option("linsol_perturbed_mode","no") - #ocp._method.set_option("mu_init",1e5) + ocp._method.set_option("mu_init",1e5) else: ocp.method(rockit.MultipleShooting(N=N-1)) - ocp.solver('ipopt', {'expand':True, 'print_time':False, 'ipopt.tol':tolerance, 'ipopt.print_info_string':'yes', 'ipopt.max_iter':max_iter, 'ipopt.print_level':print_level, 'ipopt.ma57_automatic_scaling':'no', 'ipopt.linear_solver':'mumps'}) + ocp.solver('ipopt', {'expand':True, 'print_time':False, 'ipopt.tol':tolerance, 'ipopt.print_info_string':'yes', 'ipopt.max_iter':max_iter, 'ipopt.print_level':print_level, 'ipopt.ma57_automatic_scaling':'no', 'ipopt.linear_solver':'mumps', 'ipopt.mu_init':1e5}) """ Encapsulate solver in a casadi function so that it can be easily reused """ ocp.solve() # code generation @@ -253,12 +253,18 @@ def calculate_invariants(self, measured_positions, stepsize, use_previous_soluti self.values_variables = self.ocp_function(measured_positions.T, stepsize, *self.values_variables) + + + # Return the results invars_sol, p_obj_sol, R_t_sol = self.values_variables # unpack the results invariants = np.array(invars_sol).T # make a N-1 x 3 array invariants = np.vstack((invariants, invariants[-1, :])) # make a N x 3 array by repeating last row calculated_trajectory = np.array(p_obj_sol).T # make a N x 3 array calculated_movingframe = np.transpose(np.reshape(R_t_sol.T, (-1, 3, 3)), (0, 2, 1)) # make a N x 3 x 3 array + + #ocp_helper.solution_check_pos(measured_positions,calculated_trajectory,rms = 1e-4) + return invariants, calculated_trajectory, calculated_movingframe def calculate_invariants_OLD(self, measured_positions, stepsize): @@ -295,14 +301,14 @@ def calculate_invariants_OLD(self, measured_positions, stepsize): self.ocp.set_initial(self.p_obj, measured_positions.T) # Initialize controls - self.ocp.set_initial(self.U, [1, 1e-1, 1e-12]) + self.ocp.set_initial(self.U, [1, 1e-1, 1e-10]) # Set values parameters self.ocp.set_value(self.p_obj_m, measured_positions.T) self.ocp.set_value(self.h, stepsize) # Solve the OCP - sol = self.ocp.solve_limited() + sol = self.ocp.solve() # Extract the solved variables invariants = np.array([sol.sample(self.U[i], grid='control')[1] for i in range(3)]).T @@ -316,33 +322,33 @@ def calculate_invariants_OLD(self, measured_positions, stepsize): # Example data for measured positions and stepsize N = 100 - t = np.linspace(0, 4, N) + t = np.linspace(0, 1, N) measured_positions = np.column_stack((1 * np.cos(t), 1 * np.sin(t), 0.1 * t)) stepsize = t[1]-t[0] rms_val = 10**-3 # Test the functionalities of the class - OCP = OCP_calc_pos(window_len=N, rms_error_traj=rms_val, fatrop_solver=True, solver_options={'max_iter': 100}) + OCP = OCP_calc_pos(window_len=N, rms_error_traj=rms_val, fatrop_solver=True, solver_options={'max_iter': 200}) # Call the calculate_invariants function and measure the elapsed time #start_time = time.time() - calc_invariants, calc_trajectory, calc_movingframes = OCP.calculate_invariants(measured_positions, stepsize) + calc_invariants, calc_trajectory, calc_movingframes = OCP.calculate_invariants_OLD(measured_positions, stepsize) #elapsed_time = time.time() - start_time ocp_helper.solution_check_pos(measured_positions,calc_trajectory,rms = rms_val) - # fig = plt.figure() - # ax = fig.add_subplot(111, projection='3d') - # ax.plot(measured_positions[:, 0], measured_positions[:, 1], measured_positions[:, 2],'b.-') - # ax.plot(calc_trajectory[:, 0], calc_trajectory[:, 1], calc_trajectory[:, 2],'r--') + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + ax.plot(measured_positions[:, 0], measured_positions[:, 1], measured_positions[:, 2],'b.-') + ax.plot(calc_trajectory[:, 0], calc_trajectory[:, 1], calc_trajectory[:, 2],'r--') # fig = plt.figure() # plt.plot(calc_invariants) # plt.show() - #plt.plot(calc_trajectory) - #plt.show() + # plt.plot(calc_trajectory) + # plt.show() # # Print the results and elapsed time # print("Calculated invariants:") # print(calc_invariants) diff --git a/invariants_py/dynamics_vector_invariants.py b/invariants_py/dynamics_vector_invariants.py index 281f1f7..42f5f38 100644 --- a/invariants_py/dynamics_vector_invariants.py +++ b/invariants_py/dynamics_vector_invariants.py @@ -206,6 +206,25 @@ def define_integrator_invariants_position(h): return integrator +def define_integrator_invariants_position_seq(h): + """Define a CasADi function that integrates the vector invariants for rotation over a time interval h.""" + + # System states + R_t = cas.MX.sym('R_t',3,3) # translational Frenet-Serret frame + p_obj = cas.MX.sym('p_obj',3,1) # object position + x = cas.vertcat(cas.vec(R_t), p_obj) + #np = length(R_obj(:)) + length(p_obj) + + # System controls (invariants) + u = cas.MX.sym('i',3,1) + + ## Integrate symbolically and create a casadi function with inputs/outputs + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, u, h) + out_plus1 = cas.vertcat(cas.vec(R_t_plus1), p_obj_plus1) + integrator = cas.Function("phi", [x,u,h] , [out_plus1]) + + return integrator + def define_integrator_invariants_position_jerkmodel(h): # System states diff --git a/invariants_py/ocp_initialization.py b/invariants_py/ocp_initialization.py index 5a7941a..17b3da0 100644 --- a/invariants_py/ocp_initialization.py +++ b/invariants_py/ocp_initialization.py @@ -204,7 +204,7 @@ def estimate_initial_frames(vector_traj): def initialize_VI_pos2(measured_positions,stepsize): N = np.size(measured_positions,0) - invariants, meas_pos, mf = calculate_discretized_invariants(measured_positions,stepsize) + invariants, meas_pos, mf = calculate_discretized_invariants(measured_positions,stepsize, tolerance_singularity_vel = 5*1e-1, tolerance_singularity_curv = 1e-2) #Pdiff = np.diff(measured_positions, axis=0) #Pdiff = np.vstack((Pdiff, Pdiff[-1])) @@ -224,11 +224,13 @@ def initialize_VI_pos2(measured_positions,stepsize): #print(R_t_init) - p_obj_sol = measured_positions.T + p_obj_init = measured_positions.T #invars = np.vstack((1e0*np.ones((1,N-1)),1e-1*np.ones((1,N-1)), 1e-12*np.ones((1,N-1)))) - return [invariants[:-1,:].T, p_obj_sol, R_t_init] + invariants_init = invariants[:-1,:].T+1e-10 # add small slack to prevent NaN + + return [invariants_init, p_obj_init, R_t_init] def initialize_VI_rot(input_trajectory): diff --git a/tests/test_compare_ipopt_fatrop_position.py b/tests/test_compare_ipopt_fatrop_position.py index 6e1c847..146b78a 100644 --- a/tests/test_compare_ipopt_fatrop_position.py +++ b/tests/test_compare_ipopt_fatrop_position.py @@ -7,7 +7,7 @@ class TestOCPcalcpos(unittest.TestCase): def setUp(self): # Example data for measured positions and stepsize N = 100 - t = np.linspace(0, 4, N) + t = np.linspace(0, 1, N) self.measured_positions = np.column_stack((1 * np.cos(t), 1 * np.sin(t), 0.1 * t)) self.stepsize = t[1] - t[0] From 8c716adc8606542b21c2d65274319c8016cf1254 Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Tue, 3 Dec 2024 16:17:36 +0100 Subject: [PATCH 10/11] sequential integrator appears to be problematic --- .../opti_calculate_vector_invariants_position.py | 2 +- .../rockit_calculate_vector_invariants_position.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py index bf0b5a6..9909948 100644 --- a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py @@ -40,7 +40,7 @@ def __init__(self, window_len = 100, opti.subject_to(ocp_helper.tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0) # Dynamics constraints (Multiple shooting) - integrator = dynamics.define_integrator_invariants_position_seq(h) + integrator = dynamics.define_integrator_invariants_position(h) for k in range(window_len-1): # Integrate current state to obtain next state (next rotation and position) Xk_end = integrator(X[k],U[:,k],h) diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py index 6fb4532..dc94d5d 100644 --- a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py +++ b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py @@ -66,7 +66,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b # System dynamics (integrate current states + controls to obtain next states) # this relates the states/controls over the whole window - (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position_seq(R_t, p_obj, invars, h) + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position(R_t, p_obj, invars, h) ocp.set_next(p_obj,p_obj_plus1) ocp.set_next(R_t,R_t_plus1) @@ -156,7 +156,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b """ Dummy values """ # Solve already once with dummy values so that Fatrop can do code generation (Q: can this step be avoided somehow?) ocp.set_initial(R_t, np.eye(3)) - ocp.set_initial(invars, np.array([1,0,0])) + ocp.set_initial(invars, np.array([1,10e-10,10e-10])) ocp.set_initial(p_obj, np.vstack((np.linspace(1, 2, N), np.ones((2, N))))) ocp.set_value(p_obj_m, np.vstack((np.linspace(1, 2, N), np.ones((2, N))))) ocp.set_value(h, 0.01) From 2e9d83d0fe81ecbb75167fdda964f07b80784d3e Mon Sep 17 00:00:00 2001 From: Maxim Vochten Date: Wed, 11 Dec 2024 13:01:26 +0100 Subject: [PATCH 11/11] updated actions/cache version from v2 to v4 (deprecated) --- .github/workflows/install_and_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/install_and_test.yml b/.github/workflows/install_and_test.yml index 6c8777c..8c083e3 100644 --- a/.github/workflows/install_and_test.yml +++ b/.github/workflows/install_and_test.yml @@ -24,7 +24,7 @@ jobs: python-version: '3.x' - name: Cache pip dependencies - uses: actions/cache@v2 + uses: actions/cache@v4 with: path: ~/.cache/pip key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }}