sequential integrator for opti implementations

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

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)

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

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

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]