diff --git a/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py b/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py new file mode 100644 index 0000000..16d2d82 --- /dev/null +++ b/examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py @@ -0,0 +1,262 @@ + +# Imports +import numpy as np +from math import pi +import invariants_py.data_handler as dh +import matplotlib.pyplot as plt +import invariants_py.reparameterization as reparam +import scipy.interpolate as ip +from invariants_py.calculate_invariants.opti_calculate_vector_invariants_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.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 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 +#%% +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] +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)) +ax = fig.add_subplot(111, projection='3d') +ax = plt.axes(projection='3d') +ax.plot(trajectory_position[:,0],trajectory_position[:,1],trajectory_position[:,2],'.-') +n_frames = 10 +indx = np.trunc(np.linspace(0,len(trajectory_orientation)-1,n_frames)) +indx = indx.astype(int) + +for i in indx: + pl.plot_3d_frame(trajectory_position[i,:],trajectory_orientation[i,:,:],1,0.01,['red','green','blue'],ax) + pl.plot_stl(opener_location,trajectory_position[i,:],trajectory_orientation[i,:,:],colour="c",alpha=0.2,ax=ax) + +#%% +# define class for OCP results +class OCP_results: + + def __init__(self,FSt_frames,FSr_frames,Obj_pos,Obj_frames,invariants): + self.FSt_frames = FSt_frames + self.FSr_frames = FSr_frames + self.Obj_pos = Obj_pos + self.Obj_frames = Obj_frames + self.invariants = 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.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') +ax = plt.axes(projection='3d') +ax.plot(trajectory_position[:,0],trajectory_position[:,1],trajectory_position[:,2],'b') +ax.plot(optim_calc_results.Obj_pos[:,0],optim_calc_results.Obj_pos[:,1],optim_calc_results.Obj_pos[:,2],'r') +indx = np.trunc(np.linspace(0,len(optim_calc_results.Obj_pos)-1,n_frames)) +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="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) + +pl.plot_invariants(optim_calc_results.invariants,[],arclength_n) + +if plt.get_backend() != 'agg': + plt.show() + +#%% +# Spline of model +knots = np.concatenate(([arclength_n[0]],[arclength_n[0]],arclength_n,[arclength_n[-1]],[arclength_n[-1]])) +degree = 3 +spline_model_trajectory = ip.BSpline(knots,optim_calc_results.invariants,degree) + +def interpolate_model_invariants(demo_invariants, progress_values): + + resampled_invariants = np.array([demo_invariants(i) for i in progress_values]) + new_stepsize = progress_values[1] - progress_values[0] + + resampled_invariants[:,0] = resampled_invariants[:,0] * (progress_values[-1] - progress_values[0]) + return resampled_invariants, new_stepsize + +#%% +current_progress = 0 +number_samples = 100 + +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]) +p_obj_end = optim_calc_results.Obj_pos[-1] + np.array([0.1,0.1,0.1]) +rotate = R.from_euler('z', 0, degrees=True) +R_obj_end = orthonormalize(rotate.apply(optim_calc_results.Obj_frames[-1])) +FSt_end = orthonormalize(optim_calc_results.FSt_frames[-1]) +FSr_end = orthonormalize(optim_calc_results.FSr_frames[-1]) + +# 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 +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 = 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 = 'ipopt') +rockit_online_generation_problem = OCP_gen_pose_rockit(boundary_constraints, number_samples,False) + +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) +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) +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') +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_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) + + +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(progress_values,fatrop_gen_results.invariants[:,0],'g') +plt.plot(progress_values,rockit_gen_results.invariants[:,0],'m') +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(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(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(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(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(0,0) +plt.title('i_t3') + +if plt.get_backend() != 'agg': + plt.show() \ No newline at end of file diff --git a/invariants_py/dynamics_vector_invariants.py b/invariants_py/dynamics_vector_invariants.py index 281f1f7..6d337ec 100644 --- a/invariants_py/dynamics_vector_invariants.py +++ b/invariants_py/dynamics_vector_invariants.py @@ -256,4 +256,25 @@ def define_integrator_invariants_rotation(h): out_plus1 = cas.vertcat(cas.vec(R_r_plus1), cas.vec(R_obj_plus1)) integrator = cas.Function("phi", [x,u,h] , [out_plus1]) + return integrator + +def define_integrator_invariants_pose(h): + """Define a CasADi function that integrates the vector invariants for pose over a time interval h.""" + + # System states + R_t = cas.MX.sym('R_t',3,3) # translational Frenet-Serret frame + p_obj = cas.MX.sym('p_obj',3,1) # object position + R_r = cas.MX.sym('R_r',3,3) # rotational Frenet-Serret frame + R_obj = cas.MX.sym('R_obj',3,3) # object orientation + x = cas.vertcat(cas.vec(R_r), cas.vec(R_obj),cas.vec(R_t), p_obj) + + # System controls (invariants) + u = cas.MX.sym('i',6,1) + + ## Integrate symbolically and create a casadi function with inputs/outputs + (R_t_plus1, p_obj_plus1) = integrate_vector_invariants_position(R_t, p_obj, u[3:], h) + (R_r_plus1, R_obj_plus1) = integrate_vector_invariants_rotation(R_r, R_obj, u[:3], h) + out_plus1 = cas.vertcat(cas.vec(R_r_plus1), cas.vec(R_obj_plus1),cas.vec(R_t_plus1), p_obj_plus1) + integrator = cas.Function("phi", [x,u,h] , [out_plus1]) + return integrator \ No newline at end of file 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_fatrop.py new file mode 100644 index 0000000..71ad902 --- /dev/null +++ b/invariants_py/generate_trajectory/opti_generate_pose_traj_from_vector_invars_fatrop.py @@ -0,0 +1,320 @@ +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.kinematics.orientation_kinematics import rotate_x +from invariants_py import spline_handler as sh + +class OCP_gen_pose: + + 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 = [] + R_obj = [] + R_r = [] + 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 + R_obj.append(opti.variable(3,3)) # object orientation + R_r.append(opti.variable(3,3)) # rotational Frenet-Serret frame + X.append(cas.vertcat(cas.vec(R_r[k]), cas.vec(R_obj[k]),cas.vec(R_t[k]), cas.vec(p_obj[k]))) + for k in range(N-1): + invars.append(opti.variable(6,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(6,1)) # model invariants + w_invars.append(opti.parameter(6,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 "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 "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) + 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(tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0) + 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 "position" in boundary_constraints and "initial" in boundary_constraints["position"]: + opti.subject_to(p_obj[0] == p_obj_start) + if "position" in boundary_constraints and "final" in boundary_constraints["position"]: + opti.subject_to(p_obj[-1] == p_obj_end) + 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 "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 "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 "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.) + 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 "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.) + + # Dynamic constraints + integrator = dynamics.define_integrator_invariants_pose(h) + for k in range(N-1): + # Integrate current state to obtain next state (next rotation and position) + Xk_end = integrator(X[k],invars[k],h) + + # Gap closing constraint + opti.subject_to(Xk_end==X[k+1]) + + ''' 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)) + opti.set_initial(R_r[k], np.eye(3)) + opti.set_initial(R_obj[k], np.eye(3)) + for k in range(N-1): + opti.set_initial(invars[k], 0.001+np.zeros(6)) + opti.set_value(invars_demo[k], 0.001+np.zeros(6)) + opti.set_value(w_invars[k], 0.001+np.zeros(6)) + 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 "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 "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)) + 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 "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 "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 "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") + 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, *p_obj, *R_t, *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","p_obj1","R_t1","R_obj1","R_r1"], # input labels for debugging + # ["invars2","p_obj2","R_t2","R_obj2","R_r2"]) # output labels for debugging + + + # Save variables + self.R_t = R_t + self.p_obj = p_obj + self.R_r = R_r + self.R_obj = R_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 "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 "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 + 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.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.ones((6))) + 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.ones((6))) + 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) + + boundary_values_list = [] + for sublist in boundary_constraints.values(): + try: + for subsublist in sublist.values(): + for value in subsublist.values(): + boundary_values_list.append(value) + except: + for value in sublist.values(): + boundary_values_list.append(value) + + if self.first_window and not initial_values: + self.solution = generate_initvals_from_constraints_opti(boundary_constraints, np.size(invariant_model,0)) + self.first_window = False + elif self.first_window: + self.solution = [*initial_values["invariants"][:N-1,:], *initial_values["trajectory"]["position"][:N,:], *initial_values["moving-frame"]["translational"][:N],*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,6)) + p_obj_sol = np.zeros((N,3)) + R_t_sol = np.zeros((3,3,N)) + 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 + p_obj_sol[i,:] = self.solution[N-1+i].T + R_t_sol = self.solution[2*N-1+i] + R_obj_sol[i,:,:] = self.solution[3*N-1+i] + R_r_sol[i,:,:] = self.solution[4*N-1+i] + + # Extract the solved variables + invariants = np.array(invars) + invariants = np.vstack((invariants,[invariants[-1,:]])) + calculated_trajectory_pos = np.array(p_obj_sol) + calculated_movingframe_pos = np.array(R_t_sol) + calculated_trajectory_rot = np.array(R_obj_sol) + calculated_movingframe_rot = np.array(R_r_sol) + + return invariants, calculated_trajectory_pos, calculated_trajectory_rot, calculated_movingframe_pos, calculated_movingframe_rot + +if __name__ == "__main__": + + # Randomly chosen data + N = 100 + invariant_model = np.zeros((N,6)) + + # Boundary constraints + boundary_constraints = { + "position": { + "initial": np.array([0, 0, 0]), + "final": np.array([1, 0, 0]) + }, + "orientation": { + "initial": np.eye(3), + "final": rotate_x(np.pi/6) + }, + "moving-frame": { + "translational": { + "initial": np.eye(3), + "final": np.eye(3) + }, + "rotational": { + "initial": np.eye(3), + "final": np.eye(3) + } + }, + } + step_size = 0.1 + + # Create an instance of OCP_gen_pos + ocp = OCP_gen_pose(boundary_constraints,solver='ipopt', N=N) + + # Call the generate_trajectory function + invariants, calculated_trajectory_pos, calculated_trajectory_rot, calculated_movingframe_pos, calculated_movingframe_rot = 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_pos, calculated_trajectory_rot, calculated_movingframe_pos, calculated_movingframe_rot = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size) + + # Print the results + #print("Invariants:", invariants) + print("Calculated Trajectory Position:", calculated_trajectory_pos) + print("Calculated Trajectory Rotation:", calculated_trajectory_rot) + #print("Calculated Moving Frame:", calculated_movingframe) \ No newline at end of file diff --git a/invariants_py/ocp_initialization.py b/invariants_py/ocp_initialization.py index 6aea9e0..12c9525 100644 --- a/invariants_py/ocp_initialization.py +++ b/invariants_py/ocp_initialization.py @@ -154,7 +154,7 @@ def generate_initvals_from_constraints_opti(boundary_constraints,N, skip = {}, q if solution_pos is not None: if solution_rot is not None: - solution = [np.vstack((solution_rot[0],solution_pos[0]))] + solution_pos[1:] + solution_rot[1:] # concatenate invariants and combine lists + solution = [*np.hstack((solution_rot[:len(initial_invariants)],solution_pos[:len(initial_invariants)]))] + solution_pos[len(initial_invariants):] + solution_rot[len(initial_invariants):] # concatenate invariants and combine lists else: solution = [solution_pos,initial_values] else: