test

invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py

@@ -22,11 +22,12 @@
 import rockit
 from invariants_py import ocp_helper, ocp_initialization
 from invariants_py.ocp_helper import check_solver
-from invariants_py.dynamics_vector_invariants import integrate_vector_invariants_position
+from invariants_py.dynamics_vector_invariants import integrate_vector_invariants_position, integrate_vector_invariants_position_seq
+from invariants_py import discretized_vector_invariants as dvi
 
 class OCP_calc_pos:
 
-    def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, bool_unsigned_invariants=False, planar_task=False, solver_options = {}, geometric=False):
+    def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, bool_unsigned_invariants=False, planar_task=False, solver_options = {}, geometric=False, periodic=False):
         """
         Initializes an instance of the RockitCalculateVectorInvariantsPosition class.
         It specifies the optimal control problem (OCP) for calculating the invariants of a trajectory in a symbolic way.
@@ -65,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)
 
@@ -76,28 +77,38 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b
 
         # Lower bounds on controls
         if bool_unsigned_invariants:
-            ocp.subject_to(invars[0,:]>=0) # velocity always positive
-            #ocp.subject_to(invars[1,:]>=0) # curvature rate always positive
+            ocp.subject_to(invars[0]>=0) # velocity always positive
+            #ocp.subject_to(invars[1]>=0) # curvature rate always positive
+
+        #ocp.subject_to(-np.pi <= invars[1,:]*h) # curvature rate bounded by 1
+        #ocp.subject_to(-np.pi <= invars[2,:]*h) # curvature rate bounded by 1
+        #ocp.subject_to(invars[1,:]*h <= np.pi) # curvature rate bounded by 1
+        #ocp.subject_to(invars[2,:]*h <= np.pi) # curvature rate bounded by 1
 
         # Measurement fitting constraint
         # TODO: what about specifying the tolerance per sample instead of the sum?
         ek = cas.dot(p_obj - p_obj_m, p_obj - p_obj_m) # squared position error
         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)
+            total_ek = ocp.sum(ek,grid='control',include_last=True)          
             ocp.subject_to(total_ek < (N*rms_error_traj**2))
         else:
             # Fatrop does not support summing over grid points inside a constraint, so we implement the sum using a running cost to achieve the same result as above
             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 ))
+
+            #ocp.subject_to(ek < rms_error_traj**2) # squared position error should be less than the specified tolerance rms_error_traj
 
-            # TODO this is still needed because last sample is not included in the sum now
-            #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
+        #     # TODO this is still needed because last sample is not included in the sum now
+        #     #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
+
+        total_ek = ocp.sum(ek,grid='control',include_last=True)
+        ocp.add_objective(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
@@ -118,6 +129,12 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b
             L = ocp.state()  # introduce extra state L for the speed
             ocp.set_next(L, L)  # enforce constant speed
             ocp.subject_to(invars[0] - L == 0, include_last=False)  # relate to first invariant
+
+        #if periodic:
+        #    ocp.subject_to(ocp.at_tf(p_obj) - ocp.at_t0(p_obj) == 0)
+        #     # Periodic boundary conditions (optional)   
+        #     ocp.subject_to(ocp.at_tf(R_t) == ocp.at_t0(R_t))
+
 
         """ Objective function """
 
@@ -144,8 +161,8 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b
             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-6)
-            ocp._method.set_option("linsol_perturbed_mode","no")
-            ocp._method.set_option("mu_init",1e5)
+            #ocp._method.set_option("linsol_perturbed_mode","no")
+            ocp._method.set_option("mu_init",1e2)
 
         else:
             ocp.method(rockit.MultipleShooting(N=N-1))
@@ -222,7 +239,7 @@ def calculate_invariants(self, measured_positions, stepsize, use_previous_soluti
 
         # print(measured_positions.T)
         # print(stepsize)
-        # print(*self.values_variables)
+        # print(*self.values_variables)False
 
         self.values_variables = self.ocp_function(measured_positions.T, stepsize, *self.values_variables)
 

invariants_py/discretized_vector_invariants.py

@@ -141,15 +141,15 @@ def calculate_binormal(vector_traj,tangent, tolerance_singularity, reference_vec
 
     return binormal
 
-def calculate_moving_frames(vector_traj, tolerance_singularity = 1e-2):
+def calculate_moving_frames(vector_traj, tolerance_singularity_vel = 1e-2, tolerance_singularity_curv = 1e-2):
 
     N = np.size(vector_traj, 0)
 
     # Calculate first axis
-    e_tangent = calculate_tangent(vector_traj, tolerance_singularity)
+    e_tangent = calculate_tangent(vector_traj, tolerance_singularity_vel)
 
     # Calculate third axis
-    e_binormal = calculate_binormal(vector_traj,e_tangent,tolerance_singularity,reference_vector=np.array([0,0,1]))
+    e_binormal = calculate_binormal(vector_traj,e_tangent,tolerance_singularity_curv,reference_vector=np.array([0,0,1]))
 
     # Calculate second axis as cross product of third and first axis
     e_normal = np.array([ np.cross(e_binormal[i,:],e_tangent[i,:]) for i in range(N) ])
@@ -214,7 +214,7 @@ def calculate_vector_invariants(R_mf_traj,vector_traj,progress_step):
 
     return invariants
 
-def calculate_discretized_invariants(measured_positions, progress, tolerance_singularity = 1e0):
+def calculate_discretized_invariants(measured_positions, progress_step, tolerance_singularity_vel = 1e-1, tolerance_singularity_curv = 1e-2):
     """
     Calculate the vector invariants of a measured position trajectory 
     based on a discrete approximation of the moving frame.
@@ -228,15 +228,15 @@ def calculate_discretized_invariants(measured_positions, progress, tolerance_sin
         mf: moving frame (Nx3x3)
     """
 
-    progress_step = np.mean(np.diff(progress))#.reshape(-1,1)
+    #progress_step = np.mean(np.diff(progress))#.reshape(-1,1)
 
     # Calculate velocity vector in a discretized way
     pos_vector_differences = np.diff(measured_positions,axis=0)
     vel_vector = pos_vector_differences / progress_step
     #print(vel_vector)
 
     # Calculate the moving frame
-    R_mf_traj = calculate_moving_frames(vel_vector, tolerance_singularity)
+    R_mf_traj = calculate_moving_frames(vel_vector, tolerance_singularity_vel, tolerance_singularity_curv)
     #print(R_mf_traj)
 
     # Calculate the invariants

invariants_py/ocp_initialization.py

@@ -1,7 +1,7 @@
 import numpy as np
 import invariants_py.kinematics.orientation_kinematics as SO3
 from invariants_py.reparameterization import interpR
-from invariants_py.discretized_vector_invariants import calculate_vector_invariants, calculate_velocity_from_discrete_rotations
+from invariants_py.discretized_vector_invariants import calculate_discretized_invariants, calculate_velocity_from_discrete_rotations
 
 
 def initial_trajectory_movingframe_rotation(R_obj_start,R_obj_end,N=100):
@@ -204,26 +204,28 @@ def estimate_initial_frames(vector_traj):
 def  initialize_VI_pos2(measured_positions,stepsize):
 
     N = np.size(measured_positions,0)
-    Pdiff = np.diff(measured_positions, axis=0)
-    Pdiff = np.vstack((Pdiff, Pdiff[-1]))
+    invariants, meas_pos, mf = calculate_discretized_invariants(measured_positions,stepsize)
+
+    #Pdiff = np.diff(measured_positions, axis=0)
+    #Pdiff = np.vstack((Pdiff, Pdiff[-1]))
 
-    [ex,ey,ez] = estimate_initial_frames(Pdiff)
+    #[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,:]))
+    #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 = calculate_vector_invariants(R_t_init2,Pdiff,stepsize) + 1e-12*np.ones((N,3))
+    #invars = calculate_vector_invariants(R_t_init2,Pdiff,stepsize) + 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,:]])   
+        R_t_init[:,i] = np.hstack([mf[i,0],mf[i,1],mf[i,2]])   
 
-    p_obj_sol =  measured_positions.T 
+    p_obj_sol =  meas_pos.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[:-1,:].T, p_obj_sol, R_t_init]
+    return [invariants[:-1,:].T, p_obj_sol, R_t_init]
 
 def initialize_VI_rot(input_trajectory):