Merge branch 'optistack-fatrop'

.gitignore

@@ -16,5 +16,5 @@ __pycache__/
 casadi_codegen.obj
 invariants_py/data/sine_wave_invariants.csv
 venv/
-debug_fatrop_*
+debug_fatrop*
 nlp_hess_l.casadi

examples/calculate_screw_invariants_pose.py

@@ -4,7 +4,7 @@
 from invariants_py import data_handler as dh
 import numpy as np
 from invariants_py.reparameterization import interpT
-from invariants_py.calculate_invariants.opti_calculate_screw_invariants_pose import OCP_calc_pose
+from invariants_py.calculate_invariants.opti_calculate_screw_invariants_pose_fatrop import OCP_calc_pose
 
 import matplotlib.pyplot as plt
 from scipy.spatial.transform import Rotation as R
@@ -147,33 +147,28 @@ def main():
 
     # Compute the number of new samples
     N = int(1 + np.floor(timestamps[-1] / dt))
+    print(f'Number of samples: {N}')
 
     # Generate new equidistant time vector
     time_new = np.linspace(0, timestamps[-1], N)
 
     # Interpolate pose matrices to new time vector
-    T = interpT(timestamps, T, time_new)
+    T = interpT(time_new, timestamps, T)
 
     # Plot the input trajectory
     plot_trajectory_kettle(T, 'Input Trajectory')
 
     # Initialize OCP object and calculate pose
-    OCP = OCP_calc_pose(T, rms_error_traj=5 * 10**-2)
+    OCP = OCP_calc_pose(N, rms_error_traj_pos = 1e-3, rms_error_traj_rot= 1e-2, solver='fatrop')
 
     # Calculate screw invariants and other outputs
-    U, T_sol_, T_isa_ = OCP.calculate_invariants(T, dt)
+    U, T_sol, T_isa = OCP.calculate_invariants(T, dt)
 
-    # Initialize an array for the solution trajectory
-    T_sol = np.zeros((N, 4, 4))
-    for k in range(N):
-        T_sol[k, :3, :] = T_sol_[k]
-        T_sol[k, 3, 3] = 1
-
     # Plot the reconstructed trajectory
     plot_trajectory_kettle(T_sol, 'Reconstructed Trajectory')
 
     # Plot the screw invariants
-    plot_screw_invariants(time_new[:-1], U.T)
+    plot_screw_invariants(time_new, U)
 
     # Display the plots if not running in a non-interactive backend
     if plt.get_backend() != 'agg':

invariants_py/calculate_invariants/opti_calculate_screw_invariants_pose.py

@@ -2,13 +2,21 @@
 import casadi as cas
 from invariants_py.dynamics_screw_invariants import define_integrator_invariants_pose
 from invariants_py import ocp_helper
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+def form_homogeneous_matrix(T):
+    T_hom = np.eye(4)
+    T_hom[:3, :] = T
+    return T_hom
 
 class OCP_calc_pose:    
 
-    def __init__(self, T_input, bool_unsigned_invariants = False, rms_error_traj = 10**-2):
-
-        N = T_input.shape[0]
-
+    def __init__(self, N, 
+                rms_error_traj_pos = 10**-3,
+                rms_error_traj_rot = 10**-2,
+                bool_unsigned_invariants = False):
+
         opti = cas.Opti() # use OptiStack package from Casadi for easy bookkeeping of variables (no cumbersome indexing)
 
         # Define system states X (unknown object pose + moving frame pose at every time step)
@@ -28,13 +36,13 @@ def __init__(self, T_input, bool_unsigned_invariants = False, rms_error_traj = 1
         opti.subject_to(ocp_helper.tril_vec(T_obj[0][0:3,0:3].T @ T_obj[0][0:3,0:3] - np.eye(3)) == 0)
 
         # Dynamics constraints (Multiple shooting)
-        integrator = define_integrator_invariants_pose(h)
+        geometric_integrator = define_integrator_invariants_pose(h)
         for k in range(N-1):
             # Integrate current state to obtain next state (next rotation and position)
-            Xk_end = integrator(X[k],U[:,k],h)
+            Xk_end = geometric_integrator(X[k],U[:,k],h)
 
             # Continuity constraint (closing the gap)
-            opti.subject_to(Xk_end==X[k+1])
+            opti.subject_to(X[k+1] == Xk_end)
 
         # Lower bounds on controls
         if bool_unsigned_invariants:
@@ -46,11 +54,13 @@ def __init__(self, T_input, bool_unsigned_invariants = False, rms_error_traj = 1
         trajectory_error_rot = 0
         for k in range(N):
             err_pos = T_obj[k][0:3,3] - T_obj_m[k][0:3,3] # position error
-            err_rot = T_obj_m[k][0:3,0:3].T @ T_obj[k][0:3,0:3] - np.eye(3) # orientation error
+            err_rot = (T_obj[k][0:3,0:3].T @ T_obj_m[k][0:3,0:3]) - np.eye(3) # orientation error
             trajectory_error_pos = trajectory_error_pos + cas.dot(err_pos,err_pos)
             trajectory_error_rot = trajectory_error_rot + cas.dot(err_rot,err_rot)  
-        opti.subject_to(trajectory_error_pos/N/rms_error_traj**2 < 1)
-        opti.subject_to(trajectory_error_rot/N/rms_error_traj**2 < 1)
+        opti.subject_to(trajectory_error_pos < N*rms_error_traj_pos**2)
+        opti.subject_to(trajectory_error_rot < N*rms_error_traj_rot**2)
+
+        #objective_fit = trajectory_error_pos + trajectory_error_rot
 
         # Minimize moving frame invariants to deal with singularities and noise
         objective_reg = 0
@@ -59,9 +69,11 @@ def __init__(self, T_input, bool_unsigned_invariants = False, rms_error_traj = 1
             objective_reg = objective_reg + cas.dot(err_abs,err_abs) # cost term
         objective = objective_reg/(N-1) # normalize with window length
 
+        #objective = 1e-8*objective + objective_fit
+
         # 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','print_info_string':'yes'})
+        opti.solver('ipopt',{"print_time":True,"expand":True},{'max_iter':300,'tol':1e-6,'print_level':5,'ma57_automatic_scaling':'no','linear_solver':'mumps','print_info_string':'yes'}) # 'gamma_theta':1e-12
 
         # Store variables
         self.opti = opti
@@ -80,7 +92,6 @@ def calculate_invariants(self, T_obj_m, h):
         T_obj_init = T_obj_m
         T_isa_init0 = np.vstack([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]).T
         T_isa_init = np.tile(T_isa_init0, (self.N, 1, 1)) # initial guess for moving frame poses
-
 
         # Set initial guess
         for i in range(self.N):
@@ -90,29 +101,85 @@ def calculate_invariants(self, T_obj_m, h):
         self.opti.set_value(self.h, h)
 
         for i in range(self.N-1):
-            self.opti.set_initial(self.U[:,i], np.zeros(6)+0.001*np.random.randn(6))
+            self.opti.set_initial(self.U[:,i], np.zeros(6)+0.1)
 
         # Solve
         sol = self.opti.solve()
 
         # Return solution
-        T_isa = [sol.value(self.T_isa[k]) for k in range(self.N)]
-        T_obj = [sol.value(self.T_obj[k]) for k in range(self.N)]
-        U = sol.value(self.U)
+        T_isa = np.array([form_homogeneous_matrix(sol.value(i)) for i in self.T_isa])
+        T_obj = np.array([form_homogeneous_matrix(sol.value(i)) for i in self.T_obj])
+        U = np.array(sol.value(self.U))
+        U = np.append(U, [U[-1, :]], axis=0)
 
         return U, T_obj, T_isa
-
-
+
 if __name__ == "__main__":
 
-    N=100
-    T_obj_m = np.tile(np.eye(4), (100, 1, 1)) # example: measured object poses
+    from invariants_py.reparameterization import interpT
+    import invariants_py.kinematics.orientation_kinematics as SE3
+
+    # Test data    
+    N = 100
+    T_start = np.eye(4)  # Rotation matrix 1
+    T_mid = np.eye(4)
+    T_mid[:3, :3] = SE3.rotate_z(np.pi)  # Rotation matrix 3
+    T_end = np.eye(4)
+    T_end[:3, :3] = SE3.RPY(np.pi/2, 0, np.pi/2)  # Rotation matrix 2
+
+    # Interpolate between R_start and R_end
+    T_obj_m = interpT(np.linspace(0,1,N), np.array([0,0.5,1]), np.stack([T_start, T_mid, T_end],0))
+
+    print(T_obj_m)
+    # check orthonormality of rotation matrix part of T_obj_m
+    # for i in range(N):
+    #     print(np.linalg.norm(T_obj_m[i,0:3,0:3].T @ T_obj_m[i,0:3,0:3] - np.eye(3)))
+
 
-    OCP = OCP_calc_pose(T_obj_m, rms_error_traj=10**-3)
+    OCP = OCP_calc_pose(N, rms_error_traj_pos = 10e-3, rms_error_traj_rot = 10e-3, bool_unsigned_invariants=True)
 
     # Example: calculate invariants for a given trajectory
     h = 0.01 # step size for integration of dynamic equations
-
     U, T_obj, T_isa = OCP.calculate_invariants(T_obj_m, h)
 
     print("Invariants U: ", U)
+    print("T_obj: ", T_obj)
+    print("T_isa: ", T_isa)
+
+    # Assuming T_isa is already defined and is an Nx4x4 matrix
+    N = T_isa.shape[0]
+
+    # Extract points and directions
+    points = T_isa[:, :3, 3]  # First three elements of the fourth column
+    directions = T_isa[:, :3, 0]  # First three elements of the first column
+
+    # Create a 3D plot
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection='3d')
+
+    # Plot the points
+    ax.scatter(points[:, 0], points[:, 1], points[:, 2], color='r', label='Points on the line')
+
+    # Plot the directions as lines
+    for i in range(N):
+        start_point = points[i] - directions[i] * 0.1 
+        end_point = points[i] + directions[i] * 0.1  # Scale the direction for better visualization
+        ax.plot([start_point[0], end_point[0]], [start_point[1], end_point[1]], [start_point[2], end_point[2]], color='b')
+
+    # Set axis limits
+    ax.set_xlim([-0.1, +0.1])
+    ax.set_ylim([-0.1, +0.1])
+    ax.set_zlim([-0.1, +0.1])
+
+    # Set labels
+    ax.set_xlabel('X')
+    ax.set_ylabel('Y')
+    ax.set_zlabel('Z')
+    ax.set_title('Plotting the instantaneous screw axis')
+
+    # Add legend
+    ax.legend()
+
+    # Show plot
+    plt.show()
+