diff --git a/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py b/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py index 354c040..cc45fca 100644 --- a/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py +++ b/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py @@ -8,12 +8,10 @@ import scipy.interpolate as ip from invariants_py.calculate_invariants.opti_calculate_vector_invariants_rotation import OCP_calc_rot from invariants_py.calculate_invariants.opti_calculate_vector_invariants_position import OCP_calc_pos as OCP_calc_pos -from invariants_py.generate_trajectory.opti_generate_orientation_traj_from_vector_invars import OCP_gen_rot -from invariants_py.generate_trajectory.opti_generate_position_traj_from_vector_invars import OCP_gen_pos as OCP_gen_pos -from invariants_py.generate_trajectory.opti_generate_pose_traj_from_vector_invars_fatrop import OCP_gen_pose as OCP_gen_pose_fatrop +from invariants_py.generate_trajectory.opti_generate_pose_traj_from_vector_invars import OCP_gen_pose from invariants_py.generate_trajectory.rockit_generate_pose_traj_from_vector_invars import OCP_gen_pose as OCP_gen_pose_rockit -from IPython.display import clear_output from scipy.spatial.transform import Rotation as R +from IPython.display import clear_output from invariants_py.kinematics.rigidbody_kinematics import orthonormalize_rotation as orthonormalize from stl import mesh import invariants_py.plotting_functions.plotters as pl @@ -121,7 +119,6 @@ def interpolate_model_invariants(demo_invariants, progress_values): # define new class for OCP results optim_gen_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((number_samples,6))) -fatrop_gen_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((number_samples,6))) rockit_gen_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((number_samples,6))) # Linear initialization @@ -160,9 +157,7 @@ def interpolate_model_invariants(demo_invariants, progress_values): } # specify optimization problem symbolically -FS_online_generation_problem_pos = OCP_gen_pos(N=number_samples,w_invars = weights_params['w_invars'][3:]) -FS_online_generation_problem_rot = OCP_gen_rot(window_len=number_samples,w_invars = weights_params['w_invars'][:3]) -fatrop_online_generation_problem = OCP_gen_pose_fatrop(boundary_constraints, number_samples, solver = 'fatrop') +FS_online_generation_problem = OCP_gen_pose(boundary_constraints, number_samples, solver = 'fatrop') rockit_online_generation_problem = OCP_gen_pose_rockit(boundary_constraints, number_samples,True) initial_values = { @@ -179,33 +174,23 @@ def interpolate_model_invariants(demo_invariants, progress_values): } # Solve -optim_gen_results.invariants[:,3:], optim_gen_results.Obj_pos, optim_gen_results.FSt_frames = FS_online_generation_problem_pos.generate_trajectory(U_demo = model_invariants[:,3:], p_obj_init = optim_calc_results.Obj_pos, R_t_init = optim_calc_results.FSt_frames, R_t_start = FSt_start, R_t_end = FSt_end, p_obj_start = p_obj_start, p_obj_end = p_obj_end, step_size = new_stepsize) -optim_gen_results.invariants[:,:3], optim_gen_results.Obj_frames, optim_gen_results.FSr_frames = FS_online_generation_problem_rot.generate_trajectory(U_demo = model_invariants[:,:3], R_obj_init = optim_calc_results.Obj_frames, R_r_init = optim_calc_results.FSr_frames, R_r_start = FSr_start, R_r_end = FSr_end, R_obj_start = R_obj_start, R_obj_end = R_obj_end, step_size = new_stepsize) -fatrop_gen_results.invariants, fatrop_gen_results.Obj_pos, fatrop_gen_results.Obj_frames, fatrop_gen_results.FSt_frames, fatrop_gen_results.FSr_frames = fatrop_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights_params,initial_values) +optim_gen_results.invariants, optim_gen_results.Obj_pos, optim_gen_results.Obj_frames, optim_gen_results.FSt_frames, optim_gen_results.FSr_frames = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights_params,initial_values) rockit_gen_results.invariants, rockit_gen_results.Obj_pos, rockit_gen_results.Obj_frames, rockit_gen_results.FSt_frames, rockit_gen_results.FSr_frames, tot_time, joint_values = rockit_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights_params,initial_values) -# for i in range(len(optim_gen_results.Obj_frames)): -# optim_gen_results.Obj_frames[i] = orthonormalize(optim_gen_results.Obj_frames[i]) - fig = plt.figure(figsize=(14,8)) ax = fig.add_subplot(111, projection='3d') ax.plot(optim_calc_results.Obj_pos[:,0],optim_calc_results.Obj_pos[:,1],optim_calc_results.Obj_pos[:,2],'b') ax.plot(optim_gen_results.Obj_pos[:,0],optim_gen_results.Obj_pos[:,1],optim_gen_results.Obj_pos[:,2],'r') -ax.plot(fatrop_gen_results.Obj_pos[:,0],fatrop_gen_results.Obj_pos[:,1],fatrop_gen_results.Obj_pos[:,2],'g') -ax.plot(rockit_gen_results.Obj_pos[:,0],rockit_gen_results.Obj_pos[:,1],rockit_gen_results.Obj_pos[:,2],'m') +ax.plot(rockit_gen_results.Obj_pos[:,0],rockit_gen_results.Obj_pos[:,1],rockit_gen_results.Obj_pos[:,2],'g') for i in indx: pl.plot_stl(opener_location,optim_calc_results.Obj_pos[i,:],optim_calc_results.Obj_frames[i,:,:],colour="c",alpha=0.2,ax=ax) indx_online = np.trunc(np.linspace(0,len(optim_gen_results.Obj_pos)-1,n_frames)) indx_online = indx_online.astype(int) for i in indx_online: - # pl.plot_3d_frame(optim_calc_results.Obj_pos[i,:],optim_calc_results.Obj_frames[i,:,:],1,0.01,['red','green','blue'],ax) - # pl.plot_3d_frame(optim_gen_results.Obj_pos[i,:],optim_gen_results.Obj_frames[i,:,:],1,0.01,['red','green','blue'],ax) pl.plot_stl(opener_location,optim_gen_results.Obj_pos[i,:],optim_gen_results.Obj_frames[i,:,:],colour="r",alpha=0.2,ax=ax) - pl.plot_stl(opener_location,fatrop_gen_results.Obj_pos[i,:],fatrop_gen_results.Obj_frames[i,:,:],colour="g",alpha=0.2,ax=ax) - pl.plot_stl(opener_location,rockit_gen_results.Obj_pos[i,:],rockit_gen_results.Obj_frames[i,:,:],colour="m",alpha=0.2,ax=ax) + pl.plot_stl(opener_location,rockit_gen_results.Obj_pos[i,:],rockit_gen_results.Obj_frames[i,:,:],colour="g",alpha=0.2,ax=ax) pl.plot_orientation(optim_calc_results.Obj_frames,optim_gen_results.Obj_frames,current_index) -pl.plot_orientation(optim_calc_results.Obj_frames,fatrop_gen_results.Obj_frames,current_index) pl.plot_orientation(optim_calc_results.Obj_frames,rockit_gen_results.Obj_frames,current_index) @@ -213,48 +198,42 @@ def interpolate_model_invariants(demo_invariants, progress_values): plt.subplot(2,3,1) plt.plot(arclength_n,optim_calc_results.invariants[:,0],'b') plt.plot(progress_values,optim_gen_results.invariants[:,0],'r') -plt.plot(progress_values,fatrop_gen_results.invariants[:,0],'g') -plt.plot(progress_values,rockit_gen_results.invariants[:,0],'m') +plt.plot(progress_values,rockit_gen_results.invariants[:,0],'g') plt.plot(0,0) plt.title('i_r1') plt.subplot(2,3,2) plt.plot(arclength_n,optim_calc_results.invariants[:,1],'b') plt.plot(progress_values,(optim_gen_results.invariants[:,1]),'r') -plt.plot(progress_values,(fatrop_gen_results.invariants[:,1]),'g') -plt.plot(progress_values,(rockit_gen_results.invariants[:,1]),'m') +plt.plot(progress_values,(rockit_gen_results.invariants[:,1]),'g') plt.plot(0,0) plt.title('i_r2') plt.subplot(2,3,3) plt.plot(arclength_n,optim_calc_results.invariants[:,2],'b') plt.plot(progress_values,(optim_gen_results.invariants[:,2]),'r') -plt.plot(progress_values,(fatrop_gen_results.invariants[:,2]),'g') -plt.plot(progress_values,(rockit_gen_results.invariants[:,2]),'m') +plt.plot(progress_values,(rockit_gen_results.invariants[:,2]),'g') plt.plot(0,0) plt.title('i_r3') plt.subplot(2,3,4) plt.plot(arclength_n,optim_calc_results.invariants[:,0],'b') plt.plot(progress_values,optim_gen_results.invariants[:,0],'r') -plt.plot(arclength_n,fatrop_gen_results.invariants[:,0],'g') -plt.plot(arclength_n,rockit_gen_results.invariants[:,0],'m') +plt.plot(arclength_n,rockit_gen_results.invariants[:,0],'g') plt.plot(0,0) plt.title('i_t1') plt.subplot(2,3,5) plt.plot(arclength_n,optim_calc_results.invariants[:,1],'b') plt.plot(progress_values,(optim_gen_results.invariants[:,1]),'r') -plt.plot(arclength_n,fatrop_gen_results.invariants[:,1],'g') -plt.plot(arclength_n,rockit_gen_results.invariants[:,1],'m') +plt.plot(arclength_n,rockit_gen_results.invariants[:,1],'g') plt.plot(0,0) plt.title('i_t1') plt.subplot(2,3,6) plt.plot(arclength_n,optim_calc_results.invariants[:,2],'b') plt.plot(progress_values,(optim_gen_results.invariants[:,2]),'r') -plt.plot(arclength_n,fatrop_gen_results.invariants[:,2],'g') -plt.plot(arclength_n,rockit_gen_results.invariants[:,2],'m') +plt.plot(arclength_n,rockit_gen_results.invariants[:,2],'g') plt.plot(0,0) plt.title('i_t3') diff --git a/examples/trajectory_generation/COMPARING trajectory_generation_position.py b/examples/trajectory_generation/COMPARING trajectory_generation_position.py index 7a7884b..1e0c9da 100644 --- a/examples/trajectory_generation/COMPARING trajectory_generation_position.py +++ b/examples/trajectory_generation/COMPARING trajectory_generation_position.py @@ -8,7 +8,6 @@ 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 @@ -117,44 +116,38 @@ def interpolate_model_invariants(demo_invariants, progress_values): 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='fatrop') +FS_online_generation_problem = OCP_gen_pos(boundary_constraints,number_samples,solver='fatrop') rockit_FS_online_generation_problem = rockit_OCP_gen_pos(boundary_constraints,number_samples,fatrop_solver=True) # 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) +new_invars, new_trajectory, new_movingframes = 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') +ax.plot(rockit_trajectory[:,0],rockit_trajectory[:,1],rockit_trajectory[:,2],'g') 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(arclength_n,rockit_invars[:,0],'g') 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(arclength_n,rockit_invars[:,1],'g') 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(arclength_n,rockit_invars[:,2],'g') plt.plot(0,0) plt.title('Torsion [rad/m]') diff --git a/examples/trajectory_generation/COMPARING trajectory_generation_rotation.py b/examples/trajectory_generation/COMPARING trajectory_generation_rotation.py index 674b0fb..2b28895 100644 --- a/examples/trajectory_generation/COMPARING trajectory_generation_rotation.py +++ b/examples/trajectory_generation/COMPARING trajectory_generation_rotation.py @@ -9,9 +9,7 @@ import scipy.interpolate as ip from invariants_py.calculate_invariants.opti_calculate_vector_invariants_rotation import OCP_calc_rot from invariants_py.generate_trajectory.opti_generate_orientation_traj_from_vector_invars import OCP_gen_rot -from invariants_py.generate_trajectory.opti_generate_orientation_traj_from_vector_invars_fatrop import OCP_gen_rot as fatrop_OCP_gen_rot from invariants_py.generate_trajectory.rockit_generate_orientation_traj_from_vector_invars import OCP_gen_rot as rockit_OCP_gen_rot -from IPython.display import clear_output from invariants_py.plotting_functions.plotters import plot_3d_frame, plot_orientation, plot_stl from stl import mesh from scipy.spatial.transform import Rotation as R @@ -138,13 +136,11 @@ def interpolate_model_invariants(demo_invariants, progress_values): weights['w_invars'] = np.array([5 * 10 ** 1, 1.0, 1.0]) # 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='fatrop') +FS_online_generation_problem = 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) -fatrop_invars, fatrop_trajectory, fatrop_movingframes = fatrop_FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values=initial_values) +new_invars, new_trajectory, new_movingframes = 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) # for i in range(len(new_trajectory)): @@ -164,34 +160,29 @@ def interpolate_model_invariants(demo_invariants, progress_values): for i in indx_online: plot_3d_frame(trajectory_position_online[i,:],new_trajectory[i,:,:],1,0.05,['red','green','blue'],ax) plot_stl(opener_location,trajectory_position_online[i,:],new_trajectory[i,:,:],colour="r",alpha=0.2,ax=ax) - plot_stl(opener_location,trajectory_position_online[i,:],fatrop_trajectory[i,:,:],colour="g",alpha=0.2,ax=ax) - plot_stl(opener_location,trajectory_position_online[i,:],rockit_trajectory[i,:,:],colour="m",alpha=0.2,ax=ax) + plot_stl(opener_location,trajectory_position_online[i,:],rockit_trajectory[i,:,:],colour="g",alpha=0.2,ax=ax) plot_orientation(calculate_trajectory, new_trajectory,current_index) -plot_orientation(calculate_trajectory, fatrop_trajectory,current_index) plot_orientation(calculate_trajectory, rockit_trajectory,current_index) fig = plt.figure() plt.subplot(1,3,1) plt.plot(arclength_n,invariants[:,0],'b') plt.plot(progress_values,new_invars[:,0],'r') -plt.plot(progress_values,fatrop_invars[:,0],'g') -plt.plot(progress_values,rockit_invars[:,0],'m') +plt.plot(progress_values,rockit_invars[:,0],'g') plt.plot(0,0) plt.title('Velocity [m/m]') plt.subplot(1,3,2) plt.plot(arclength_n,invariants[:,1],'b') plt.plot(progress_values,(new_invars[:,1]),'r') -plt.plot(progress_values,(fatrop_invars[:,1]),'g') -plt.plot(progress_values,(rockit_invars[:,1]),'m') +plt.plot(progress_values,(rockit_invars[:,1]),'g') plt.plot(0,0) plt.title('Curvature [rad/m]') plt.subplot(1,3,3) plt.plot(arclength_n,invariants[:,2],'b') plt.plot(progress_values,(new_invars[:,2]),'r') -plt.plot(progress_values,(fatrop_invars[:,2]),'g') -plt.plot(progress_values,(rockit_invars[:,2]),'m') +plt.plot(progress_values,(rockit_invars[:,2]),'g') plt.plot(0,0) plt.title('Torsion [rad/m]') diff --git a/examples/trajectory_generation/trajectory_generation_fullpose.py b/examples/trajectory_generation/trajectory_generation_fullpose.py index 5d4072a..2a9468b 100644 --- a/examples/trajectory_generation/trajectory_generation_fullpose.py +++ b/examples/trajectory_generation/trajectory_generation_fullpose.py @@ -8,21 +8,29 @@ import scipy.interpolate as ip from invariants_py.calculate_invariants.opti_calculate_vector_invariants_rotation import OCP_calc_rot from invariants_py.calculate_invariants.opti_calculate_vector_invariants_position import OCP_calc_pos as OCP_calc_pos -from invariants_py.generate_trajectory.opti_generate_orientation_traj_from_vector_invars import OCP_gen_rot -from invariants_py.generate_trajectory.opti_generate_position_traj_from_vector_invars import OCP_gen_pos as OCP_gen_pos -from IPython.display import clear_output +from invariants_py.generate_trajectory.opti_generate_pose_traj_from_vector_invars import OCP_gen_pose from scipy.spatial.transform import Rotation as R +from IPython.display import clear_output from invariants_py.kinematics.rigidbody_kinematics import orthonormalize_rotation as orthonormalize from stl import mesh import invariants_py.plotting_functions.plotters as pl +from invariants_py.ocp_initialization import initial_trajectory_movingframe_rotation +import random + #%% +show_plots = True +solver = 'fatrop' + data_location = dh.find_data_path('beer_1.txt') opener_location = dh.find_data_path('opener.stl') +bottle_location = dh.find_data_path('bottle.stl') 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_position = pose[:,:3,3] +home_pos = [0,0,0] # Use this if not considering the robot +# home_pos = [0.3056, 0.0635, 0.441] # Define home position of the robot +trajectory_position = pose[:,:3,3] + home_pos trajectory_orientation = pose[:,:3,:3] fig = plt.figure(figsize=(8,8)) @@ -51,12 +59,13 @@ def __init__(self,FSt_frames,FSr_frames,Obj_pos,Obj_frames,invariants): optim_calc_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((len(trajectory),6))) # specify optimization problem symbolically -FS_calculation_problem_pos = OCP_calc_pos(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 0.01) -FS_calculation_problem_rot = OCP_calc_rot(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 10*pi/180) +FS_calculation_problem_pos = OCP_calc_pos(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 0.004) +FS_calculation_problem_rot = OCP_calc_rot(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 2*pi/180) # calculate invariants given measurements optim_calc_results.invariants[:,3:], optim_calc_results.Obj_pos, optim_calc_results.FSt_frames = FS_calculation_problem_pos.calculate_invariants(trajectory,stepsize) optim_calc_results.invariants[:,:3], optim_calc_results.Obj_frames, optim_calc_results.FSr_frames = FS_calculation_problem_rot.calculate_invariants(trajectory,stepsize) +optim_calc_results.Obj_pos += home_pos fig = plt.figure(figsize=(8,8)) ax = fig.add_subplot(111, projection='3d') @@ -67,8 +76,8 @@ def __init__(self,FSt_frames,FSr_frames,Obj_pos,Obj_frames,invariants): indx = indx.astype(int) for i in indx: pl.plot_3d_frame(optim_calc_results.Obj_pos[i,:],optim_calc_results.Obj_frames[i,:,:],1,0.01,['red','green','blue'],ax) - pl.plot_stl(opener_location,trajectory_position[i,:],trajectory_orientation[i,:,:],colour="r",alpha=0.2,ax=ax) - pl.plot_stl(opener_location,optim_calc_results.Obj_pos[i,:],optim_calc_results.Obj_frames[i,:,:],colour="c",alpha=0.2,ax=ax) + pl.plot_stl(opener_location,trajectory_position[i,:],trajectory_orientation[i,:,:],colour="c",alpha=0.2,ax=ax) + pl.plot_stl(opener_location,optim_calc_results.Obj_pos[i,:],optim_calc_results.Obj_frames[i,:,:],colour="r",alpha=0.2,ax=ax) pl.plot_orientation(optim_calc_results.Obj_frames,trajectory_orientation) @@ -115,16 +124,62 @@ def interpolate_model_invariants(demo_invariants, progress_values): # define new class for OCP results optim_gen_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((number_samples,6))) +# Linear initialization +R_obj_init = reparam.interpR(np.linspace(0, 1, len(optim_calc_results.Obj_frames)), [0,1], np.array([R_obj_start, R_obj_end])) + +R_r_init, R_r_init_array, invars_init = initial_trajectory_movingframe_rotation(R_obj_start, R_obj_end) + +boundary_constraints = { + "position": { + "initial": p_obj_start, + "final": p_obj_end + }, + "orientation": { + "initial": R_obj_start, + "final": R_obj_end + }, + "moving-frame": { + "translational": { + "initial": FSt_start, + "final": FSt_end + }, + "rotational": { + "initial": R_r_init, + "final": R_r_init + } + }, +} + +# Define OCP weights +weights_params = { + "w_invars": np.array([1, 1, 1, 5*10**1, 1.0, 1.0]), + "w_high_start": 60, + "w_high_end": number_samples, + "w_high_invars": 10*np.array([1, 1, 1, 5*10**1, 1.0, 1.0]), + "w_high_active": 0 +} + # specify optimization problem symbolically -FS_online_generation_problem_pos = OCP_gen_pos(N=number_samples,w_invars = np.array([5*10**1, 1.0, 1.0])) -FS_online_generation_problem_rot = OCP_gen_rot(window_len=number_samples,w_invars = 10**2*np.array([10**1, 1.0, 1.0])) +FS_online_generation_problem = OCP_gen_pose(boundary_constraints, number_samples, solver = solver) + +initial_values = { + "trajectory": { + "position": optim_calc_results.Obj_pos, + "orientation": R_obj_init + }, + "moving-frame": { + "translational": optim_calc_results.FSt_frames, + "rotational": R_r_init_array, + }, + "invariants": model_invariants, + # "joint-values": robot_params["q_init"] if robot_params["urdf_file_name"] is not None else {} +} # Solve -optim_gen_results.invariants[:,3:], optim_gen_results.Obj_pos, optim_gen_results.FSt_frames = FS_online_generation_problem_pos.generate_trajectory(U_demo = model_invariants[:,3:], p_obj_init = optim_calc_results.Obj_pos, R_t_init = optim_calc_results.FSt_frames, R_t_start = FSt_start, R_t_end = FSt_end, p_obj_start = p_obj_start, p_obj_end = p_obj_end, step_size = new_stepsize) -optim_gen_results.invariants[:,:3], optim_gen_results.Obj_frames, optim_gen_results.FSr_frames = FS_online_generation_problem_rot.generate_trajectory(U_demo = model_invariants[:,:3], R_obj_init = optim_calc_results.Obj_frames, R_r_init = optim_calc_results.FSr_frames, R_r_start = FSr_start, R_r_end = FSr_end, R_obj_start = R_obj_start, R_obj_end = R_obj_end, step_size = new_stepsize) +optim_gen_results.invariants, optim_gen_results.Obj_pos, optim_gen_results.Obj_frames, optim_gen_results.FSt_frames, optim_gen_results.FSr_frames = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights_params,initial_values) -for i in range(len(optim_gen_results.Obj_frames)): - optim_gen_results.Obj_frames[i] = orthonormalize(optim_gen_results.Obj_frames[i]) +# for i in range(len(optim_gen_results.Obj_frames)): +# optim_gen_results.Obj_frames[i] = orthonormalize(optim_gen_results.Obj_frames[i]) fig = plt.figure(figsize=(14,8)) ax = fig.add_subplot(111, projection='3d') @@ -141,80 +196,174 @@ def interpolate_model_invariants(demo_invariants, progress_values): pl.plot_stl(opener_location,optim_gen_results.Obj_pos[i,:],optim_gen_results.Obj_frames[i,:,:],colour="r",alpha=0.2,ax=ax) pl.plot_orientation(optim_calc_results.Obj_frames,optim_gen_results.Obj_frames,current_index) -pl.plot_invariants(optim_calc_results.invariants, optim_gen_results.invariants, arclength_n, progress_values) + +fig = plt.figure() +plt.subplot(2,3,1) +plt.plot(arclength_n,optim_calc_results.invariants[:,0],'b') +plt.plot(progress_values,optim_gen_results.invariants[:,0],'r') +plt.plot(0,0) +plt.title('i_r1') + +plt.subplot(2,3,2) +plt.plot(arclength_n,optim_calc_results.invariants[:,1],'b') +plt.plot(progress_values,(optim_gen_results.invariants[:,1]),'r') +plt.plot(0,0) +plt.title('i_r2') + +plt.subplot(2,3,3) +plt.plot(arclength_n,optim_calc_results.invariants[:,2],'b') +plt.plot(progress_values,(optim_gen_results.invariants[:,2]),'r') +plt.plot(0,0) +plt.title('i_r3') + +plt.subplot(2,3,4) +plt.plot(arclength_n,optim_calc_results.invariants[:,0],'b') +plt.plot(progress_values,optim_gen_results.invariants[:,0],'r') +plt.plot(0,0) +plt.title('i_t1') + +plt.subplot(2,3,5) +plt.plot(arclength_n,optim_calc_results.invariants[:,1],'b') +plt.plot(progress_values,(optim_gen_results.invariants[:,1]),'r') +plt.plot(0,0) +plt.title('i_t1') + +plt.subplot(2,3,6) +plt.plot(arclength_n,optim_calc_results.invariants[:,2],'b') +plt.plot(progress_values,(optim_gen_results.invariants[:,2]),'r') +plt.plot(0,0) +plt.title('i_t3') if plt.get_backend() != 'agg': plt.show() -#%% Visualization -window_len = 20 +#%% Generation of multiple trajectories to test FATROP calculation speed + +current_progress = 0 +number_samples = 100 +number_of_trajectories = 100 + +progress_values = np.linspace(current_progress, arclength_n[-1], number_samples) +model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) + +# pl.plot_interpolated_invariants(optim_calc_results.invariants, model_invariants, arclength_n, progress_values) + +# new constraints +current_index = round(current_progress*len(trajectory)) +p_obj_start = optim_calc_results.Obj_pos[current_index] +R_obj_start = orthonormalize(optim_calc_results.Obj_frames[current_index]) +FSt_start = orthonormalize(optim_calc_results.FSt_frames[current_index]) +FSr_start = orthonormalize(optim_calc_results.FSr_frames[current_index]) +FSt_end = orthonormalize(optim_calc_results.FSt_frames[-1]) +FSr_end = orthonormalize(optim_calc_results.FSr_frames[-1]) + +boundary_constraints = { + "position": { + "initial": p_obj_start, + "final": p_obj_start # will be updated later + }, + "orientation": { + "initial": R_obj_start, + "final": [] + }, + "moving-frame": { + "translational": { + "initial": FSt_start, + "final": FSt_end + }, + "rotational": { + "initial": FSr_start, + "final": FSr_end + } + }, +} + +initial_values = { + "trajectory": { + "position": optim_calc_results.Obj_pos, + "orientation": [], + }, + "moving-frame": { + "translational": optim_calc_results.FSt_frames, + "rotational": [] + }, + "invariants": model_invariants, +} # define new class for OCP results -optim_iter_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((window_len,6))) +optim_gen_results = OCP_results(FSt_frames = [], FSr_frames = [], Obj_pos = [], Obj_frames = [], invariants = np.zeros((number_samples,6))) # specify optimization problem symbolically -FS_online_generation_problem_pos = OCP_gen_pos(N=window_len,w_invars = np.array([5*10**1, 1.0, 1.0])) -FS_online_generation_problem_rot = OCP_gen_rot(window_len=window_len,w_invars = 10**1*np.array([10**1, 1.0, 1.0])) +FS_online_generation_problem = OCP_gen_pose(boundary_constraints, number_samples, solver = solver) + +if show_plots: + fig = plt.figure(figsize=(14,8)) + ax = fig.add_subplot(111, projection='3d') + ax.plot(optim_calc_results.Obj_pos[:,0],optim_calc_results.Obj_pos[:,1],optim_calc_results.Obj_pos[:,2],'b') +tot_time = 0 +counter = 0 +max_time = 0 +targets = np.zeros((number_of_trajectories,4)) +for k in range(len(targets)): +# for x in range(-2,3): + # for y in range(-2,3): + # p_obj_end = optim_calc_results.Obj_pos[-1] + np.array([0.05*x,0.05*y,0]) + targets[k,:-1] = optim_calc_results.Obj_pos[-1] + np.array([random.uniform(-0.2,0.2),random.uniform(-0.2,0.2),random.uniform(-0.05,0.05)]) + targets[k,-1] = random.uniform(0,30) + p_obj_end = targets[k,:-1] + rotate = R.from_euler('z', targets[k,-1], degrees=True) + R_obj_end = orthonormalize(rotate.apply(optim_calc_results.Obj_frames[-1])) + + R_obj_init = reparam.interpR(np.linspace(0, 1, len(trajectory)), [0,1], np.array([R_obj_start, R_obj_end])) + + R_r_init, R_r_init_array, invars_init = initial_trajectory_movingframe_rotation(R_obj_start, R_obj_end) + + boundary_constraints["position"]["final"] = p_obj_end + boundary_constraints["orientation"]["final"] = R_obj_end + boundary_constraints["moving-frame"]["rotational"]["initial"] = R_r_init + boundary_constraints["moving-frame"]["rotational"]["final"] = R_r_init + + initial_values["trajectory"]["orientation"] = R_obj_init + initial_values["moving-frame"]["rotational"] = R_r_init_array + -current_progress = 0.0 -old_progress = 0.0 + # Solve + optim_gen_results.invariants, optim_gen_results.Obj_pos, optim_gen_results.Obj_frames, optim_gen_results.FSt_frames, optim_gen_results.FSr_frames = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights_params,initial_values) -R_obj_end = optim_calc_results.Obj_frames[-1] # initialise R_obj_end with end point of reconstructed trajectory -optim_iter_results.Obj_pos = optim_calc_results.Obj_pos.copy() -optim_iter_results.Obj_frames = optim_calc_results.Obj_frames.copy() -optim_iter_results.FSt_frames = optim_calc_results.FSt_frames.copy() -optim_iter_results.FSr_frames = optim_calc_results.FSr_frames.copy() -fig = plt.figure(figsize=(14,8)) -ax = fig.add_subplot(111, projection='3d') -fig_invars = plt.figure(figsize=(10, 6)) + for i in range(len(optim_gen_results.Obj_frames)): + optim_gen_results.Obj_frames[i] = orthonormalize(optim_gen_results.Obj_frames[i]) -while current_progress <= 1.0: - - print(f"current progress = {current_progress}") + if show_plots: + ax.plot(optim_gen_results.Obj_pos[:,0],optim_gen_results.Obj_pos[:,1],optim_gen_results.Obj_pos[:,2],'r') - # Resample invariants for current progress - progress_values = np.linspace(current_progress, arclength_n[-1], window_len) - model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) - - # Boundary constraints - current_index = round( (current_progress - old_progress) * len(optim_iter_results.Obj_pos)) - p_obj_start = optim_iter_results.Obj_pos[current_index] - R_obj_start = optim_iter_results.Obj_frames[current_index] - FSt_start = optim_iter_results.FSt_frames[current_index] - FSr_start = optim_iter_results.FSr_frames[current_index] - p_obj_end = optim_calc_results.Obj_pos[-1] + current_progress*np.array([0.1,0.1,0.1]) - rotate = R.from_euler('z', 0/window_len, degrees=True) - R_obj_end = orthonormalize(rotate.apply(R_obj_end)) - FSt_end = optim_iter_results.FSt_frames[-1] - FSr_end = optim_iter_results.FSr_frames[-1] - - # Calculate remaining trajectory - optim_iter_results.invariants[:,3:], optim_iter_results.Obj_pos, optim_iter_results.FSt_frames = FS_online_generation_problem_pos.generate_trajectory(U_demo = model_invariants[:,3:], p_obj_init = optim_calc_results.Obj_pos, R_t_init = optim_calc_results.FSt_frames, R_t_start = FSt_start, R_t_end = FSt_end, p_obj_start = p_obj_start, p_obj_end = p_obj_end, step_size = new_stepsize) - optim_iter_results.invariants[:,:3], optim_iter_results.Obj_frames, optim_iter_results.FSr_frames = FS_online_generation_problem_rot.generate_trajectory(U_demo = model_invariants[:,:3], R_obj_init = optim_calc_results.Obj_frames, R_r_init = optim_calc_results.FSr_frames, R_r_start = FSr_start, R_r_end = FSr_end, R_obj_start = R_obj_start, R_obj_end = R_obj_end, step_size = new_stepsize) - - for i in range(len(optim_iter_results.Obj_frames)): - optim_iter_results.Obj_frames[i] = orthonormalize(optim_iter_results.Obj_frames[i]) - - # Visualization - clear_output(wait=True) - - ax.clear() - ax.plot(optim_calc_results.Obj_pos[:,0],optim_calc_results.Obj_pos[:,1],optim_calc_results.Obj_pos[:,2],'b') - ax.plot(optim_iter_results.Obj_pos[:,0],optim_iter_results.Obj_pos[:,1],optim_iter_results.Obj_pos[:,2],'r') - for i in indx: - pl.plot_stl(opener_location,optim_calc_results.Obj_pos[i,:],optim_calc_results.Obj_frames[i,:,:],colour="c",alpha=0.2,ax=ax) - indx_iter = np.trunc(np.linspace(0,len(optim_iter_results.Obj_pos)-1,n_frames)) - indx_iter = indx_iter.astype(int) - for i in indx_iter: - pl.plot_3d_frame(optim_iter_results.Obj_pos[i,:],optim_iter_results.Obj_frames[i,:,:],1,0.01,['red','green','blue'],ax) - pl.plot_stl(opener_location,optim_iter_results.Obj_pos[i,:],optim_iter_results.Obj_frames[i,:,:],colour="r",alpha=0.2,ax=ax) - - pl.plot_invariants(optim_calc_results.invariants,optim_iter_results.invariants,arclength_n,progress_values,fig=fig_invars) + # if solver == 'fatrop': + # new_time = tot_time_pos + tot_time_rot + # if new_time > max_time: + # max_time = new_time + # tot_time = tot_time + new_time + # counter += 1 + +# if solver == 'fatrop': +# print('') +# print("AVERAGE time to generate new trajectory: ") +# print(str(tot_time/counter) + "[s]") +# print('') +# print("MAXIMUM time to generate new trajectory: ") +# print(str(max_time) + "[s]") + +# fig = plt.figure(figsize=(10,6)) +# ax1 = fig.add_subplot(111, projection='3d') +# ax1 = plt.axes(projection='3d') +# ax1.plot(trajectory_position[:,0],trajectory_position[:,1],trajectory_position[:,2],'b') +# ax1.plot(targets[:,0],targets[:,1],targets[:,2],'r.') + +if show_plots: + fig = plt.figure(figsize=(5,5)) + ax2 = fig.add_subplot() + ax2.plot(targets[:,-1],'r.') + if plt.get_backend() != 'agg': - plt.show(block=False) - - old_progress = current_progress - current_progress = old_progress + 1/window_len \ No newline at end of file + plt.show() \ No newline at end of file diff --git a/examples/trajectory_generation/trajectory_generation_position.py b/examples/trajectory_generation/trajectory_generation_position.py index 3548b73..6a200e3 100644 --- a/examples/trajectory_generation/trajectory_generation_position.py +++ b/examples/trajectory_generation/trajectory_generation_position.py @@ -9,6 +9,10 @@ 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 IPython.display import clear_output +import random + +show_plots = True +solver = 'fatrop' data_location = dh.find_data_path('beer_1.txt') trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt') @@ -61,7 +65,7 @@ def interpolate_model_invariants(demo_invariants, progress_values): #%% current_progress = 0 -number_samples = 40 +number_samples = 100 calculate_trajectory = init_vals_calculate_trajectory movingframes = init_vals_movingframes @@ -83,16 +87,42 @@ def interpolate_model_invariants(demo_invariants, progress_values): # 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,0,0]) +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 = np.array([5*10**1, 1.0, 1.0])) +FS_online_generation_problem = OCP_gen_pos(boundary_constraints,number_samples,solver=solver) # 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) +new_invars, new_trajectory, new_movingframes = 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') @@ -121,74 +151,99 @@ def interpolate_model_invariants(demo_invariants, progress_values): if plt.get_backend() != 'agg': plt.show() +#%% Generation of multiple trajectories to test FATROP calculation speed -#%% Visualization - -window_len = 20 - -# specify optimization problem symbolically -FS_online_generation_problem = OCP_gen_pos(N=window_len,w_invars = 10**1*np.array([10**1, 1.0, 1.0])) - -current_progress = 0.0 -old_progress = 0.0 - -calculate_trajectory = init_vals_calculate_trajectory -movingframes = init_vals_movingframes +current_progress = 0 +number_samples = 100 +number_of_trajectories = 100 -fig = plt.figure(figsize=(14,8)) -ax = fig.add_subplot(111, projection='3d') -ax.plot(trajectory[:,0],trajectory[:,1],trajectory[:,2],'b') +progress_values = np.linspace(current_progress, arclength_n[-1], number_samples) +model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) -fig2 = plt.figure() -ax1 = fig2.add_subplot(1,3,1) -ax2 = fig2.add_subplot(1,3,2) -ax3 = fig2.add_subplot(1,3,3) +# pl.plot_interpolated_invariants(optim_calc_results.invariants, model_invariants, arclength_n, progress_values) -while current_progress <= 1.0: - - print(f"current progress = {current_progress}") +# new constraints +current_index = round(current_progress*len(trajectory)) +p_obj_start = calculate_trajectory[current_index] +FSt_start = movingframes[current_index] +FSt_end = movingframes[-1] + +boundary_constraints = { + "position": { + "initial": p_obj_start, + "final": p_obj_start # will be updated later + }, + "moving-frame": { + "translational": { + "initial": FSt_start, + "final": FSt_end + }, + }, +} + +initial_values = { + "trajectory": { + "position": calculate_trajectory, + }, + "moving-frame": { + "translational": movingframes, + }, + "invariants": { + "translational": model_invariants, + } +} - # Resample invariants for current progress - progress_values = np.linspace(current_progress, arclength_n[-1], window_len) - model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) - - # Boundary constraints - current_index = round( (current_progress - old_progress) * len(calculate_trajectory)) - p_obj_start = calculate_trajectory[current_index] - p_obj_end = trajectory[-1] - current_progress*np.array([-0.2, 0.0, 0.0]) - R_FS_start = movingframes[current_index] - R_FS_end = movingframes[-1] - - # Calculate remaining trajectory - new_invars, calculate_trajectory, 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) - - # Visualization - clear_output(wait=True) - - ax.clear() - ax.plot(trajectory[:,0],trajectory[:,1],trajectory[:,2],'b') - ax.plot(calculate_trajectory[:,0],calculate_trajectory[:,1],calculate_trajectory[:,2],'r') - - ax1.clear() - ax1.plot(progress_values,new_invars[:,0],'r') - ax1.plot(arclength_n,invariants[:,0],'b') - ax1.plot(0,0) - ax1.set_title('velocity [m/m]') - - ax2.clear() - ax2.plot(progress_values,(new_invars[:,1]),'r') - ax2.plot(arclength_n,invariants[:,1],'b') - ax2.plot(0,0) - ax2.set_title('curvature [rad/m]') +# specify optimization problem symbolically +FS_online_generation_problem = OCP_gen_pos(boundary_constraints, number_samples, solver = solver) + +if show_plots: + fig = plt.figure(figsize=(14,8)) + ax = fig.add_subplot(111, projection='3d') + ax.plot(calculate_trajectory[:,0],calculate_trajectory[:,1],calculate_trajectory[:,2],'b') + +tot_time = 0 +counter = 0 +max_time = 0 +targets = np.zeros((number_of_trajectories,4)) +for k in range(len(targets)): + targets[k,:-1] = calculate_trajectory[-1] + np.array([random.uniform(-0.2,0.2),random.uniform(-0.2,0.2),random.uniform(-0.05,0.05)]) + targets[k,-1] = random.uniform(0,30) + p_obj_end = targets[k,:-1] + + boundary_constraints["position"]["final"] = p_obj_end + + # Solve + new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values) + + if show_plots: + ax.plot(new_trajectory[:,0],new_trajectory[:,1],new_trajectory[:,2],'r') + + # if solver == 'fatrop': + # new_time = tot_time_pos + tot_time_rot + # if new_time > max_time: + # max_time = new_time + # tot_time = tot_time + new_time - ax3.clear() - ax3.plot(progress_values,(new_invars[:,2]),'r') - ax3.plot(arclength_n,invariants[:,2],'b') - ax3.plot(0,0) - ax3.set_title('torsion [rad/m]') + # counter += 1 + +# if solver == 'fatrop': +# print('') +# print("AVERAGE time to generate new trajectory: ") +# print(str(tot_time/counter) + "[s]") +# print('') +# print("MAXIMUM time to generate new trajectory: ") +# print(str(max_time) + "[s]") + +# fig = plt.figure(figsize=(10,6)) +# ax1 = fig.add_subplot(111, projection='3d') +# ax1 = plt.axes(projection='3d') +# ax1.plot(trajectory_position[:,0],trajectory_position[:,1],trajectory_position[:,2],'b') +# ax1.plot(targets[:,0],targets[:,1],targets[:,2],'r.') + +if show_plots: + fig = plt.figure(figsize=(5,5)) + ax2 = fig.add_subplot() + ax2.plot(targets[:,-1],'r.') if plt.get_backend() != 'agg': - plt.show() - - old_progress = current_progress - current_progress = old_progress + 1/window_len \ No newline at end of file + plt.show() \ No newline at end of file diff --git a/examples/trajectory_generation/trajectory_generation_rotation.py b/examples/trajectory_generation/trajectory_generation_rotation.py index a3b759c..7201e6d 100644 --- a/examples/trajectory_generation/trajectory_generation_rotation.py +++ b/examples/trajectory_generation/trajectory_generation_rotation.py @@ -9,12 +9,16 @@ import scipy.interpolate as ip from invariants_py.calculate_invariants.opti_calculate_vector_invariants_rotation import OCP_calc_rot from invariants_py.generate_trajectory.opti_generate_orientation_traj_from_vector_invars import OCP_gen_rot -from IPython.display import clear_output from invariants_py.plotting_functions.plotters import plot_3d_frame, plot_orientation, plot_stl from stl import mesh from scipy.spatial.transform import Rotation as R from invariants_py.kinematics.rigidbody_kinematics import orthonormalize_rotation as orthonormalize +from invariants_py.ocp_initialization import initial_trajectory_movingframe_rotation +import random #%% +show_plots = True +solver = 'fatrop' + data_location = dh.find_data_path('beer_1.txt') opener_location = dh.find_data_path('opener.stl') trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'txt') @@ -83,8 +87,8 @@ def interpolate_model_invariants(demo_invariants, progress_values): return resampled_invariants, new_stepsize #%% -current_progress = 0.3 -number_samples = 60 +current_progress = 0 +number_samples = 100 progress_values = np.linspace(current_progress, arclength_n[-1], number_samples) model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) @@ -108,14 +112,41 @@ def interpolate_model_invariants(demo_invariants, progress_values): R_r_start = orthonormalize(movingframes[current_index]) R_r_end = orthonormalize(movingframes[-1]) +boundary_constraints = { + "orientation": { + "initial": R_obj_start, + "final": R_obj_end + }, + "moving-frame": { + "rotational": { + "initial": R_r_start, + "final": R_r_end + } + }, +} +initial_values = { + "trajectory": { + "orientation": calculate_trajectory + }, + "moving-frame": { + "rotational": movingframes, + }, + "invariants": { + "rotational": 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_rot(window_len=number_samples,w_invars = 10**2*np.array([10**1, 1.0, 1.0])) +FS_online_generation_problem = OCP_gen_rot(boundary_constraints,number_samples,solver=solver) # 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) -for i in range(len(new_trajectory)): - new_trajectory[i] = orthonormalize(new_trajectory[i]) +new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values=initial_values) + +# for i in range(len(new_trajectory)): +# new_trajectory[i] = orthonormalize(new_trajectory[i]) fig = plt.figure(figsize=(14,8)) ax = fig.add_subplot(111, projection='3d') @@ -135,99 +166,123 @@ def interpolate_model_invariants(demo_invariants, progress_values): 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(progress_values,new_invars[:,0],'r') 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(progress_values,(new_invars[:,1]),'r') 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(progress_values,(new_invars[:,2]),'r') plt.plot(0,0) plt.title('Torsion [rad/m]') if plt.get_backend() != 'agg': plt.show() -#%% Visualization -window_len = 20 +#%% Generation of multiple trajectories to test FATROP calculation speed -# specify optimization problem symbolically -FS_online_generation_problem = OCP_gen_rot(window_len=window_len,w_invars = 10**1*np.array([10**1, 1.0, 1.0])) +current_progress = 0 +number_samples = 100 +number_of_trajectories = 100 -current_progress = 0.0 -old_progress = 0.0 +progress_values = np.linspace(current_progress, arclength_n[-1], number_samples) +model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) -R_obj_end = calculate_trajectory[-1] # initialise R_obj_end with end point of reconstructed trajectory -iterative_trajectory = calculate_trajectory.copy() -iterative_movingframes = movingframes.copy() -trajectory_position_iter = trajectory_position.copy() +# pl.plot_interpolated_invariants(optim_calc_results.invariants, model_invariants, arclength_n, progress_values) -fig_traj = plt.figure(figsize=(14,8)) -ax = fig_traj.add_subplot(111, projection='3d') +# new constraints +current_index = round(current_progress*len(trajectory)) +R_obj_start = orthonormalize(calculate_trajectory[current_index]) +FSr_start = orthonormalize(movingframes[current_index]) +FSr_end = orthonormalize(movingframes[-1]) + +boundary_constraints = { + "orientation": { + "initial": R_obj_start, + "final": [] + }, + "moving-frame": { + "rotational": { + "initial": FSr_start, + "final": FSr_end + } + }, +} + +initial_values = { + "trajectory": { + "orientation": [], + }, + "moving-frame": { + "rotational": [] + }, + "invariants": { + "rotational": model_invariants, + } +} -fig_invars, axes = plt.subplots(1, 3, sharey=True, figsize=(10,3)) - -while current_progress <= 1.0: - - print(f"current progress = {current_progress}") +# specify optimization problem symbolically +FS_online_generation_problem = OCP_gen_rot(boundary_constraints, number_samples, solver = solver) - # Resample invariants for current progress - progress_values = np.linspace(current_progress, arclength_n[-1], window_len) - model_invariants,new_stepsize = interpolate_model_invariants(spline_model_trajectory,progress_values) - - # Boundary constraints - current_index = round( (current_progress - old_progress) * len(iterative_trajectory)) - R_obj_start = iterative_trajectory[current_index] - rotate = R.from_euler('z', 30/window_len, degrees=True) - R_obj_end = orthonormalize(rotate.apply(R_obj_end)) - R_r_start = iterative_movingframes[current_index] - R_r_end = iterative_movingframes[-1] - - # Calculate remaining trajectory - new_invars, iterative_trajectory, iterative_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) - for i in range(len(iterative_trajectory)): - iterative_trajectory[i] = orthonormalize(iterative_trajectory[i]) - - # Visualization - clear_output(wait=True) - +if show_plots: + fig = plt.figure(figsize=(14,8)) + ax = fig.add_subplot(111, projection='3d') ax.plot(trajectory_position[:,0],trajectory_position[:,1],trajectory_position[:,2],'b') - for i in indx: - plot_stl(opener_location,trajectory_position[i,:],calculate_trajectory[i,:,:],colour="c",alpha=0.2,ax=ax) - cut_trajectory_position = trajectory_position_iter[current_index:,:] - interp = ip.interp1d(np.linspace(0,1,len(cut_trajectory_position)),cut_trajectory_position, axis = 0) - trajectory_position_iter = interp(np.linspace(0,1,len(iterative_trajectory))) - indx_iter = np.trunc(np.linspace(0,len(iterative_trajectory)-1,n_frames)) - indx_iter = indx_iter.astype(int) - for i in indx_iter: - plot_3d_frame(trajectory_position_iter[i,:],iterative_trajectory[i,:,:],1,0.05,['red','green','blue'],ax) - plot_stl(opener_location,trajectory_position_iter[i,:],iterative_trajectory[i,:,:],colour="r",alpha=0.2,ax=ax) - if plt.get_backend() != 'agg': - plt.show() - - axes[0].plot(progress_values,new_invars[:,0],'r') - axes[0].plot(arclength_n,invariants[:,0],'b') - axes[0].plot(0,0) - axes[0].set_title('velocity [m/m]') - - axes[1].plot(progress_values,(new_invars[:,1]),'r') - axes[1].plot(arclength_n,invariants[:,1],'b') - axes[1].plot(0,0) - axes[1].set_title('curvature [rad/m]') + +tot_time = 0 +counter = 0 +max_time = 0 +targets = np.zeros((number_of_trajectories,1)) +for k in range(len(targets)): + targets[k] = random.uniform(0,30) + p_obj_end = targets[k,:-1] + rotate = R.from_euler('z', targets[k,-1], degrees=True) + R_obj_end = orthonormalize(rotate.apply(calculate_trajectory[-1])) + + R_obj_init = reparam.interpR(np.linspace(0, 1, len(trajectory)), [0,1], np.array([R_obj_start, R_obj_end])) + + R_r_init, R_r_init_array, invars_init = initial_trajectory_movingframe_rotation(R_obj_start, R_obj_end) + + boundary_constraints["orientation"]["final"] = R_obj_end + boundary_constraints["moving-frame"]["rotational"]["initial"] = R_r_init + boundary_constraints["moving-frame"]["rotational"]["final"] = R_r_init + + initial_values["trajectory"]["orientation"] = R_obj_init + initial_values["moving-frame"]["rotational"] = R_r_init_array - axes[2].plot(progress_values,(new_invars[:,2]),'r') - axes[2].plot(arclength_n,invariants[:,2],'b') - axes[2].plot(0,0) - axes[2].set_title('torsion [rad/m]') + # Solve + new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values) + + for i in range(len(new_trajectory)): + new_trajectory[i] = orthonormalize(new_trajectory[i]) - old_progress = current_progress - current_progress = old_progress + 1/window_len \ No newline at end of file + # if solver == 'fatrop': + # new_time = tot_time_pos + tot_time_rot + # if new_time > max_time: + # max_time = new_time + # tot_time = tot_time + new_time + + # counter += 1 + +# if solver == 'fatrop': +# print('') +# print("AVERAGE time to generate new trajectory: ") +# print(str(tot_time/counter) + "[s]") +# print('') +# print("MAXIMUM time to generate new trajectory: ") +# print(str(max_time) + "[s]") + +# fig = plt.figure(figsize=(10,6)) +# ax1 = fig.add_subplot(111, projection='3d') +# ax1 = plt.axes(projection='3d') +# ax1.plot(trajectory_position[:,0],trajectory_position[:,1],trajectory_position[:,2],'b') +# ax1.plot(targets[:,0],targets[:,1],targets[:,2],'r.') diff --git a/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars.py b/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars.py index 46f2efa..b887f39 100644 --- a/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars.py +++ b/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars.py @@ -1,11 +1,16 @@ import numpy as np import casadi as cas +from invariants_py.ocp_helper import check_solver, tril_vec, tril_vec_no_diag import invariants_py.dynamics_vector_invariants as dynamics +from invariants_py.kinematics.orientation_kinematics import rotate_x +from invariants_py.ocp_initialization import generate_initvals_from_constraints_opti class OCP_gen_rot: - def __init__(self, window_len = 100, bool_unsigned_invariants = False, w_pos = 1, w_rot = 1, w_invars = (10**-3)*np.array([1.0, 1.0, 1.0]), max_iters = 300): + def __init__(self, boundary_constraints, window_len = 100, bool_unsigned_invariants = False, solver = 'ipopt'): + # fatrop_solver = check_solver(fatrop_solver) + ''' Create decision variables and parameters for the optimization problem ''' opti = cas.Opti() # use OptiStack package from Casadi for easy bookkeeping of variables (no cumbersome indexing) @@ -14,119 +19,245 @@ def __init__(self, window_len = 100, bool_unsigned_invariants = False, w_pos = 1 R_obj = [] R_r = [] X = [] + invars = [] for k in range(window_len): - R_obj.append(opti.variable(3,3)) # object orientation R_r.append(opti.variable(3,3)) # rotational Frenet-Serret frame + R_obj.append(opti.variable(3,3)) # object orientation X.append(cas.vertcat(cas.vec(R_r[k]), cas.vec(R_obj[k]))) - - # Define system controls (invariants at every time step) - U = opti.variable(3,window_len-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) - + h = opti.parameter(1,1) # step size for integration of dynamic model + invars_demo = [] + w_invars = [] + for k in range(window_len-1): + invars_demo.append(opti.parameter(3,1)) # model invariants + w_invars.append(opti.parameter(3,1)) # weights for invariants + # Boundary values - R_r_start = opti.parameter(3,3) - R_r_end = opti.parameter(3,3) - R_obj_start = opti.parameter(3,3) - R_obj_end = opti.parameter(3,3) - - U_demo = opti.parameter(3,window_len-1) # model invariants + if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: + R_obj_start = opti.parameter(3,3) + if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: + R_obj_end = opti.parameter(3,3) + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: + R_r_start = opti.parameter(3,3) + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: + R_r_end = opti.parameter(3,3) ''' Specifying the constraints ''' - # Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator) - opti.subject_to( R_r[0].T @ R_r[0] == np.eye(3)) - opti.subject_to( R_obj[0].T @ R_obj[0] == np.eye(3)) - # Boundary constraints - opti.subject_to(R_r[0] == R_r_start) - opti.subject_to(R_r[-1] == R_r_end) - opti.subject_to(R_obj[0] == R_obj_start) - opti.subject_to(R_obj[-1] == R_obj_end) # Dynamic constraints integrator = dynamics.define_integrator_invariants_rotation(h) for k in range(window_len-1): # Integrate current state to obtain next state (next rotation and position) - Xk_end = integrator(X[k],U[:,k],h) + 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 objective_fit = 0 for k in range(window_len-1): - err_invars = w_invars*(U[:,k] - U_demo[:,k]) - objective_fit = objective_fit + cas.dot(err_invars,err_invars) - # objective_fit = objective_fit + 1/window_len*cas.dot(err_invars,err_invars) + err_invars = w_invars[k]*(invars[k] - invars_demo[k]) + # objective_fit = objective_fit + cas.dot(err_invars,err_invars) + objective_fit = objective_fit + 1/window_len*cas.dot(err_invars,err_invars) objective = objective_fit ''' Define solver and save variables ''' opti.minimize(objective) - opti.solver('ipopt',{"print_time":True,"expand":True},{'tol':1e-4,'print_level':0,'ma57_automatic_scaling':'no','linear_solver':'mumps','max_iter':100}) + if solver == 'ipopt': + 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'}) + elif solver == 'fatrop': + opti.solver('fatrop',{"expand":True,'fatrop.max_iter':300,'fatrop.tol':1e-6,'fatrop.print_level':5, "structure_detection":"auto","debug":True,"fatrop.mu_init":0.1}) + + # Solve already once with dummy measurements + for k in range(window_len): + opti.set_initial(R_r[k], np.eye(3)) + opti.set_initial(R_obj[k], np.eye(3)) + for k in range(window_len-1): + opti.set_initial(invars[k], np.array([1,0.01,0.01])) + opti.set_value(invars_demo[k], 0.001+np.zeros(3)) + opti.set_value(w_invars[k], 0.001+np.zeros(3)) + opti.set_value(h,0.1) + # Boundary constraints + if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: + opti.set_value(R_obj_start, np.eye(3)) + if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: + opti.set_value(R_obj_end, rotate_x(np.pi/4)) + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: + opti.set_value(R_r_start, np.eye(3)) + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: + opti.set_value(R_r_end, np.eye(3)) + sol = opti.solve_limited() + bounds = [] + bounds_labels = [] + # Boundary constraints + if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: + bounds.append(R_obj_start) + bounds_labels.append("R_obj_start") + if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: + bounds.append(R_obj_end) + bounds_labels.append("R_obj_end") + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: + bounds.append(R_r_start) + bounds_labels.append("R_r_start") + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: + bounds.append(R_r_end) + bounds_labels.append("R_r_end") + + # Construct a CasADi function out of the opti object. This function can be called with the initial guess to solve the NLP. Faster than doing opti.set_initial + opti.solve + opti.value + solution = [*invars, *R_obj, *R_r] + self.opti_function = opti.to_function('opti_function', + [h,*invars_demo,*w_invars,*bounds,*solution], # inputs + [*solution]) #outputs + # ["h_value","invars_model","weights",*bounds_labels,"invars1","R_obj1","R_r1"], # input labels for debugging + # ["invars2","R_obj2","R_r2"]) # output labels for debugging + # Save variables self.R_r = R_r self.R_obj = R_obj - self.U = U - self.U_demo = U_demo - self.R_r_start = R_r_start - self.R_r_end = R_r_end - self.R_obj_start = R_obj_start - self.R_obj_end = R_obj_end + self.invars = invars + self.invars_demo = invars_demo + self.w_ivars = w_invars + if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: + self.R_obj_start = R_obj_start + if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: + self.R_obj_end = R_obj_end + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: + self.R_r_start = R_r_start + if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: + self.R_r_end = R_r_end self.h = h self.window_len = window_len self.opti = opti - - - def generate_trajectory(self,U_demo,R_obj_init,R_r_init,R_r_start,R_r_end,R_obj_start,R_obj_end,step_size): - - - N = self.window_len - - # Initialize states - for k in range(N): - self.opti.set_initial(self.R_r[k], R_r_init[k]) - self.opti.set_initial(self.R_obj[k], R_obj_init[k]) - - # Initialize controls - for k in range(N-1): - self.opti.set_initial(self.U[:,k], U_demo[k,:]) - - # Set values boundary constraints - self.opti.set_value(self.R_r_start,R_r_start) - self.opti.set_value(self.R_r_end,R_r_end) - self.opti.set_value(self.R_obj_start,R_obj_start) - self.opti.set_value(self.R_obj_end,R_obj_end) - - # Set values parameters - self.opti.set_value(self.h,step_size) - for k in range(N-1): - self.opti.set_value(self.U_demo[:,k], U_demo[k,:]) - - # ###################### - # ## DEBUGGING: check integrator in initial values, time step 0 to 1 - # x0 = cas.vertcat(cas.vec(np.eye(3,3)), cas.vec(measured_positions[0])) - # u0 = 1e-8*np.ones((3,1)) - # integrator = dynamics.define_integrator_invariants_position(self.stepsize) - # x1 = integrator(x0,u0) - # print(x1) - # ###################### - - # Solve the NLP - sol = self.opti.solve_limited() + self.first_window = True self.sol = sol + self.solver = solver + + def generate_trajectory(self, invariant_model, boundary_constraints, step_size, weights_params = {}, initial_values = {}): + N = invariant_model.shape[0] + # Get the weights for the invariants or set default values + w_invars = weights_params.get('w_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) + w_high_start = weights_params.get('w_high_start', 1) + w_high_end = weights_params.get('w_high_end', 0) + w_high_invars = weights_params.get('w_high_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) + w_high_active = weights_params.get('w_high_active', 0) + + # Set the weights for the invariants + w_invars = np.tile(w_invars, (len(invariant_model),1)).T + if w_high_active: + w_invars[:, w_high_start:w_high_end+1] = w_high_invars.reshape(-1, 1) + + to_skip = ("position","translational") + boundary_values_list = [] + sublist_counter = 0 + subsublist_counter = 0 + for sublist in boundary_constraints.values(): + if list(boundary_constraints.keys())[sublist_counter] not in to_skip: + try: + for subsublist in sublist.values(): + if list(sublist.keys())[subsublist_counter] not in to_skip: + for value in subsublist.values(): + boundary_values_list.append(value) + subsublist_counter += 1 + except: + if list(sublist.keys())[subsublist_counter] not in to_skip: + for value in sublist.values(): + boundary_values_list.append(value) + sublist_counter += 1 + subsublist_counter = 0 + + if self.first_window and not initial_values: + self.solution = 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"]["rotational"][:N-1,:], *initial_values["trajectory"]["orientation"][:N], *initial_values["moving-frame"]["rotational"][:N]] + self.first_window = False + + # Call solve function + self.solution = self.opti_function(step_size,*invariant_model[:-1],*w_invars[:,:-1].T,*boundary_values_list,*self.solution) + + # Return the results + invars = np.zeros((N-1,3)) + R_obj_sol = np.zeros((N,3,3)) + R_r_sol = np.zeros((N,3,3)) + for i in range(N): # unpack the results + if i!= N-1: + invars[i,:] = self.solution[i].T + R_obj_sol[i,:,:] = self.solution[N-1+i] + R_r_sol[i,:,:] = self.solution[2*N-1+i] + # Extract the solved variables - invariants = sol.value(self.U).T + invariants = np.array(invars) invariants = np.vstack((invariants,[invariants[-1,:]])) - calculated_trajectory = np.array([sol.value(i) for i in self.R_obj]) - calculated_movingframe = np.array([sol.value(i) for i in self.R_r]) - + calculated_trajectory = np.array(R_obj_sol) + calculated_movingframe = np.array(R_r_sol) + return invariants, calculated_trajectory, calculated_movingframe + +if __name__ == "__main__": + + # Randomly chosen data + N = 100 + invariant_model = np.ones((N,3))*[1,0.001,0.001] + + # Boundary constraints + boundary_constraints = { + "orientation": { + "initial": np.eye(3), + "final": rotate_x(np.pi/6) + }, + "moving-frame": { + "rotational": { + "initial": np.eye(3), + "final": np.eye(3) + } + }, + } + step_size = 0.1 + + # Create an instance of OCP_gen_pos + 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) + + # Print the results + # print("Invariants:", invariants) + #print("Calculated Trajectory:", calculated_trajectory) + # print("Calculated Moving Frame:", calculated_movingframe) + + # Second call to generate_trajectory + boundary_constraints["orientation"]["initial"] = boundary_constraints["orientation"]["final"] + boundary_constraints["orientation"]["final"] = rotate_x(np.pi/8) + invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) + + # Print the results + #print("Invariants:", invariants) + print("Calculated Trajectory:", calculated_trajectory) + #print("Calculated Moving Frame:", calculated_movingframe) diff --git a/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars_fatrop.py b/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars_fatrop.py deleted file mode 100644 index b887f39..0000000 --- a/invariants_py/generate_trajectory/opti_generate_orientation_traj_from_vector_invars_fatrop.py +++ /dev/null @@ -1,263 +0,0 @@ -import numpy as np -import casadi as cas -from invariants_py.ocp_helper import check_solver, tril_vec, tril_vec_no_diag -import invariants_py.dynamics_vector_invariants as dynamics -from invariants_py.kinematics.orientation_kinematics import rotate_x -from invariants_py.ocp_initialization import generate_initvals_from_constraints_opti - -class OCP_gen_rot: - - def __init__(self, boundary_constraints, window_len = 100, bool_unsigned_invariants = False, solver = 'ipopt'): - - # fatrop_solver = check_solver(fatrop_solver) - - ''' Create decision variables and parameters for the optimization problem ''' - - 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) - R_obj = [] - R_r = [] - X = [] - invars = [] - for k in range(window_len): - R_r.append(opti.variable(3,3)) # rotational Frenet-Serret frame - R_obj.append(opti.variable(3,3)) # object orientation - X.append(cas.vertcat(cas.vec(R_r[k]), cas.vec(R_obj[k]))) - 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 - invars_demo = [] - w_invars = [] - for k in range(window_len-1): - invars_demo.append(opti.parameter(3,1)) # model invariants - w_invars.append(opti.parameter(3,1)) # weights for invariants - - # Boundary values - if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: - R_obj_start = opti.parameter(3,3) - if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: - R_obj_end = opti.parameter(3,3) - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: - R_r_start = opti.parameter(3,3) - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: - R_r_end = opti.parameter(3,3) - - - ''' Specifying the constraints ''' - - - - # Dynamic constraints - integrator = dynamics.define_integrator_invariants_rotation(h) - for k in range(window_len-1): - # Integrate current state to obtain next state (next rotation and position) - Xk_end = integrator(X[k],invars[k],h) - - # Gap closing constraint - 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 - objective_fit = 0 - for k in range(window_len-1): - err_invars = w_invars[k]*(invars[k] - invars_demo[k]) - # objective_fit = objective_fit + cas.dot(err_invars,err_invars) - objective_fit = objective_fit + 1/window_len*cas.dot(err_invars,err_invars) - - objective = objective_fit - - ''' Define solver and save variables ''' - opti.minimize(objective) - if solver == 'ipopt': - 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'}) - elif solver == 'fatrop': - opti.solver('fatrop',{"expand":True,'fatrop.max_iter':300,'fatrop.tol':1e-6,'fatrop.print_level':5, "structure_detection":"auto","debug":True,"fatrop.mu_init":0.1}) - - # Solve already once with dummy measurements - for k in range(window_len): - opti.set_initial(R_r[k], np.eye(3)) - opti.set_initial(R_obj[k], np.eye(3)) - for k in range(window_len-1): - opti.set_initial(invars[k], np.array([1,0.01,0.01])) - opti.set_value(invars_demo[k], 0.001+np.zeros(3)) - opti.set_value(w_invars[k], 0.001+np.zeros(3)) - opti.set_value(h,0.1) - # Boundary constraints - if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: - opti.set_value(R_obj_start, np.eye(3)) - if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: - opti.set_value(R_obj_end, rotate_x(np.pi/4)) - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: - opti.set_value(R_r_start, np.eye(3)) - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: - opti.set_value(R_r_end, np.eye(3)) - sol = opti.solve_limited() - - bounds = [] - bounds_labels = [] - # Boundary constraints - if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: - bounds.append(R_obj_start) - bounds_labels.append("R_obj_start") - if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: - bounds.append(R_obj_end) - bounds_labels.append("R_obj_end") - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: - bounds.append(R_r_start) - bounds_labels.append("R_r_start") - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: - bounds.append(R_r_end) - bounds_labels.append("R_r_end") - - # Construct a CasADi function out of the opti object. This function can be called with the initial guess to solve the NLP. Faster than doing opti.set_initial + opti.solve + opti.value - solution = [*invars, *R_obj, *R_r] - self.opti_function = opti.to_function('opti_function', - [h,*invars_demo,*w_invars,*bounds,*solution], # inputs - [*solution]) #outputs - # ["h_value","invars_model","weights",*bounds_labels,"invars1","R_obj1","R_r1"], # input labels for debugging - # ["invars2","R_obj2","R_r2"]) # output labels for debugging - - # Save variables - self.R_r = R_r - self.R_obj = R_obj - self.invars = invars - self.invars_demo = invars_demo - self.w_ivars = w_invars - if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: - self.R_obj_start = R_obj_start - if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: - self.R_obj_end = R_obj_end - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]: - self.R_r_start = R_r_start - if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]: - self.R_r_end = R_r_end - self.h = h - self.window_len = window_len - self.opti = opti - self.first_window = True - self.sol = sol - self.solver = solver - - def generate_trajectory(self, invariant_model, boundary_constraints, step_size, weights_params = {}, initial_values = {}): - - N = invariant_model.shape[0] - - # Get the weights for the invariants or set default values - w_invars = weights_params.get('w_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) - w_high_start = weights_params.get('w_high_start', 1) - w_high_end = weights_params.get('w_high_end', 0) - w_high_invars = weights_params.get('w_high_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) - w_high_active = weights_params.get('w_high_active', 0) - - # Set the weights for the invariants - w_invars = np.tile(w_invars, (len(invariant_model),1)).T - if w_high_active: - w_invars[:, w_high_start:w_high_end+1] = w_high_invars.reshape(-1, 1) - - to_skip = ("position","translational") - boundary_values_list = [] - sublist_counter = 0 - subsublist_counter = 0 - for sublist in boundary_constraints.values(): - if list(boundary_constraints.keys())[sublist_counter] not in to_skip: - try: - for subsublist in sublist.values(): - if list(sublist.keys())[subsublist_counter] not in to_skip: - for value in subsublist.values(): - boundary_values_list.append(value) - subsublist_counter += 1 - except: - if list(sublist.keys())[subsublist_counter] not in to_skip: - for value in sublist.values(): - boundary_values_list.append(value) - sublist_counter += 1 - subsublist_counter = 0 - - if self.first_window and not initial_values: - self.solution = 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"]["rotational"][:N-1,:], *initial_values["trajectory"]["orientation"][:N], *initial_values["moving-frame"]["rotational"][:N]] - self.first_window = False - - # Call solve function - self.solution = self.opti_function(step_size,*invariant_model[:-1],*w_invars[:,:-1].T,*boundary_values_list,*self.solution) - - # Return the results - invars = np.zeros((N-1,3)) - R_obj_sol = np.zeros((N,3,3)) - R_r_sol = np.zeros((N,3,3)) - for i in range(N): # unpack the results - if i!= N-1: - invars[i,:] = self.solution[i].T - R_obj_sol[i,:,:] = self.solution[N-1+i] - R_r_sol[i,:,:] = self.solution[2*N-1+i] - - # Extract the solved variables - invariants = np.array(invars) - invariants = np.vstack((invariants,[invariants[-1,:]])) - calculated_trajectory = np.array(R_obj_sol) - calculated_movingframe = np.array(R_r_sol) - - return invariants, calculated_trajectory, calculated_movingframe - -if __name__ == "__main__": - - # Randomly chosen data - N = 100 - invariant_model = np.ones((N,3))*[1,0.001,0.001] - - # Boundary constraints - boundary_constraints = { - "orientation": { - "initial": np.eye(3), - "final": rotate_x(np.pi/6) - }, - "moving-frame": { - "rotational": { - "initial": np.eye(3), - "final": np.eye(3) - } - }, - } - step_size = 0.1 - - # Create an instance of OCP_gen_pos - 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) - - # Print the results - # print("Invariants:", invariants) - #print("Calculated Trajectory:", calculated_trajectory) - # print("Calculated Moving Frame:", calculated_movingframe) - - # Second call to generate_trajectory - boundary_constraints["orientation"]["initial"] = boundary_constraints["orientation"]["final"] - boundary_constraints["orientation"]["final"] = rotate_x(np.pi/8) - invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) - - # Print the results - #print("Invariants:", invariants) - print("Calculated Trajectory:", calculated_trajectory) - #print("Calculated Moving Frame:", calculated_movingframe) diff --git a/invariants_py/generate_trajectory/opti_generate_pose_traj_from_vector_invars_fatrop.py b/invariants_py/generate_trajectory/opti_generate_pose_traj_from_vector_invars.py similarity index 100% rename from invariants_py/generate_trajectory/opti_generate_pose_traj_from_vector_invars_fatrop.py rename to invariants_py/generate_trajectory/opti_generate_pose_traj_from_vector_invars.py diff --git a/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars.py b/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars.py index 732b039..3bbff6b 100644 --- a/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars.py +++ b/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars.py @@ -1,129 +1,274 @@ import numpy as np import casadi as cas import invariants_py.dynamics_vector_invariants as dynamics -from invariants_py import ocp_helper -from invariants_py.ocp_initialization import generate_initvals_from_constraints +from invariants_py.ocp_helper import check_solver, tril_vec, tril_vec_no_diag +from invariants_py.ocp_initialization import generate_initvals_from_constraints_opti from invariants_py import spline_handler as sh class OCP_gen_pos: - def __init__(self, N = 40, w_invars = (10**-3)*np.array([1.0, 1.0, 1.0])): + def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = False, solver = 'ipopt'): + + # fatrop_solver = check_solver(fatrop_solver) + ''' Create decision variables and parameters for the optimization problem ''' opti = cas.Opti() # use OptiStack package from Casadi for easy bookkeeping of variables # Define system states X (unknown object pose + moving frame pose at every time step) - p_obj = [opti.variable(3,1) for _ in range(N)] # object position - R_t = [opti.variable(3,3) for _ in range(N)] # translational Frenet-Serret frame - X = [cas.vertcat(cas.vec(R_t[k]), cas.vec(p_obj[k])) for k in range(N)] - - # Define system controls (invariants at every time step) - U = opti.variable(3,N-1) # invariants + p_obj = [] + R_t = [] + X = [] + invars = [] + for k in range(N): + R_t.append(opti.variable(3,3)) # translational Frenet-Serret frame + p_obj.append(opti.variable(3,1)) # object position + X.append(cas.vertcat(cas.vec(R_t[k]), cas.vec(p_obj[k]))) + if k < N-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 - U_demo = opti.parameter(3,N-1) # model invariants + invars_demo = [] + w_invars = [] + for k in range(N-1): + invars_demo.append(opti.parameter(3,1)) # model invariants + w_invars.append(opti.parameter(3,1)) # weights for invariants # Boundary values - R_t_start = opti.parameter(3,3) - R_t_end = opti.parameter(3,3) - p_obj_start = opti.parameter(3,1) - p_obj_end = opti.parameter(3,1) + if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: + p_obj_start = opti.parameter(3,1) + if "position" in boundary_constraints and "final" in boundary_constraints["position"]: + p_obj_end = opti.parameter(3,1) + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: + R_t_start = opti.parameter(3,3) + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: + R_t_end = opti.parameter(3,3) ''' Specifying the constraints ''' - # Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator) - opti.subject_to(ocp_helper.tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0) - - # Boundary constraints - #opti.subject_to(R_t[0] == R_t_start) - #opti.subject_to(R_t[-1] == R_t_end) - opti.subject_to(p_obj[0] == p_obj_start) - opti.subject_to(p_obj[-1] == p_obj_end) # Dynamic constraints integrator = dynamics.define_integrator_invariants_position(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 = 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) + + # # Lower bounds on controls + # if bool_unsigned_invariants: + # opti.subject_to(invars[k][0]>=0) # lower bounds on control + # opti.subject_to(invars[k][1]>=0) # lower bounds on control + + # Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator) + if k == 0: + opti.subject_to(tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0) + # Boundary constraints + if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: + opti.subject_to(p_obj[0] == p_obj_start) + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: + opti.subject_to(tril_vec_no_diag(R_t[0].T @ R_t_start - np.eye(3)) == 0.) + if "position" in boundary_constraints and "final" in boundary_constraints["position"]: + opti.subject_to(p_obj[-1] == p_obj_end) + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: + opti.subject_to(tril_vec_no_diag(R_t[-1].T @ R_t_end - np.eye(3)) == 0.) ''' Specifying the objective ''' # Fitting constraint to remain close to measurements objective_fit = 0 for k in range(N-1): - err_invars = w_invars*(U[:,k] - U_demo[:,k]) + err_invars = w_invars[k]*(invars[k] - invars_demo[k]) objective_fit = objective_fit + 1/N*cas.dot(err_invars,err_invars) objective = objective_fit ''' Define solver and save variables ''' opti.minimize(objective) - opti.solver('ipopt',{"print_time":True},{'print_level':5,'max_iter':100}) - #opti.solver('ipopt',{"print_time":True,"expand":True},{'gamma_theta':1e-12,'tol':1e-6,'print_level':5,'ma57_automatic_scaling':'no','linear_solver':'mumps','max_iter':100}) - + + if solver == 'ipopt': + 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'}) + elif solver == 'fatrop': + opti.solver('fatrop',{"expand":True,'fatrop.max_iter':300,'fatrop.tol':1e-6,'fatrop.print_level':5, "structure_detection":"auto","debug":True,"fatrop.mu_init":0.1}) + # ocp._method.set_name("/codegen/generation_position") + + # Solve already once with dummy measurements + for k in range(N): + opti.set_initial(R_t[k], np.eye(3)) + opti.set_initial(p_obj[k], np.zeros(3)) + for k in range(N-1): + opti.set_initial(invars[k], 0.001+np.zeros(3)) + opti.set_value(invars_demo[k], 0.001+np.zeros(3)) + opti.set_value(w_invars[k], 0.001+np.zeros(3)) + opti.set_value(h,0.1) + # Boundary constraints + if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: + opti.set_value(p_obj_start, np.array([0,0,0])) + if "position" in boundary_constraints and "final" in boundary_constraints["position"]: + opti.set_value(p_obj_end, np.array([1,0,0])) + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: + opti.set_value(R_t_start, np.eye(3)) + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: + opti.set_value(R_t_end, np.eye(3)) + sol = opti.solve_limited() + + bounds = [] + bounds_labels = [] + # Boundary constraints + if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: + bounds.append(p_obj_start) + bounds_labels.append("p_obj_start") + if "position" in boundary_constraints and "final" in boundary_constraints["position"]: + bounds.append(p_obj_end) + bounds_labels.append("p_obj_end") + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: + bounds.append(R_t_start) + bounds_labels.append("R_t_start") + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: + bounds.append(R_t_end) + bounds_labels.append("R_t_end") + + # Construct a CasADi function out of the opti object. This function can be called with the initial guess to solve the NLP. Faster than doing opti.set_initial + opti.solve + opti.value + solution = [*invars, *p_obj, *R_t] + self.opti_function = opti.to_function('opti_function', + [h,*invars_demo,*w_invars,*bounds,*solution], # inputs + [*solution]) #outputs + # ["h_value","invars_model","weights",*bounds_labels,"invars1","p_obj1","R_t1"], # input labels for debugging + # ["invars2","p_obj2","R_t2"]) # output labels for debugging + + # Save variables self.R_t = R_t self.p_obj = p_obj - self.U = U - self.U_demo = U_demo - self.R_t_start = R_t_start - self.R_t_end = R_t_end - self.p_obj_start = p_obj_start - self.p_obj_end = p_obj_end + self.invars = invars + self.invars_demo = invars_demo + self.w_ivars = w_invars + if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: + self.p_obj_start = p_obj_start + if "position" in boundary_constraints and "final" in boundary_constraints["position"]: + self.p_obj_end = p_obj_end + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: + self.R_t_start = R_t_start + if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: + self.R_t_end = R_t_end self.h = h self.N = N self.opti = opti + self.first_window = True + self.sol = sol + self.solver = solver + + + def generate_trajectory(self, invariant_model, boundary_constraints, step_size, weights_params = {}, initial_values = {}): + + N = invariant_model.shape[0] + # Get the weights for the invariants or set default values + w_invars = weights_params.get('w_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) + w_high_start = weights_params.get('w_high_start', N) + w_high_end = weights_params.get('w_high_end', N) + w_high_invars = weights_params.get('w_high_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) + w_high_active = weights_params.get('w_high_active', 0) + + # Set the weights for the invariants + w_invars = np.tile(w_invars, (len(invariant_model),1)).T + if w_high_active: + w_invars[:, w_high_start:w_high_end+1] = w_high_invars.reshape(-1, 1) + + to_skip = ("orientation","rotational") + boundary_values_list = [] + sublist_counter = 0 + subsublist_counter = 0 + for sublist in boundary_constraints.values(): + if list(boundary_constraints.keys())[sublist_counter] not in to_skip: + try: + for subsublist in sublist.values(): + if list(sublist.keys())[subsublist_counter] not in to_skip: + for value in subsublist.values(): + boundary_values_list.append(value) + subsublist_counter += 1 + except: + if list(sublist.keys())[subsublist_counter] not in to_skip: + for value in sublist.values(): + boundary_values_list.append(value) + sublist_counter += 1 + subsublist_counter = 0 + + if self.first_window and not initial_values: + 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,:], *initial_values["trajectory"]["position"][:N,:], *initial_values["moving-frame"]["translational"][:N]] + self.first_window = False + + # Call solve function + self.solution = self.opti_function(step_size,*invariant_model[:-1],*w_invars[:,:-1].T,*boundary_values_list,*self.solution) + + # Return the results + invars = np.zeros((N-1,3)) + p_obj_sol = np.zeros((N,3)) + R_t_sol = np.zeros((3,3,N)) + for i in range(N): # unpack the results + if i!= N-1: + invars[i,:] = self.solution[i].T + p_obj_sol[i,:] = self.solution[N-1+i].T + R_t_sol = self.solution[2*N-1+i] + + # Extract the solved variables + invariants = np.array(invars) + invariants = np.vstack((invariants,[invariants[-1,:]])) + calculated_trajectory = np.array(p_obj_sol) + calculated_movingframe = np.array(R_t_sol) + + return invariants, calculated_trajectory, calculated_movingframe - def generate_trajectory(self,U_demo,p_obj_init,R_t_init,R_t_start,R_t_end,p_obj_start,p_obj_end,step_size): + # def generate_trajectory(self,invars_demo,p_obj_init,R_t_init,R_t_start,R_t_end,p_obj_start,p_obj_end,step_size): - N = self.N + # N = self.N - # Initialize states - for k in range(N): - self.opti.set_initial(self.R_t[k], R_t_init[k]) - self.opti.set_initial(self.p_obj[k], p_obj_init[k]) + # # Initialize states + # for k in range(N): + # self.opti.set_initial(self.R_t[k], R_t_init[k]) + # self.opti.set_initial(self.p_obj[k], p_obj_init[k]) - # Initialize controls - for k in range(N-1): - self.opti.set_initial(self.U[:,k], U_demo[k,:]) + # # Initialize controls + # for k in range(N-1): + # self.opti.set_initial(self.invars[:,k], invars_demo[k,:]) - # Set values boundary constraints - self.opti.set_value(self.R_t_start,R_t_start) - self.opti.set_value(self.R_t_end,R_t_end) - self.opti.set_value(self.p_obj_start,p_obj_start) - self.opti.set_value(self.p_obj_end,p_obj_end) + # # Set values boundary constraints + # self.opti.set_value(self.R_t_start,R_t_start) + # self.opti.set_value(self.R_t_end,R_t_end) + # self.opti.set_value(self.p_obj_start,p_obj_start) + # self.opti.set_value(self.p_obj_end,p_obj_end) - # Set values parameters - self.opti.set_value(self.h,step_size) - for k in range(N-1): - self.opti.set_value(self.U_demo[:,k], U_demo[k,:]) + # # Set values parameters + # self.opti.set_value(self.h,step_size) + # for k in range(N-1): + # self.opti.set_value(self.invars_demo[:,k], invars_demo[k,:]) - # ###################### - # ## DEBUGGING: check integrator in initial values, time step 0 to 1 - # x0 = cas.vertcat(cas.vec(np.eye(3,3)), cas.vec(measured_positions[0])) - # u0 = 1e-8*np.ones((3,1)) - # integrator = dynamics.define_integrator_invariants_position(self.stepsize) - # x1 = integrator(x0,u0) - # print(x1) - # ###################### + # # ###################### + # # ## DEBUGGING: check integrator in initial values, time step 0 to 1 + # # x0 = cas.vertcat(cas.vec(np.eye(3,3)), cas.vec(measured_positions[0])) + # # u0 = 1e-8*np.ones((3,1)) + # # integrator = dynamics.define_integrator_invariants_position(self.stepsize) + # # x1 = integrator(x0,u0) + # # print(x1) + # # ###################### - # Solve the NLP - sol = self.opti.solve_limited() - self.sol = sol + # # Solve the NLP + # sol = self.opti.solve_limited() + # self.sol = sol - # Extract the solved variables - invariants = sol.value(self.U).T - invariants = np.vstack((invariants,[invariants[-1,:]])) - calculated_trajectory = np.array([sol.value(i) for i in self.p_obj]) - calculated_movingframe = np.array([sol.value(i) for i in self.R_t]) + # # Extract the solved variables + # invariants = sol.value(self.invars).T + # invariants = np.vstack((invariants,[invariants[-1,:]])) + # calculated_trajectory = np.array([sol.value(i) for i in self.p_obj]) + # calculated_movingframe = np.array([sol.value(i) for i in self.R_t]) - return invariants, calculated_trajectory, calculated_movingframe + # return invariants, calculated_trajectory, calculated_movingframe - def generate_trajectory_global(self,U_demo,initial_values,boundary_constraints,step_size): + def generate_trajectory_global(self,invars_demo,initial_values,boundary_constraints,step_size): N = self.N @@ -134,7 +279,7 @@ def generate_trajectory_global(self,U_demo,initial_values,boundary_constraints,s # Initialize controls for k in range(N-1): - self.opti.set_initial(self.U[:,k], U_demo[k,:]) + self.opti.set_initial(self.invars[:,k], invars_demo[k,:]) # Set values boundary constraints #self.opti.set_value(self.R_t_start,boundary_constraints["moving-frame"]["translational"]["initial"]) @@ -145,14 +290,14 @@ def generate_trajectory_global(self,U_demo,initial_values,boundary_constraints,s # Set values parameters self.opti.set_value(self.h,step_size) for k in range(N-1): - self.opti.set_value(self.U_demo[:,k], U_demo[k,:]) + self.opti.set_value(self.invars_demo[:,k], invars_demo[k,:]) # Solve the NLP sol = self.opti.solve_limited() self.sol = sol # Extract the solved variables - invariants = sol.value(self.U).T + invariants = sol.value(self.invars).T invariants = np.vstack((invariants,[invariants[-1,:]])) calculated_trajectory = np.array([sol.value(i) for i in self.p_obj]) calculated_movingframe = np.array([sol.value(i) for i in self.R_t]) @@ -165,7 +310,7 @@ def generate_trajectory_translation(invariant_model, boundary_constraints, N=100 OCP = OCP_gen_pos(N = N) # Initial values - initial_values, initial_values_dict = generate_initvals_from_constraints(boundary_constraints, N) + initial_values, initial_values_dict = generate_initvals_from_constraints_opti(boundary_constraints, N) # Resample model invariants to desired number of N samples spline_invariant_model = sh.create_spline_model(invariant_model[:,0], invariant_model[:,1:]) @@ -177,3 +322,44 @@ def generate_trajectory_translation(invariant_model, boundary_constraints, N=100 return invariants, trajectory, mf, progress_values +if __name__ == "__main__": + + # Randomly chosen data + N = 100 + invariant_model = np.zeros((N,3)) + + # Boundary constraints + boundary_constraints = { + "position": { + "initial": np.array([0, 0, 0]), + "final": np.array([1, 0, 0]) + }, + "moving-frame": { + "translational": { + "initial": np.eye(3), + "final": np.eye(3) + } + }, + } + step_size = 0.1 + + # Create an instance of OCP_gen_pos + ocp = OCP_gen_pos(boundary_constraints,solver='ipopt', N=N) + + # Call the generate_trajectory function + invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) + + # Print the results + # print("Invariants:", invariants) + #print("Calculated Trajectory:", calculated_trajectory) + # print("Calculated Moving Frame:", calculated_movingframe) + + # Second call to generate_trajectory + boundary_constraints["position"]["initial"] = np.array([1, 0, 0]) + boundary_constraints["position"]["final"] = np.array([1, 2, 2]) + invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) + + # Print the results + #print("Invariants:", invariants) + print("Calculated Trajectory:", calculated_trajectory) + #print("Calculated Moving Frame:", calculated_movingframe) \ No newline at end of file diff --git a/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars_fatrop.py b/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars_fatrop.py deleted file mode 100644 index 3bbff6b..0000000 --- a/invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars_fatrop.py +++ /dev/null @@ -1,365 +0,0 @@ -import numpy as np -import casadi as cas -import invariants_py.dynamics_vector_invariants as dynamics -from invariants_py.ocp_helper import check_solver, tril_vec, tril_vec_no_diag -from invariants_py.ocp_initialization import generate_initvals_from_constraints_opti -from invariants_py import spline_handler as sh - -class OCP_gen_pos: - - def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = False, solver = 'ipopt'): - - # fatrop_solver = check_solver(fatrop_solver) - - ''' Create decision variables and parameters for the optimization problem ''' - opti = cas.Opti() # use OptiStack package from Casadi for easy bookkeeping of variables - - # Define system states X (unknown object pose + moving frame pose at every time step) - p_obj = [] - R_t = [] - X = [] - invars = [] - for k in range(N): - R_t.append(opti.variable(3,3)) # translational Frenet-Serret frame - p_obj.append(opti.variable(3,1)) # object position - X.append(cas.vertcat(cas.vec(R_t[k]), cas.vec(p_obj[k]))) - if k < N-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 - invars_demo = [] - w_invars = [] - for k in range(N-1): - invars_demo.append(opti.parameter(3,1)) # model invariants - w_invars.append(opti.parameter(3,1)) # weights for invariants - - # Boundary values - if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: - p_obj_start = opti.parameter(3,1) - if "position" in boundary_constraints and "final" in boundary_constraints["position"]: - p_obj_end = opti.parameter(3,1) - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: - R_t_start = opti.parameter(3,3) - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: - R_t_end = opti.parameter(3,3) - - ''' Specifying the constraints ''' - - - # Dynamic constraints - integrator = dynamics.define_integrator_invariants_position(h) - for k in range(N-1): - # Integrate current state to obtain next state (next rotation and position) - Xk_end = integrator(X[k],invars[k],h) - - # Gap closing constraint - opti.subject_to(X[k+1]==Xk_end) - - # # Lower bounds on controls - # if bool_unsigned_invariants: - # opti.subject_to(invars[k][0]>=0) # lower bounds on control - # opti.subject_to(invars[k][1]>=0) # lower bounds on control - - # Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator) - if k == 0: - opti.subject_to(tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0) - # Boundary constraints - if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: - opti.subject_to(p_obj[0] == p_obj_start) - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: - opti.subject_to(tril_vec_no_diag(R_t[0].T @ R_t_start - np.eye(3)) == 0.) - if "position" in boundary_constraints and "final" in boundary_constraints["position"]: - opti.subject_to(p_obj[-1] == p_obj_end) - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: - opti.subject_to(tril_vec_no_diag(R_t[-1].T @ R_t_end - np.eye(3)) == 0.) - - ''' Specifying the objective ''' - - # Fitting constraint to remain close to measurements - objective_fit = 0 - for k in range(N-1): - err_invars = w_invars[k]*(invars[k] - invars_demo[k]) - objective_fit = objective_fit + 1/N*cas.dot(err_invars,err_invars) - objective = objective_fit - - ''' Define solver and save variables ''' - opti.minimize(objective) - - if solver == 'ipopt': - 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'}) - elif solver == 'fatrop': - opti.solver('fatrop',{"expand":True,'fatrop.max_iter':300,'fatrop.tol':1e-6,'fatrop.print_level':5, "structure_detection":"auto","debug":True,"fatrop.mu_init":0.1}) - # ocp._method.set_name("/codegen/generation_position") - - # Solve already once with dummy measurements - for k in range(N): - opti.set_initial(R_t[k], np.eye(3)) - opti.set_initial(p_obj[k], np.zeros(3)) - for k in range(N-1): - opti.set_initial(invars[k], 0.001+np.zeros(3)) - opti.set_value(invars_demo[k], 0.001+np.zeros(3)) - opti.set_value(w_invars[k], 0.001+np.zeros(3)) - opti.set_value(h,0.1) - # Boundary constraints - if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: - opti.set_value(p_obj_start, np.array([0,0,0])) - if "position" in boundary_constraints and "final" in boundary_constraints["position"]: - opti.set_value(p_obj_end, np.array([1,0,0])) - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: - opti.set_value(R_t_start, np.eye(3)) - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: - opti.set_value(R_t_end, np.eye(3)) - sol = opti.solve_limited() - - bounds = [] - bounds_labels = [] - # Boundary constraints - if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: - bounds.append(p_obj_start) - bounds_labels.append("p_obj_start") - if "position" in boundary_constraints and "final" in boundary_constraints["position"]: - bounds.append(p_obj_end) - bounds_labels.append("p_obj_end") - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: - bounds.append(R_t_start) - bounds_labels.append("R_t_start") - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: - bounds.append(R_t_end) - bounds_labels.append("R_t_end") - - # Construct a CasADi function out of the opti object. This function can be called with the initial guess to solve the NLP. Faster than doing opti.set_initial + opti.solve + opti.value - solution = [*invars, *p_obj, *R_t] - self.opti_function = opti.to_function('opti_function', - [h,*invars_demo,*w_invars,*bounds,*solution], # inputs - [*solution]) #outputs - # ["h_value","invars_model","weights",*bounds_labels,"invars1","p_obj1","R_t1"], # input labels for debugging - # ["invars2","p_obj2","R_t2"]) # output labels for debugging - - - # Save variables - self.R_t = R_t - self.p_obj = p_obj - self.invars = invars - self.invars_demo = invars_demo - self.w_ivars = w_invars - if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: - self.p_obj_start = p_obj_start - if "position" in boundary_constraints and "final" in boundary_constraints["position"]: - self.p_obj_end = p_obj_end - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]: - self.R_t_start = R_t_start - if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]: - self.R_t_end = R_t_end - self.h = h - self.N = N - self.opti = opti - self.first_window = True - self.sol = sol - self.solver = solver - - - def generate_trajectory(self, invariant_model, boundary_constraints, step_size, weights_params = {}, initial_values = {}): - - N = invariant_model.shape[0] - - # Get the weights for the invariants or set default values - w_invars = weights_params.get('w_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) - w_high_start = weights_params.get('w_high_start', N) - w_high_end = weights_params.get('w_high_end', N) - w_high_invars = weights_params.get('w_high_invars', (10**-3)*np.array([1.0, 1.0, 1.0])) - w_high_active = weights_params.get('w_high_active', 0) - - # Set the weights for the invariants - w_invars = np.tile(w_invars, (len(invariant_model),1)).T - if w_high_active: - w_invars[:, w_high_start:w_high_end+1] = w_high_invars.reshape(-1, 1) - - to_skip = ("orientation","rotational") - boundary_values_list = [] - sublist_counter = 0 - subsublist_counter = 0 - for sublist in boundary_constraints.values(): - if list(boundary_constraints.keys())[sublist_counter] not in to_skip: - try: - for subsublist in sublist.values(): - if list(sublist.keys())[subsublist_counter] not in to_skip: - for value in subsublist.values(): - boundary_values_list.append(value) - subsublist_counter += 1 - except: - if list(sublist.keys())[subsublist_counter] not in to_skip: - for value in sublist.values(): - boundary_values_list.append(value) - sublist_counter += 1 - subsublist_counter = 0 - - if self.first_window and not initial_values: - 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,:], *initial_values["trajectory"]["position"][:N,:], *initial_values["moving-frame"]["translational"][:N]] - self.first_window = False - - # Call solve function - self.solution = self.opti_function(step_size,*invariant_model[:-1],*w_invars[:,:-1].T,*boundary_values_list,*self.solution) - - # Return the results - invars = np.zeros((N-1,3)) - p_obj_sol = np.zeros((N,3)) - R_t_sol = np.zeros((3,3,N)) - for i in range(N): # unpack the results - if i!= N-1: - invars[i,:] = self.solution[i].T - p_obj_sol[i,:] = self.solution[N-1+i].T - R_t_sol = self.solution[2*N-1+i] - - # Extract the solved variables - invariants = np.array(invars) - invariants = np.vstack((invariants,[invariants[-1,:]])) - calculated_trajectory = np.array(p_obj_sol) - calculated_movingframe = np.array(R_t_sol) - - return invariants, calculated_trajectory, calculated_movingframe - - # def generate_trajectory(self,invars_demo,p_obj_init,R_t_init,R_t_start,R_t_end,p_obj_start,p_obj_end,step_size): - - - # N = self.N - - # # Initialize states - # for k in range(N): - # self.opti.set_initial(self.R_t[k], R_t_init[k]) - # self.opti.set_initial(self.p_obj[k], p_obj_init[k]) - - # # Initialize controls - # for k in range(N-1): - # self.opti.set_initial(self.invars[:,k], invars_demo[k,:]) - - # # Set values boundary constraints - # self.opti.set_value(self.R_t_start,R_t_start) - # self.opti.set_value(self.R_t_end,R_t_end) - # self.opti.set_value(self.p_obj_start,p_obj_start) - # self.opti.set_value(self.p_obj_end,p_obj_end) - - # # Set values parameters - # self.opti.set_value(self.h,step_size) - # for k in range(N-1): - # self.opti.set_value(self.invars_demo[:,k], invars_demo[k,:]) - - # # ###################### - # # ## DEBUGGING: check integrator in initial values, time step 0 to 1 - # # x0 = cas.vertcat(cas.vec(np.eye(3,3)), cas.vec(measured_positions[0])) - # # u0 = 1e-8*np.ones((3,1)) - # # integrator = dynamics.define_integrator_invariants_position(self.stepsize) - # # x1 = integrator(x0,u0) - # # print(x1) - # # ###################### - - # # Solve the NLP - # sol = self.opti.solve_limited() - # self.sol = sol - - # # Extract the solved variables - # invariants = sol.value(self.invars).T - # invariants = np.vstack((invariants,[invariants[-1,:]])) - # calculated_trajectory = np.array([sol.value(i) for i in self.p_obj]) - # calculated_movingframe = np.array([sol.value(i) for i in self.R_t]) - - # return invariants, calculated_trajectory, calculated_movingframe - - def generate_trajectory_global(self,invars_demo,initial_values,boundary_constraints,step_size): - - N = self.N - - # Initialize states - for k in range(N): - self.opti.set_initial(self.R_t[k], initial_values["moving-frame"]["translational"][k]) - self.opti.set_initial(self.p_obj[k], initial_values["trajectory"]["position"][k]) - - # Initialize controls - for k in range(N-1): - self.opti.set_initial(self.invars[:,k], invars_demo[k,:]) - - # Set values boundary constraints - #self.opti.set_value(self.R_t_start,boundary_constraints["moving-frame"]["translational"]["initial"]) - #self.opti.set_value(self.R_t_end,boundary_constraints["moving-frame"]["translational"]["final"]) - self.opti.set_value(self.p_obj_start,boundary_constraints["position"]["initial"]) - self.opti.set_value(self.p_obj_end,boundary_constraints["position"]["final"]) - - # Set values parameters - self.opti.set_value(self.h,step_size) - for k in range(N-1): - self.opti.set_value(self.invars_demo[:,k], invars_demo[k,:]) - - # Solve the NLP - sol = self.opti.solve_limited() - self.sol = sol - - # Extract the solved variables - invariants = sol.value(self.invars).T - invariants = np.vstack((invariants,[invariants[-1,:]])) - calculated_trajectory = np.array([sol.value(i) for i in self.p_obj]) - calculated_movingframe = np.array([sol.value(i) for i in self.R_t]) - - return invariants, calculated_trajectory, calculated_movingframe - -def generate_trajectory_translation(invariant_model, boundary_constraints, N=100): - - # Specify optimization problem symbolically - OCP = OCP_gen_pos(N = N) - - # Initial values - initial_values, initial_values_dict = generate_initvals_from_constraints_opti(boundary_constraints, N) - - # Resample model invariants to desired number of N samples - spline_invariant_model = sh.create_spline_model(invariant_model[:,0], invariant_model[:,1:]) - progress_values = np.linspace(invariant_model[0,0],invariant_model[-1,0],N) - model_invariants,progress_step = sh.interpolate_invariants(spline_invariant_model, progress_values) - - # Calculate remaining trajectory - invariants, trajectory, mf = OCP.generate_trajectory_global(model_invariants,initial_values_dict,boundary_constraints,progress_step) - - return invariants, trajectory, mf, progress_values - -if __name__ == "__main__": - - # Randomly chosen data - N = 100 - invariant_model = np.zeros((N,3)) - - # Boundary constraints - boundary_constraints = { - "position": { - "initial": np.array([0, 0, 0]), - "final": np.array([1, 0, 0]) - }, - "moving-frame": { - "translational": { - "initial": np.eye(3), - "final": np.eye(3) - } - }, - } - step_size = 0.1 - - # Create an instance of OCP_gen_pos - ocp = OCP_gen_pos(boundary_constraints,solver='ipopt', N=N) - - # Call the generate_trajectory function - invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) - - # Print the results - # print("Invariants:", invariants) - #print("Calculated Trajectory:", calculated_trajectory) - # print("Calculated Moving Frame:", calculated_movingframe) - - # Second call to generate_trajectory - boundary_constraints["position"]["initial"] = np.array([1, 0, 0]) - boundary_constraints["position"]["final"] = np.array([1, 2, 2]) - invariants, calculated_trajectory, calculated_movingframe = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) - - # Print the results - #print("Invariants:", invariants) - print("Calculated Trajectory:", calculated_trajectory) - #print("Calculated Moving Frame:", calculated_movingframe) \ No newline at end of file