Merge branch 'main' of gitlab.kuleuven.be:robotgenskill/public_code/i…

…nvariants_py into main

.gitlab-ci.yml

@@ -11,9 +11,13 @@ before_script:
   - python -V  # Print out python version for debugging
 
 run_examples:
-  except:
-    changes:
-      - "**/*.md"  # do not run a pipeline if only markdown files are changed
+  rules:
+    - changes:
+        - "**/*.md"
+      when: never
+    - changes:
+        - "**/*"
+      when: always
   script:
     # install invariants_py
     - pip install --upgrade pip
@@ -30,8 +34,10 @@ run_examples:
     - cd ../..
 
     # install rockit and rockit-fatrop plugin
-    - git clone https://gitlab.kuleuven.be/meco-software/rockit.git
-    - git clone https://gitlab.kuleuven.be/u0110259/rockit_fatrop_plugin.git ./rockit/rockit/external/fatrop
+    - git clone -b acados_codegen https://gitlab.kuleuven.be/meco-software/rockit.git
+    - git clone -b fatropy https://gitlab.kuleuven.be/u0110259/rockit_fatrop_plugin.git ./rockit/rockit/external/fatrop
+    #- git clone https://gitlab.kuleuven.be/meco-software/rockit.git
+    #- git clone https://gitlab.kuleuven.be/u0110259/rockit_fatrop_plugin.git ./rockit/rockit/external/fatrop
     - cd rockit
     - pip install .
     - cd ..

invariants_py/calculate_invariants/rockit_calculate_vector_invariants_rotation.py

@@ -59,14 +59,15 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, rms_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))
+        #ocp.subject_to(ocp.at_tf(10e4*running_ek/N < 10e4*rms_error_traj**2))
 
-        # 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 = 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(total_ek_scaled < 1)
+        #total_ek_scaled = total_ek/N/rms_error_traj**2 # scaled total error
+        ocp.subject_to(total_ek/N < rms_error_traj**2) # scaled total error
+        #ocp.subject_to(total_ek_scaled < 1)
 
         # opti.subject_to(U[1,-1] == U[1,-2]); # Last sample has no impact on RMS error ##### HOW TO ACCESS U[1,-2] IN ROCKIT
 
@@ -158,8 +159,9 @@ def calculate_invariants(self,measured_rotations,stepsize):
 
     # Specify OCP symbolically
     N = np.size(measured_orientations,0)
-    OCP = OCP_calc_rot(window_len=N,fatrop_solver=False, rms_error_traj=10*pi/180)
+    OCP = OCP_calc_rot(window_len=N,fatrop_solver=True, 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)
-    print(calc_invariants)
+    print(calc_invariants)
+

invariants_py/ocp_helper.py

@@ -46,18 +46,18 @@ def solution_check_pos(p_obj_m,p_obj,rms = 10**-2):
     for i in range(N):
         ek = cas.dot(p_obj[i] - p_obj_m[i],p_obj[i] - p_obj_m[i])
         tot_ek += ek
-        if tot_ek > N*rms**2:
-            print("")
-            print("Value of error is" , np.sqrt(tot_ek/N), "and should be less than", rms)
-            raise Exception("The constraint is not satisfied! Something is wrong in the calculation")           
+    if tot_ek > N*rms**2:
+        print("")
+        print("Value of error is" , np.sqrt(tot_ek/N), "and should be less than", rms)
+        raise Exception("The constraint is not satisfied! Something is wrong in the calculation")        
 
 def solution_check_rot(R_obj_m,R_obj,rms = 4*np.pi/180):
     N = R_obj.shape[0]
     tot_ek = 0
-    for i in range(N):
+    for i in range(N-1):
         ek = cas.dot(R_obj_m[i].T @ R_obj[i] - np.eye(3),R_obj_m[i].T @ R_obj[i] - np.eye(3))
         tot_ek +=ek
-        if tot_ek > N*rms**2:
-            print("")
-            print("Value of error is" , np.sqrt(tot_ek/N), "and should be less than", rms)
-            raise Exception("The constraint is not satisfied! Something is wrong in the calculation")        
+    if tot_ek/N > rms**2:
+        print("")
+        print("Value of error is" , np.sqrt(tot_ek/N), "and should be less than", rms)
+        raise Exception("The constraint is not satisfied! Something is wrong in the calculation")

invariants_py/plotting_functions/plotters.py

@@ -618,7 +618,8 @@ def plot_moving_frames(calc_trajectory, movingframes, length=0.075, skip_frames=
     ax.set_ylabel('Y [m]')
     ax.set_zlabel('Z [m]')
     plt.title('Trajectory and moving frames')
-    plt.show()
+    if plt.get_backend() != 'agg':
+        plt.show()
 
 # Animate the moving frames
 def animate_moving_frames(calc_trajectory, movingframes, length=0.075, skip_frames=5, save_animation=False, filename='animation.gif'):
@@ -653,4 +654,5 @@ def update_frame(frame):
     ani = animation.FuncAnimation(fig, update_frame, frames=len(calc_trajectory), interval=100, repeat=True)
     if save_animation:
         ani.save(filename, writer='imagemagick', fps=60)
-    plt.show()
+    if plt.get_backend() != 'agg':
+        plt.show()