Merge branch 'initialization_position' into 'main'

add info string everywhere See merge request robotgenskill/public_code/invariants_py!33
trajectory-invariants · Jul 17, 2024 · 8275d59 · 8275d59
2 parents 5faa4d6 + f184154
commit 8275d59
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 8 deletions.
diff --git a/invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose.py b/invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose.py
@@ -61,7 +61,7 @@ def __init__(self, T_input, bool_unsigned_invariants = False, rms_error_traj = 1
 
         # Solver
         opti.minimize(objective)
-        opti.solver('ipopt',{"print_time":True,"expand":True},{'gamma_theta':1e-12,'max_iter':200,'tol':1e-4,'print_level':5,'ma57_automatic_scaling':'no','linear_solver':'mumps'})
+        opti.solver('ipopt',{"print_time":True,"expand":True},{'gamma_theta':1e-12,'max_iter':200,'tol':1e-4,'print_level':5,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes'})
 
         # Store variables
         self.opti = opti

diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position.py
@@ -86,7 +86,7 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, rms_error
         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'})
+            'max_iter':max_iter,'tol':tolerance,'print_level':print_level,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes'})
 
         # Save variables
         self.R_t = R_t

diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position_mf.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position_mf.py
@@ -82,7 +82,7 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, w_pos = 1
 
         ''' Define solver and save variables '''
         opti.minimize(objective)
-        opti.solver('ipopt',{"print_time":True,"expand":True},{'max_iter':1000,'tol':1e-4,'print_level':0,'ma57_automatic_scaling':'no','linear_solver':'mumps'})
+        opti.solver('ipopt',{"print_time":True,"expand":True},{'max_iter':1000,'tol':1e-4,'print_level':5,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes'})
 
         # Save variables
         self.R_t = R_t

diff --git a/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position_mj.py b/invariants_py/calculate_invariants/opti_calculate_vector_invariants_position_mj.py
@@ -95,7 +95,7 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, w_pos = 1
 
         ''' Define solver and save variables '''
         opti.minimize(objective)
-        opti.solver('ipopt',{"print_time":True,"expand":False},{'max_iter':200,'tol':1e-8,'print_level':0,'ma57_automatic_scaling':'no','linear_solver':'mumps'})
+        opti.solver('ipopt',{"print_time":True,"expand":False},{'max_iter':200,'tol':1e-8,'print_level':0,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes'})
 
         # Set variables as attributes in clas
         first_window = True

diff --git a/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_rotation.py b/invariants_py/calculate_invariants/rockit_calculate_vector_invariants_rotation.py
@@ -93,7 +93,7 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, rms_error
         else:
             ocp.method(rockit.MultipleShooting(N=N-1))
             #ocp.solver('ipopt', {'expand':True})
-            ocp.solver('ipopt',{"print_time":True,"expand":False,'ipopt.gamma_theta':1e-12,'ipopt.max_iter':max_iter,'ipopt.tol':tolerance,'ipopt.print_level':print_level,'ipopt.ma57_automatic_scaling':'no','ipopt.linear_solver':'mumps'})
+            ocp.solver('ipopt',{"print_time":True,"expand":False,'ipopt.gamma_theta':1e-12,'ipopt.print_info_string':'yes','ipopt.max_iter':max_iter,'ipopt.tol':tolerance,'ipopt.print_level':print_level,'ipopt.ma57_automatic_scaling':'no','ipopt.linear_solver':'mumps'})
 
         # Solve already once with dummy values for code generation (TODO: can this step be avoided somehow?)
         ocp.set_initial(R_r, np.eye(3))
@@ -168,7 +168,7 @@ def calculate_invariants(self,R_meas,stepsize):
     timestep = 0.001
 
     # Specify OCP symbolically
-    OCP = OCP_calc_rot(window_len=N, fatrop_solver=True, rms_error_traj=1*pi/180)
+    OCP = OCP_calc_rot(window_len=N, fatrop_solver=False, rms_error_traj=1*pi/180)
 
     # Solve the OCP using the specified data
     calc_invariants, calc_trajectory, calc_movingframes = OCP.calculate_invariants(measured_orientations, timestep)

diff --git a/invariants_py/initialization.py b/invariants_py/initialization.py
@@ -337,13 +337,21 @@ def  initialize_VI_pos2(measured_positions):
 
     [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,:]))
+    print(R_t_init2)
+    invars = estimate_rotational_invariants(R_t_init2,Pdiff) + 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,:]])   
 
     p_obj_sol =  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 [invars, p_obj_sol, R_t_init]
+    #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]
 
 def initialize_VI_rot(input_trajectory):