Fixed small error in opti position generation; generated file to comp…

…are results of different OCP specifications

examples/trajectory_generation/COMPARING trajectory_generation_position.py

@@ -0,0 +1,162 @@
+
+# Imports
+import numpy as np
+import os
+import invariants_py.data_handler as dh
+import matplotlib.pyplot as plt
+import invariants_py.reparameterization as reparam
+import scipy.interpolate as ip
+from invariants_py.calculate_invariants.opti_calculate_vector_invariants_position import OCP_calc_pos
+from invariants_py.generate_trajectory.opti_generate_position_traj_from_vector_invars import OCP_gen_pos
+from invariants_py.generate_trajectory.opti_generate_position_traj_from_vector_invars_fatrop import OCP_gen_pos as fatrop_OCP_gen_pos
+from invariants_py.generate_trajectory.rockit_generate_position_traj_from_vector_invars import OCP_gen_pos as rockit_OCP_gen_pos
+from IPython.display import clear_output
+
+data_location = dh.find_data_path('beer_1.txt')
+trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt')
+pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
+arclength_n = arclength/arclength[-1]
+trajectory = pose[:,0:3,3]
+
+fig = plt.figure(figsize=(8,8))
+ax = fig.add_subplot(111, projection='3d')
+ax = plt.axes(projection='3d')
+ax.plot(trajectory[:,0],trajectory[:,1],trajectory[:,2],'.-')
+
+#%%
+# specify optimization problem symbolically
+FS_calculation_problem = OCP_calc_pos(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 0.001)
+
+# calculate invariants given measurements
+invariants, calculate_trajectory, movingframes = FS_calculation_problem.calculate_invariants(trajectory,stepsize)
+
+init_vals_calculate_trajectory = calculate_trajectory
+init_vals_movingframes = movingframes
+
+fig = plt.figure(figsize=(8,8))
+ax = fig.add_subplot(111, projection='3d')
+ax = plt.axes(projection='3d')
+ax.plot(trajectory[:,0],trajectory[:,1],trajectory[:,2],'.-')
+ax.plot(calculate_trajectory[:,0],calculate_trajectory[:,1],calculate_trajectory[:,2],'.-')
+
+f, (ax1, ax2, ax3) = plt.subplots(1, 3, sharey=True, figsize=(10,3))
+ax1.plot(arclength_n,invariants[:,0])
+ax1.set_title('Velocity [m/m]')
+ax2.plot(arclength_n,invariants[:,1])
+ax2.set_title('Curvature [rad/m]')
+ax3.plot(arclength_n,invariants[:,2])
+ax3.set_title('Torsion [rad/m]')
+
+#%%
+# Spline of model
+knots = np.concatenate(([arclength_n[0]],[arclength_n[0]],arclength_n,[arclength_n[-1]],[arclength_n[-1]]))
+degree = 3
+spline_model_trajectory = ip.BSpline(knots,invariants,degree)
+
+def interpolate_model_invariants(demo_invariants, progress_values):
+
+    resampled_invariants = np.array([demo_invariants(i) for i in progress_values]) 
+    new_stepsize = progress_values[1] - progress_values[0] 
+
+    resampled_invariants[:,0] = resampled_invariants[:,0] *  (progress_values[-1] - progress_values[0])
+    return resampled_invariants, new_stepsize
+
+#%% 
+current_progress = 0
+number_samples = 100
+calculate_trajectory = init_vals_calculate_trajectory
+movingframes = init_vals_movingframes
+
+progress_values = np.linspace(current_progress, arclength_n[-1], number_samples)
+model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values)
+
+f, (ax1, ax2, ax3) = plt.subplots(1, 3, sharey=True, figsize=(10,3))
+ax1.plot(arclength_n,invariants[:,0])
+ax1.plot(progress_values,model_invariants[:,0],'r.')
+ax1.set_title('Velocity [m/m]')
+ax2.plot(arclength_n,invariants[:,1])
+ax2.plot(progress_values,model_invariants[:,1],'r.')
+ax2.set_title('Curvature [rad/m]')
+ax3.plot(arclength_n,invariants[:,2])
+ax3.plot(progress_values,model_invariants[:,2],'r.')
+ax3.set_title('Torsion [rad/m]')
+
+
+# new constraints
+current_index = round(current_progress*len(trajectory))
+p_obj_start = calculate_trajectory[current_index]
+p_obj_end = calculate_trajectory[-1] - np.array([0.1, 0.1, 0.05])
+R_FS_start = movingframes[current_index]
+R_FS_end = movingframes[-1]
+
+boundary_constraints = {
+    "position": {
+        "initial": p_obj_start,
+        "final": p_obj_end
+    },
+    "moving-frame": {
+        "translational": {
+            "initial": R_FS_start,
+            "final": R_FS_end
+        }
+    },
+}
+initial_values = {
+    "trajectory": {
+        "position": calculate_trajectory
+    },
+    "moving-frame": {
+        "translational": movingframes,
+    },
+    "invariants": {
+        "translational": model_invariants,
+    }
+}
+
+weights = {}
+weights['w_invars'] = np.array([5 * 10 ** 1, 1.0, 1.0])
+
+# specify optimization problem symbolically
+FS_online_generation_problem = OCP_gen_pos(N=number_samples,w_invars = weights['w_invars'])
+fatrop_FS_online_generation_problem = fatrop_OCP_gen_pos(boundary_constraints,number_samples,solver='ipopt')
+rockit_FS_online_generation_problem = rockit_OCP_gen_pos(boundary_constraints,number_samples,fatrop_solver=False)
+
+# Solve
+new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(U_demo = model_invariants, p_obj_init = calculate_trajectory, R_t_init = movingframes, R_t_start = R_FS_start, R_t_end = R_FS_end, p_obj_start = p_obj_start, p_obj_end = p_obj_end, step_size = new_stepsize)
+fatrop_invars, fatrop_trajectory, fatrop_movingframes = fatrop_FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values=initial_values)
+rockit_invars, rockit_trajectory, rockit_movingframes, time = rockit_FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values=initial_values)
+
+fig = plt.figure(figsize=(14,8))
+ax = fig.add_subplot(111, projection='3d')
+ax.plot(trajectory[:,0],trajectory[:,1],trajectory[:,2],'b')
+ax.plot(new_trajectory[:,0],new_trajectory[:,1],new_trajectory[:,2],'r')
+ax.plot(fatrop_trajectory[:,0],fatrop_trajectory[:,1],fatrop_trajectory[:,2],'g')
+ax.plot(rockit_trajectory[:,0],rockit_trajectory[:,1],rockit_trajectory[:,2],'m')
+
+fig = plt.figure()
+plt.subplot(1,3,1)
+plt.plot(progress_values,new_invars[:,0],'r')
+plt.plot(arclength_n,invariants[:,0],'b')
+plt.plot(arclength_n,fatrop_invars[:,0],'g')
+plt.plot(arclength_n,rockit_invars[:,0],'m')
+plt.plot(0,0)
+plt.title('Velocity [m/m]')
+
+plt.subplot(1,3,2)
+plt.plot(progress_values,(new_invars[:,1]),'r')
+plt.plot(arclength_n,invariants[:,1],'b')
+plt.plot(arclength_n,fatrop_invars[:,1],'g')
+plt.plot(arclength_n,rockit_invars[:,1],'m')
+plt.plot(0,0)
+plt.title('Curvature [rad/m]')
+
+plt.subplot(1,3,3)
+plt.plot(progress_values,(new_invars[:,2]),'r')
+plt.plot(arclength_n,invariants[:,2],'b')
+plt.plot(arclength_n,fatrop_invars[:,2],'g')
+plt.plot(arclength_n,rockit_invars[:,2],'m')
+plt.plot(0,0)
+plt.title('Torsion [rad/m]')
+
+if plt.get_backend() != 'agg':
+    plt.show()

invariants_py/calculate_invariants/rockit_calculate_vector_invariants_position.py

@@ -143,7 +143,7 @@ def __init__(self, window_len=100, rms_error_traj=10**-3, fatrop_solver=False, b
             ocp._method.set_option("tol",tolerance)
             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-12)
+            # ocp._method.set_option("linsol_lu_fact_tol",1e-12) # Now gives error it doesn't exist
         else:
             ocp.method(rockit.MultipleShooting(N=N-1))
             ocp.solver('ipopt', {'expand':True, 'print_time':False, 'ipopt.tol':tolerance, 'ipopt.print_info_string':'yes', 'ipopt.max_iter':max_iter, 'ipopt.print_level':print_level, 'ipopt.ma57_automatic_scaling':'no', 'ipopt.linear_solver':'mumps'})

invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars_fatrop.py

@@ -201,7 +201,7 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,
             self.solution,initvals_dict = generate_initvals_from_constraints_opti(boundary_constraints, np.size(invariant_model,0), to_skip)
             self.first_window = False
         elif self.first_window:
-            self.solution = [initial_values["invariants"]["translational"][:N-1,:].T, initial_values["trajectory"]["position"][:N,:].T, initial_values["moving-frame"]["translational"][:N].T.transpose(1,2,0).reshape(3,3*N)]
+            self.solution = [*initial_values["invariants"]["translational"][:N-1,:], *initial_values["trajectory"]["position"][:N,:], *initial_values["moving-frame"]["translational"][:N]]
             self.first_window = False
 
         # Call solve function