Trying to fix structure for rotational traj generation OCP with fatro…

…p, does not work yet!

examples/trajectory_generation/COMPARING trajectory_generation_rotation.py

@@ -139,8 +139,8 @@ def interpolate_model_invariants(demo_invariants, progress_values):
 
 # specify optimization problem symbolically
 FS_online_generation_problem = OCP_gen_rot(window_len=number_samples,w_invars = weights['w_invars'])
-fatrop_FS_online_generation_problem = fatrop_OCP_gen_rot(boundary_constraints,number_samples,solver='ipopt')
-rockit_FS_online_generation_problem = rockit_OCP_gen_rot(boundary_constraints,number_samples,fatrop_solver=False)
+fatrop_FS_online_generation_problem = fatrop_OCP_gen_rot(boundary_constraints,number_samples,solver='fatrop')
+rockit_FS_online_generation_problem = rockit_OCP_gen_rot(boundary_constraints,number_samples,fatrop_solver=True)
 
 # Solve
 new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(U_demo = model_invariants, R_obj_init = calculate_trajectory, R_r_init = movingframes, R_r_start = R_r_start, R_r_end = R_r_end, R_obj_start = R_obj_start, R_obj_end = R_obj_end, step_size = new_stepsize)

invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars_fatrop.py

@@ -24,8 +24,8 @@ def __init__(self, boundary_constraints, window_len = 100, bool_unsigned_invaria
             R_obj.append(opti.variable(3,3)) # object orientation
             R_r.append(opti.variable(3,3)) # rotational Frenet-Serret frame
             X.append(cas.vertcat(cas.vec(R_r[k]), cas.vec(R_obj[k])))
-        for k in range(window_len-1):
-            invars.append(opti.variable(3,1))
+            if k < window_len-1:
+                invars.append(opti.variable(3,1)) # invariants
 
         # Define system parameters P (known values in optimization that need to be set right before solving)
         h = opti.parameter(1,1) # step size for integration of dynamic model
@@ -48,19 +48,7 @@ def __init__(self, boundary_constraints, window_len = 100, bool_unsigned_invaria
 
         ''' Specifying the constraints '''
 
-        # Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator)
-        opti.subject_to(tril_vec(R_obj[0].T @ R_obj[0] - np.eye(3)) == 0)
-        opti.subject_to(tril_vec(R_r[0].T @ R_r[0] - np.eye(3)) == 0)
 
-        # Boundary constraints
-        if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]:
-            opti.subject_to(tril_vec_no_diag(R_obj[0].T @ R_obj_start - np.eye(3)) == 0.)
-        if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]:
-            opti.subject_to(tril_vec_no_diag(R_obj[-1].T @ R_obj_end - np.eye(3)) == 0.)
-        if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]:
-            opti.subject_to(tril_vec_no_diag(R_r[0].T @ R_r_start - np.eye(3)) == 0.)
-        if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]:
-            opti.subject_to(tril_vec_no_diag(R_r[-1].T @ R_r_end - np.eye(3)) == 0.)
 
         # Dynamic constraints
         integrator = dynamics.define_integrator_invariants_rotation(h)
@@ -69,8 +57,23 @@ def __init__(self, boundary_constraints, window_len = 100, bool_unsigned_invaria
             Xk_end = integrator(X[k],invars[k],h)
 
             # Gap closing constraint
-            opti.subject_to(Xk_end==X[k+1])
+            opti.subject_to(X[k+1]==Xk_end)
+
+            if k == 0:
+                # Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator)
+                opti.subject_to(tril_vec(R_obj[0].T @ R_obj[0] - np.eye(3)) == 0)
+                opti.subject_to(tril_vec(R_r[0].T @ R_r[0] - np.eye(3)) == 0)
+        # Boundary constraints
+                if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]:
+                    opti.subject_to(tril_vec_no_diag(R_obj[0].T @ R_obj_start - np.eye(3)) == 0.)
+                if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]:
+                    opti.subject_to(tril_vec_no_diag(R_r[0].T @ R_r_start - np.eye(3)) == 0.)
 
+        if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]:
+            opti.subject_to(tril_vec_no_diag(R_obj[-1].T @ R_obj_end - np.eye(3)) == 0.)
+        if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]:
+            opti.subject_to(tril_vec_no_diag(R_r[-1].T @ R_r_end - np.eye(3)) == 0.)
+
         ''' Specifying the objective '''
 
         # Fitting constraint to remain close to measurements
@@ -239,7 +242,7 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,
     step_size = 0.1
 
     # Create an instance of OCP_gen_pos
-    ocp = OCP_gen_rot(boundary_constraints,solver='ipopt', window_len=N)
+    ocp = OCP_gen_rot(boundary_constraints,solver='fatrop', window_len=N)
 
     # Call the generate_trajectory function
     invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size)