diff --git a/examples/fatrop-tests/pouring_motion_example_rockit.py b/examples/fatrop-tests/pouring_motion_example_rockit.py new file mode 100644 index 0000000..773a3c3 --- /dev/null +++ b/examples/fatrop-tests/pouring_motion_example_rockit.py @@ -0,0 +1,350 @@ +""" +Application involving approach trajectory of a bottle opener towards a bottle +""" + +# Imports +import numpy as np +from math import pi +import os +import invariants_py.data_handler as dh +import matplotlib.pyplot as plt +import invariants_py.reparameterization as reparam +import scipy.interpolate as ip + +from invariants_py.calculate_invariants.rockit_calculate_vector_invariants_position import OCP_calc_pos +from invariants_py.calculate_invariants.rockit_calculate_vector_invariants_rotation import OCP_calc_rot +from invariants_py.generate_trajectory.rockit_generate_pose_traj_from_vector_invars import OCP_gen_pose +from scipy.spatial.transform import Rotation as R +from invariants_py.kinematics.rigidbody_kinematics import orthonormalize_rotation as orthonormalize +import invariants_py.plotting_functions.plotters as pl +import invariants_py.collision_detection_bottle as cd +from invariants_py.reparameterization import interpR +from invariants_py.initialization import initial_trajectory_movingframe_rotation +import random + + +data_location = dh.find_data_path('pouring-demo-riccardo.csv') +trajectory,time = dh.read_pose_trajectory_from_data(data_location, dtype = 'csv') +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 = 20 +indx = np.trunc(np.linspace(0,len(trajectory_orientation)-1,n_frames)) +indx = indx.astype(int) +opener_location = dh.find_data_path('opener.stl') +bottle_location = dh.find_data_path('bottle.stl') +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))) + +# choose solver +use_fatrop_solver = True # True = fatrop, False = ipopt + +# specify optimization problem symbolically +FS_calculation_problem_pos = OCP_calc_pos(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 0.004, fatrop_solver = use_fatrop_solver) +FS_calculation_problem_rot = OCP_calc_rot(window_len=nb_samples, bool_unsigned_invariants = False, rms_error_traj = 4*pi/180, fatrop_solver = use_fatrop_solver) + +# 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_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) + # 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.05,-0.01,-0.01]) #np.array([0.827,0.7144,0.552]) # +alpha = 0 +rotate = R.from_euler('z', alpha, degrees=True) +R_obj_end = orthonormalize(rotate.as_matrix() @ optim_calc_results.Obj_frames[-1]) +FSt_end = orthonormalize(rotate.as_matrix() @ 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))) + +# Linear initialization +R_obj_init = interpR(np.linspace(0, 1, len(optim_calc_results.Obj_frames)), [0,1], np.array([R_obj_start, R_obj_end])) +# R_r_init = interpR(np.linspace(0, 1, len(optim_calc_results.FSr_frames)), [0,1], np.array([FSr_start, FSr_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": optim_calc_results.Obj_frames[-1] + }, + "moving-frame": { + "translational": { + "initial": FSt_start, + "final": FSt_end + }, + "rotational": { + "initial": orthonormalize(optim_calc_results.FSr_frames[current_index]),#R_r_init, + "final": orthonormalize(optim_calc_results.FSr_frames[-1]) + } + }, +} + +# Define robot parameters +robot_params = { + "urdf_file_name": None, # use None if do not want to include robot model + "q_init": np.array([-pi, -2.27, 2.27, -pi/2, -pi/2, pi/4]), # Initial joint values + "tip": 'TCP_frame' # Name of the robot tip (if empty standard 'tool0' is used) + # "joint_number": 6, # Number of joints (if empty it is automatically taken from urdf file) + # "q_lim": [2*pi, 2*pi, pi, 2*pi, 2*pi, 2*pi], # Join limits (if empty it is automatically taken from urdf file) + # "root": 'world', # Name of the robot root (if empty it is automatically taken from urdf file) +} + +# specify optimization problem symbolically +FS_online_generation_problem = OCP_gen_pose(boundary_constraints, number_samples, use_fatrop_solver, robot_params) + +initial_values = { + "trajectory": { + "position": optim_calc_results.Obj_pos, + "orientation": R_obj_init + }, + "moving-frame": { + "translational": optim_calc_results.FSt_frames, + "rotational": optim_calc_results.FSr_frames, #R_r_init_array, + }, + "invariants": model_invariants, + "joint-values": robot_params["q_init"] if robot_params["urdf_file_name"] is not None else {} +} + +# Define OCP weights +weights_params = { + "w_invars": np.array([1*10, 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 +} + +# 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, tot_time, joint_values = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights_params,initial_values) + +if use_fatrop_solver: + print('') + print("TOTAL time to generate new trajectory: ") + print(str(tot_time) + "[s]") + +print('Joint values:') +print(joint_values) +print(optim_gen_results.Obj_pos[-1]) + +# optim_gen_results.Obj_frames = interpR(np.linspace(0, 1, len(optim_calc_results.Obj_frames)), [0,1], np.array([R_obj_start, R_obj_end])) # JUST TO CHECK INITIALIZATION + +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') +# 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.2,0.02,['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) +r_bottle = 0.0145+0.01 # bottle radius + margin +obj_pos = p_obj_end + [r_bottle*np.sin(alpha*pi/180) , -r_bottle*np.cos(alpha*pi/180), 0] # position of the bottle +pl.plot_stl(bottle_location,obj_pos,np.eye(3),colour="tab:gray",alpha=1,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) + +# fig99 = plt.figure(figsize=(14,8)) +# ax99 = fig99.add_subplot(111, projection='3d') +# pl.plot_stl(opener_location,[0,0,0],optim_calc_results.Obj_frames[-1],colour="r",alpha=0.5,ax=ax99) +# pl.plot_stl(opener_location,[0,0,0],R_obj_end,colour="b",alpha=0.5,ax=ax99) +# pl.plot_stl(opener_location,[0,0,0],optim_gen_results.Obj_frames[-1],colour="g",alpha=0.5,ax=ax99) + +opener_dim_x = 0.04 +opener_dim_y = 0.15 +opener_dim_z = 0 +opener_points = 30 +offset = -0.02 # position of the hook where have contact with bottle +opener_geom = np.zeros((opener_points,3)) +for j in range(opener_points // 3): + opener_geom[j*3, :] = [opener_dim_x/2, offset-j*offset, opener_dim_z] + opener_geom[j*3+1, :] = [0, offset-j*offset, opener_dim_z] + opener_geom[j*3+2, :] = [-opener_dim_x/2, offset-j*offset, opener_dim_z] + +# tilting_angle_rotx_deg=0 +# tilting_angle_roty_deg=0 +# tilting_angle_rotz_deg=0 +# mode = 'rpy' +# collision_flag, first_collision_sample, last_collision_sample = cd.collision_detection_bottle(optim_gen_results.Obj_pos,optim_gen_results.Obj_frames,obj_pos,opener_geom,tilting_angle_rotx_deg,tilting_angle_roty_deg,tilting_angle_rotz_deg,mode,ax) + +# if collision_flag: +# print("COLLISION DETECTED") +# print("First collision sample: " + str(first_collision_sample)) +# print("Last collision sample: " + str(last_collision_sample)) +# else: +# print("NO COLLISION DETECTED") + +if plt.get_backend() != 'agg': + plt.show() + + +# # 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)) +boundary_constraints["position"]["initial"] = optim_calc_results.Obj_pos[current_index] +boundary_constraints["orientation"]["initial"] = orthonormalize(optim_calc_results.Obj_frames[current_index]) +boundary_constraints["moving-frame"]["translational"]["initial"] = orthonormalize(optim_calc_results.FSt_frames[current_index]) +boundary_constraints["moving-frame"]["rotational"]["initial"] = orthonormalize(optim_calc_results.FSr_frames[current_index]) +boundary_constraints["moving-frame"]["translational"]["final"] = orthonormalize(optim_calc_results.FSt_frames[-1]) +boundary_constraints["moving-frame"]["rotational"]["final"] = 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))) + +# specify optimization problem symbolically +# FS_online_generation_problem = OCP_gen_pose(boundary_constraints, number_samples, use_fatrop_solver, robot_params) + +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') + +total_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.1,0.3),random.uniform(-0.05,0.05)]) + targets[k,-1] = random.uniform(0,30) + boundary_constraints["position"]["final"] = targets[k,:-1] + rotate = R.from_euler('z', targets[k,-1], degrees=True) + boundary_constraints["orientation"]["final"]= orthonormalize(rotate.apply(optim_calc_results.Obj_frames[-1])) + + # 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, tot_time, joint_values = 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]) + + ax.plot(optim_gen_results.Obj_pos[:,0],optim_gen_results.Obj_pos[:,1],optim_gen_results.Obj_pos[:,2],'r') + + # 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) + if use_fatrop_solver: + new_time = tot_time + if new_time > max_time: + max_time = new_time + total_time += new_time + + counter += 1 + # plt.show(block=False) +if use_fatrop_solver: + print('') + print("AVERAGE time to generate new trajectory: ") + print(str(total_time/counter) + "[s]") # last time average was 80ms + print('') + print("MAXIMUM time to generate new trajectory: ") + print(str(max_time) + "[s]") # last time maximum was 156ms + +# 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.') + +fig = plt.figure(figsize=(5,5)) +ax2 = fig.add_subplot() +ax2.plot(targets[:,-1],'r.') + +if plt.get_backend() != 'agg': + plt.show() diff --git a/invariants_py/data/pouring-demo-riccardo.csv b/invariants_py/data/pouring-demo-riccardo.csv new file mode 100644 index 0000000..017d356 --- /dev/null +++ b/invariants_py/data/pouring-demo-riccardo.csv @@ -0,0 +1,109 @@ +2.800743341445922852e+00,-2.622134222757627892e-02,-4.767094578195006371e-01,8.945761571123636569e-02,-7.111921398743396017e-01,-8.563669717168140294e-03,-7.027414404159517680e-01,-1.693728620663667583e-02 +2.900733232498168945e+00,-2.622112498127728009e-02,-4.767110565315118564e-01,8.945926630865941309e-02,-7.111913892346474197e-01,-8.566645406128454987e-03,-7.027420694440317250e-01,-1.694120121176371369e-02 +3.000787496566772461e+00,-2.621971684133224401e-02,-4.767069892382194141e-01,8.945190329968435994e-02,-7.111898887643649525e-01,-8.560901393809036697e-03,-7.027437951851592812e-01,-1.693550753340533621e-02 +3.100875616073608398e+00,-2.622077448592827903e-02,-4.767087604260639155e-01,8.945443485505558123e-02,-7.111910138017143446e-01,-8.563278620010925102e-03,-7.027425583128916742e-01,-1.693838489401097019e-02 +3.200794219970703125e+00,-2.621623692342658213e-02,-4.766778937182754583e-01,8.938699947318715089e-02,-7.111818805011992373e-01,-8.509385961247485641e-03,-7.027536506772217306e-01,-1.688874350502146682e-02 +3.300731182098388672e+00,-2.620223461329313636e-02,-4.766778615599990276e-01,8.939219650624552482e-02,-7.111788176830333708e-01,-8.521317915560634726e-03,-7.027567729684751630e-01,-1.688177913846865968e-02 +3.400737047195434570e+00,-2.622342736403687757e-02,-4.766783601875759779e-01,8.939127420381692835e-02,-7.111844207508827287e-01,-8.508437836450444269e-03,-7.027509597989830548e-01,-1.689421997084242927e-02 +3.500828504562377930e+00,-2.620675509777190687e-02,-4.766763619439933763e-01,8.939927580014281339e-02,-7.111884994145436512e-01,-8.528947796849740970e-03,-7.027468424779583778e-01,-1.688344565627486471e-02 +3.600740194320678711e+00,-2.616477381372157851e-02,-4.766633602567119654e-01,8.941302737591841354e-02,-7.111736086492399522e-01,-8.570926726377790794e-03,-7.027620838662916869e-01,-1.685500091299266257e-02 +3.700749635696411133e+00,-2.615476805811822389e-02,-4.766688887229315053e-01,8.941605368574348511e-02,-7.111667604605778070e-01,-8.574358354350095079e-03,-7.027688004441385861e-01,-1.686215636047813660e-02 +3.800731182098388672e+00,-2.603332405284596723e-02,-4.766565979862137370e-01,8.942119625367467295e-02,-7.111566451497014496e-01,-8.666421968066377601e-03,-7.027801566277565026e-01,-1.676814360503298823e-02 +3.900868892669677734e+00,-2.608630847735787447e-02,-4.766593853825011329e-01,8.942048782330486167e-02,-7.111596124426953214e-01,-8.627993072270530306e-03,-7.027766686070844315e-01,-1.680825474071141051e-02 +4.000865459442138672e+00,-2.610021108399985731e-02,-4.766742154591039893e-01,8.943499733632284676e-02,-7.111824338530114975e-01,-8.638605519102400543e-03,-7.027531858514893814e-01,-1.681903829517834847e-02 +4.100794315338134766e+00,-2.611215913964999927e-02,-4.766808137039147963e-01,8.943477262358531799e-02,-7.111926224094216042e-01,-8.632811165099806006e-03,-7.027427284172749200e-01,-1.682813294580894070e-02 +4.200842380523681641e+00,-2.611220871863038917e-02,-4.766833796968338310e-01,8.944347607120783916e-02,-7.112039755817139985e-01,-8.644276448532439705e-03,-7.027310917177149729e-01,-1.682837884868286504e-02 +4.300727367401123047e+00,-2.608699399998532648e-02,-4.766757723987382622e-01,8.947418419639380227e-02,-7.112061712018710180e-01,-8.692654394687200636e-03,-7.027285089530601603e-01,-1.681851683586142038e-02 +4.400747299194335938e+00,-2.610007436233427805e-02,-4.766875035297561403e-01,8.949439078784482415e-02,-7.112196873059900559e-01,-8.700877861272243718e-03,-7.027142891247147860e-01,-1.683683381042251626e-02 +4.500753402709960938e+00,-2.610234771115835239e-02,-4.766870257904514685e-01,8.951892943855679907e-02,-7.112290030682435482e-01,-8.725889837100202218e-03,-7.027042661273372737e-01,-1.684869156771669901e-02 +4.600721597671508789e+00,-2.611380322532522169e-02,-4.767158251598235807e-01,8.956741319115335553e-02,-7.112346615707167974e-01,-8.754966195569841014e-03,-7.026971375593883407e-01,-1.689199849469299325e-02 +4.700752496719360352e+00,-2.611948475325226449e-02,-4.767338890758095471e-01,8.961731085503152339e-02,-7.112517874029187226e-01,-8.796394544900713874e-03,-7.026786755677761898e-01,-1.691736567788842266e-02 +4.800956249237060547e+00,-2.612155605142898324e-02,-4.767416185451069932e-01,8.965469828190006374e-02,-7.112616600063477179e-01,-8.828553913998622715e-03,-7.026678298316910709e-01,-1.693601371114728665e-02 +4.900819540023803711e+00,-2.603634744157525341e-02,-4.767061096871083592e-01,8.990429254694012884e-02,-7.113327198303676102e-01,-9.114559529199602050e-03,-7.025921342195975416e-01,-1.694046741005918871e-02 +5.000827550888061523e+00,-2.372023819231107444e-02,-4.762311452490075037e-01,9.478256309225346898e-02,-7.117883328225207418e-01,-1.446462734754587476e-02,-7.020303851822423669e-01,-1.736323974105590667e-02 +5.100798130035400391e+00,-1.953398228747424414e-02,-4.749787115369809576e-01,1.034520283861351808e-01,-7.129541207838389072e-01,-2.373802377558580090e-02,-7.005841307646457627e-01,-1.774270569330208686e-02 +5.200763702392578125e+00,-1.607199462660839975e-02,-4.736422213191851527e-01,1.125800420156347903e-01,-7.149381110663333150e-01,-3.119396868210425056e-02,-6.982935342136995516e-01,-1.663050624918464143e-02 +5.300792694091796875e+00,-1.389951525013837647e-02,-4.724600701471692865e-01,1.206192612684434395e-01,-7.186016774428874987e-01,-3.545801471915339953e-02,-6.943898782005617498e-01,-1.330621738339383270e-02 +5.400792360305786133e+00,-1.274421860320596595e-02,-4.716520363294799956e-01,1.290216155279650667e-01,-7.235642332532294940e-01,-4.216659752998914990e-02,-6.888516621258905515e-01,-1.265567050011764949e-02 +5.500789165496826172e+00,-1.196140103952347475e-02,-4.714425602349245903e-01,1.385168740089886275e-01,-7.289720211585679621e-01,-4.754271367087655603e-02,-6.827973097698015748e-01,-1.128346205394035323e-02 +5.600770473480224609e+00,-1.029222334561049820e-02,-4.720120424809618465e-01,1.504713513514760459e-01,-7.328886183864837811e-01,-4.762833764076847276e-02,-6.786575411493704912e-01,-5.454936683305676437e-03 +5.700754404067993164e+00,-8.229017063503232116e-03,-4.728380230469005352e-01,1.604525543554945477e-01,-7.336886255897947517e-01,-4.640634489525582301e-02,-6.778990137653689851e-01,-6.156050363476175957e-04 +5.800746917724609375e+00,-1.027929637863057855e-03,-4.743236436379631993e-01,1.837090561612721440e-01,-7.321676847050795356e-01,-4.794872716527127932e-02,-6.793928329810593469e-01,7.535219360056730209e-03 +5.900764226913452148e+00,4.033765739633668468e-03,-4.750889302529913150e-01,1.969201795279259259e-01,-7.297249849498931296e-01,-4.774276523904723984e-02,-6.819334094318127981e-01,1.374408265075243579e-02 +6.000774383544921875e+00,1.572022032341718195e-02,-4.770114332541049995e-01,2.258930702451906747e-01,-7.233259903684896575e-01,-5.039126540053141118e-02,-6.881988622705375036e-01,2.534872774549375846e-02 +6.100780963897705078e+00,2.217701765661887064e-02,-4.780381550841906191e-01,2.411111321697452237e-01,-7.200157959876538616e-01,-5.273798864805544218e-02,-6.912435558851616202e-01,3.127786004285711130e-02 +6.200762271881103516e+00,3.500109416429840470e-02,-4.803208071145956826e-01,2.695561771019559516e-01,-7.169894224669716154e-01,-5.629969386932314607e-02,-6.933432321293708789e-01,4.507410566053357548e-02 +6.300771951675415039e+00,4.605172741982270501e-02,-4.811973012297705021e-01,2.874469853963618204e-01,-7.217888251699041691e-01,-6.139395090514457309e-02,-6.871075546830325287e-01,5.599002544747865712e-02 +6.400757312774658203e+00,5.999576077865023671e-02,-4.806832639121264572e-01,3.042780554970391238e-01,7.279188475773796796e-01,6.912679198898116772e-02,6.787854886100043172e-01,-6.786676965503621317e-02 +6.500801324844360352e+00,7.666251591207812188e-02,-4.790453855986477882e-01,3.220410185665427694e-01,-7.325016530305076268e-01,-8.436704359375746254e-02,-6.712864446235540239e-01,7.548535968975000898e-02 +6.600747823715209961e+00,9.419366674467501144e-02,-4.759754111567923163e-01,3.405465998773607450e-01,-7.348830237066497073e-01,-9.928090860040635068e-02,-6.656269245498467324e-01,8.385129676732686055e-02 +6.700841903686523438e+00,1.102215914772240596e-01,-4.718980014860218453e-01,3.582129383302045667e-01,-7.356357269346391892e-01,-1.124194936534400474e-01,-6.616514715466457197e-01,9.175655237751315085e-02 +6.800788879394531250e+00,1.198596210422208097e-01,-4.687208092074615640e-01,3.690848770731735007e-01,-7.363475072290242007e-01,-1.181645504339289832e-01,-6.588650311293522410e-01,9.862230160195742390e-02 +6.900847434997558594e+00,1.379396933987552809e-01,-4.609213046816568737e-01,3.894477084917422616e-01,-7.388076883904181047e-01,-1.220560159170298170e-01,-6.521821616374166020e-01,1.179998160829703047e-01 +7.000790357589721680e+00,1.476655229001227199e-01,-4.550638100715292400e-01,3.991315797137109378e-01,-7.402759863214757319e-01,-1.233596358635511980e-01,-6.479910967983131265e-01,1.299284525633154219e-01 +7.100716590881347656e+00,1.694396164948643313e-01,-4.372962107879829752e-01,4.165971677306053444e-01,-7.429147695661302286e-01,-1.326767587253687153e-01,-6.378190578805720268e-01,1.538225349517212126e-01 +7.200767517089843750e+00,1.815679749840363455e-01,-4.269550724107435968e-01,4.235896567461185369e-01,-7.429413834607956346e-01,-1.438730083542974492e-01,-6.331504624363032407e-01,1.626626881775241062e-01 +7.300758838653564453e+00,2.093110884044402098e-01,-4.028900789597316123e-01,4.314191489497934962e-01,-7.405389813366988205e-01,-1.728250521382241889e-01,-6.229842338484291586e-01,1.833689255313193933e-01 +7.400735855102539062e+00,2.335910253488872046e-01,-3.809692825406021388e-01,4.322480645249580089e-01,-7.361403834556916603e-01,-2.005141862948054687e-01,-6.139704545367453692e-01,2.023157875603932565e-01 +7.500715255737304688e+00,2.582084584127327109e-01,-3.601215667761088479e-01,4.320174639997664823e-01,-7.322622934908189496e-01,-2.263009848677635105e-01,-6.036638111372786275e-01,2.194761966652923058e-01 +7.600759506225585938e+00,2.798060052868264203e-01,-3.446333995520092230e-01,4.332371225842170381e-01,-7.321202985773508365e-01,-2.471870770789069693e-01,-5.913874988450060766e-01,2.305628841467011192e-01 +7.700717449188232422e+00,2.988639018519025359e-01,-3.332912062288531652e-01,4.343026978585602382e-01,-7.377221513367223649e-01,-2.636484866959429851e-01,-5.752580955450825595e-01,2.352310064593997052e-01 +7.800746679306030273e+00,3.159100530928746764e-01,-3.228299998539136961e-01,4.333186838459257562e-01,-7.463573972257590006e-01,-2.779624627572809259e-01,-5.559627338708095223e-01,2.378927141616438978e-01 +7.900808334350585938e+00,3.309322748655956214e-01,-3.102174782731797475e-01,4.291540077869041570e-01,-7.559651751230285210e-01,-2.923459240272607840e-01,-5.334483064575363809e-01,2.418334531106929308e-01 +8.000714063644409180e+00,3.438534384517406006e-01,-2.979258398568166988e-01,4.224793156333339628e-01,-7.669639340934136795e-01,-3.050571724449280264e-01,-5.085014814011677986e-01,2.452196744812227824e-01 +8.100709438323974609e+00,3.555855848537224362e-01,-2.867823527949446283e-01,4.153885929087120510e-01,-7.784430425768076311e-01,-3.161411672533631090e-01,-4.832238686708683062e-01,2.461216865255952402e-01 +8.200745582580566406e+00,3.672618984246251439e-01,-2.766361328656340990e-01,4.087710724727885436e-01,-7.890072795849142340e-01,-3.256691422501236666e-01,-4.600918235332061568e-01,2.443821525111262161e-01 +8.300734043121337891e+00,3.795607039900777968e-01,-2.670535241905277468e-01,4.028205997800797245e-01,-7.984479426809388825e-01,-3.341496690648915746e-01,-4.395359285988118536e-01,2.400688421332092259e-01 +8.400761127471923828e+00,3.916273886970158546e-01,-2.565728761862644536e-01,3.971384347249389823e-01,-8.062883335585608879e-01,-3.434121929861597144e-01,-4.199550386241565625e-01,2.358070279062494667e-01 +8.500735044479370117e+00,4.038655095167346110e-01,-2.464658606688064635e-01,3.920263785648361865e-01,-8.136386856923506494e-01,-3.509255606805633532e-01,-4.024054541765361148e-01,2.300286731145326224e-01 +8.600723743438720703e+00,4.157263626410170332e-01,-2.371368500137813196e-01,3.877916909901731879e-01,-8.204463655232006047e-01,-3.558851385414576463e-01,-3.876344668334216759e-01,2.235017887800179182e-01 +8.700714588165283203e+00,4.239883864280361569e-01,-2.313965881266517033e-01,3.856978153488774264e-01,-8.247695707085274108e-01,-3.573770958137700293e-01,-3.800432399253555471e-01,2.181831854383496916e-01 +8.800700187683105469e+00,4.386572527762237494e-01,-2.181908610435275564e-01,3.809287297429255204e-01,-8.345747602803460508e-01,-3.572975459858831471e-01,-3.617077590963567180e-01,2.121106554253239240e-01 +8.900740146636962891e+00,4.438587811901440472e-01,-2.102870780912589610e-01,3.769712428790122360e-01,-8.393933908374265895e-01,-3.598622352776347233e-01,-3.482298488297937422e-01,2.113146455487880104e-01 +9.000936269760131836e+00,4.519174116016287379e-01,-1.944732658706107287e-01,3.666656629807319989e-01,-8.486774890648232095e-01,-3.689717910949846247e-01,-3.166477324998276943e-01,2.081839341387885789e-01 +9.100828886032104492e+00,4.552278882242610791e-01,-1.881147793575968230e-01,3.608031323568504090e-01,-8.540267665045691015e-01,-3.729626557766516082e-01,-2.996019944019883696e-01,2.043912533356752881e-01 +9.200883150100708008e+00,4.602726670510570717e-01,-1.784779302458324723e-01,3.490436062145122476e-01,-8.644689630389323920e-01,-3.795042783229597982e-01,-2.663138514119566258e-01,1.942854786727323257e-01 +9.300754070281982422e+00,4.620706188550376403e-01,-1.742348836232121556e-01,3.434492107645975012e-01,-8.683124975695446501e-01,-3.841288511186208288e-01,-2.509355029350493482e-01,1.884404565627082473e-01 +9.400748491287231445e+00,4.647892779201207580e-01,-1.670865564192728359e-01,3.336818317372728382e-01,-8.748565522974554032e-01,-3.917001531425720051e-01,-2.232064045259924134e-01,1.771324473631927521e-01 +9.500745534896850586e+00,4.659093206740644311e-01,-1.638559193415794479e-01,3.290943778999602665e-01,-8.777452774603855534e-01,-3.950222198615475078e-01,-2.100087565757063068e-01,1.715138358068287461e-01 +9.600722551345825195e+00,4.676325687279371257e-01,-1.582347685753340560e-01,3.197198466351406365e-01,-8.834172733547254186e-01,-4.013599107226368812e-01,-1.824974539967341436e-01,1.586783617508366051e-01 +9.700790166854858398e+00,4.683262966317706111e-01,-1.556338566255943412e-01,3.146351832826710382e-01,-8.859308605160156613e-01,-4.048933353736280383e-01,-1.679511557371000108e-01,1.515925678981090707e-01 +9.800829172134399414e+00,4.694247156514896324e-01,-1.506621739739293919e-01,3.047152812570064206e-01,-8.900459027010220181e-01,-4.120211416567231000e-01,-1.389890779051188296e-01,1.368901243190969730e-01 +9.900810480117797852e+00,4.701682540690062528e-01,-1.468509446120015161e-01,2.977376646140893479e-01,-8.921868136869263166e-01,-4.175857062986840784e-01,-1.177677649857294812e-01,1.255213962177970655e-01 +1.000071477890014648e+01,4.710963093470162955e-01,-1.420418214327855699e-01,2.910046506835640701e-01,-8.930767529706220564e-01,-4.240380173180167134e-01,-9.728298118680422935e-02,1.146372310545911155e-01 +1.010081720352172852e+01,4.720854258573184414e-01,-1.365902197313747513e-01,2.845519908197364223e-01,-8.929863249942335823e-01,-4.309281207140267522e-01,-7.764296447913202059e-02,1.041534839458687078e-01 +1.020076060295104980e+01,4.729919077696308483e-01,-1.312966965811247499e-01,2.786069925629733879e-01,-8.927073204048918686e-01,-4.367963744608236998e-01,-5.899947126868131109e-02,9.381700135590261003e-02 +1.030071854591369629e+01,4.735491061613660846e-01,-1.269492662991752729e-01,2.728452256456905811e-01,-8.930995177656942952e-01,-4.402605256580878201e-01,-4.049548298872503865e-02,8.309053358511458987e-02 +1.040080571174621582e+01,4.739414665942826010e-01,-1.240380628861003087e-01,2.673996290854278035e-01,-8.925893022763792262e-01,-4.444553160073198006e-01,-2.305254339470078073e-02,7.219688204926436037e-02 +1.050072646141052246e+01,4.744612120050879511e-01,-1.215205970005039382e-01,2.626883806035059665e-01,-8.915422331579011539e-01,-4.485812427863807295e-01,-7.686834636969699236e-03,6.219507769359691740e-02 +1.060078835487365723e+01,4.751933052672671076e-01,-1.187767326525441258e-01,2.585999801404006426e-01,-8.900350816061382009e-01,-4.527687759358873243e-01,6.040132266952262519e-03,5.292925325442859874e-02 +1.070076465606689453e+01,4.766018998372859583e-01,-1.156145954212536020e-01,2.553040035356928250e-01,-8.879980881716167396e-01,-4.574272411531511429e-01,1.885870100362076196e-02,4.317480574393230131e-02 +1.080076551437377930e+01,4.785906280098723986e-01,-1.123365785264280881e-01,2.525989062146820618e-01,-8.857423187582843527e-01,-4.619684122671093296e-01,3.097327954059281990e-02,3.296038201544699425e-02 +1.090071511268615723e+01,4.805412281348674375e-01,-1.097263504427378744e-01,2.501914331060839936e-01,-8.838656912179871927e-01,-4.652440679488867303e-01,4.243564743065726674e-02,2.299158440086218613e-02 +1.100082516670227051e+01,4.822081783870883487e-01,-1.077884241128775622e-01,2.479391240438833521e-01,-8.817964736855857799e-01,-4.684416578084304450e-01,5.303106876445691509e-02,1.360507151964923320e-02 +1.110076975822448730e+01,4.833005848243184799e-01,-1.057732132169347528e-01,2.455843851564250280e-01,-8.795696906457762809e-01,-4.715567766340313383e-01,6.293469600859130653e-02,5.530799772426521488e-03 +1.120072388648986816e+01,4.837216214951816085e-01,-1.036502786393101450e-01,2.426421828948992221e-01,-8.771412607685385998e-01,-4.745813721554342224e-01,7.342685628646188067e-02,-2.055878481670332658e-03 +1.130072402954101562e+01,4.834742617396103892e-01,-1.020319618833022135e-01,2.390367365838144131e-01,-8.749077977322529120e-01,-4.767262746019896857e-01,8.470605363244479435e-02,-9.658625648873542247e-03 +1.140073418617248535e+01,4.828807766672360002e-01,-1.017596866898312358e-01,2.351863461461736748e-01,-8.730393103019254974e-01,-4.776813263315260460e-01,9.647686213475455130e-02,-1.775185101117861958e-02 +1.150077915191650391e+01,4.827897941161110595e-01,-1.015661123773344554e-01,2.318378771952425421e-01,-8.707232644622893147e-01,-4.790793511773749902e-01,1.078294388968332601e-01,-2.639666847906521618e-02 +1.160082578659057617e+01,4.830115248763395819e-01,-1.011421678751057152e-01,2.298482524997155219e-01,-8.689449475912244258e-01,-4.802679448503189841e-01,1.150467395435627316e-01,-3.227424554239459453e-02 +1.170072484016418457e+01,4.839667176817596950e-01,-9.985649119487974290e-02,2.263879985432977193e-01,-8.652177699004076628e-01,-4.825299751763545975e-01,1.288473370569401810e-01,-4.428766688601700613e-02 +1.180071139335632324e+01,4.847407005599548291e-01,-9.954631622523274492e-02,2.251623056197781525e-01,-8.642743130688212982e-01,-4.819912113646723228e-01,1.348157993172582336e-01,-5.038932616220831440e-02 +1.190071487426757812e+01,4.871192990000474676e-01,-9.923546417642287176e-02,2.239505175566667838e-01,-8.645776057104299639e-01,-4.766730286838588571e-01,1.457233683631376508e-01,-6.366386213762396939e-02 +1.200088834762573242e+01,4.882032300287632021e-01,-9.872840755879133678e-02,2.234435056025015631e-01,-8.628743397935572101e-01,-4.751196155308672897e-01,1.565132849023479211e-01,-7.219983596040965845e-02 +1.210083365440368652e+01,4.869854570199882438e-01,-9.821995720890383530e-02,2.229104540803313050e-01,-8.598040240669714285e-01,-4.744796862023641926e-01,1.721171429755185556e-01,-7.734181710752345562e-02 +1.220126748085021973e+01,4.831366996872752395e-01,-9.716143649082142897e-02,2.230180695914588951e-01,-8.563085790497749761e-01,-4.730028362432637246e-01,1.921196261040805653e-01,-7.806397131335100770e-02 +1.230079030990600586e+01,4.799466966891906550e-01,-9.582475828084582870e-02,2.243243914991385757e-01,-8.529326587905420309e-01,-4.717218827895917066e-01,2.095373641307663459e-01,-7.796433735414093347e-02 +1.240075969696044922e+01,4.790985391539502869e-01,-9.526041193330744838e-02,2.272125868223575929e-01,-8.497051525991946574e-01,-4.716733036889346198e-01,2.216792835116276339e-01,-7.989833224364978337e-02 +1.250078535079956055e+01,4.806557879835118285e-01,-9.596379653207759430e-02,2.308341629008126561e-01,-8.458566435760915825e-01,-4.739938721722292136e-01,2.290774302063560697e-01,-8.590621998095841760e-02 +1.260076642036437988e+01,4.817879983241873942e-01,-9.690695784616867869e-02,2.336199367578851160e-01,-8.425490970009623659e-01,-4.760263710083220134e-01,2.349280201610906837e-01,-9.120711918869206514e-02 +1.270074582099914551e+01,4.810933459190612771e-01,-9.982358676402935949e-02,2.392237875093758404e-01,-8.341718376670076429e-01,-4.792017834020639300e-01,2.536528185485641407e-01,-1.009120590905669135e-01 +1.280084133148193359e+01,4.795507143926395210e-01,-1.013785357000135112e-01,2.416610127674880260e-01,-8.299357945438181616e-01,-4.796662403296375055e-01,2.649463552127246380e-01,-1.045480926772187069e-01 +1.290075159072875977e+01,4.772328850262274047e-01,-1.031584815198139105e-01,2.446332043157982139e-01,-8.238215335622648405e-01,-4.792730118092013991e-01,2.820701724356679541e-01,-1.097810494236964585e-01 +1.300083017349243164e+01,4.769502548315205370e-01,-1.031858884833123935e-01,2.450885952125866352e-01,-8.216974895482017649e-01,-4.785166034958838188e-01,2.883691796247192518e-01,-1.125980110459869898e-01 +1.310074377059936523e+01,4.776624114106723384e-01,-1.031507121199582433e-01,2.447322535777389507e-01,-8.198836739950032815e-01,-4.783776789471077540e-01,2.921100180503121702e-01,-1.166931649938381943e-01 +1.320078611373901367e+01,4.783746059217582891e-01,-1.031657910913156007e-01,2.443196212936140366e-01,-8.174774843883627629e-01,-4.785812882788673117e-01,2.964515342110265461e-01,-1.216840206902402854e-01 +1.330079174041748047e+01,4.786262178833716741e-01,-1.031365695433420493e-01,2.438340715363008226e-01,-8.162298063108228696e-01,-4.784610738539409880e-01,2.990059602895667190e-01,-1.242551399646881771e-01 +1.340074682235717773e+01,4.786873571359601676e-01,-1.026327968730963713e-01,2.435529610897492481e-01,-8.158243242577112309e-01,-4.789086964689375225e-01,2.993153108052385436e-01,-1.244486926947631078e-01 +1.350080919265747070e+01,4.787950601853473431e-01,-1.023140305960230922e-01,2.435140973191338709e-01,-8.156709060022689917e-01,-4.791978943512968936e-01,2.992496649592008007e-01,-1.244989524957024057e-01 +1.360077762603759766e+01,4.788147543322843758e-01,-1.021277091361127431e-01,2.434438793779414023e-01,-8.155408847887090085e-01,-4.793533449288836512e-01,2.993281992424272064e-01,-1.245634981919912510e-01 diff --git a/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py b/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py index edfdf12..6a43888 100644 --- a/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py +++ b/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py @@ -134,12 +134,12 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False if include_robot_model: objective_invariants = ocp.sum(1/window_len*cas.dot(w_invars*(invars - invars_demo),w_invars*(invars - invars_demo)),include_last=True) # Objective for joint limits - e_pos = cas.dot(p_obj_fwkin - p_obj,p_obj_fwkin - p_obj) - e_rot = cas.dot(R_obj.T @ R_obj_fwkin - np.eye(3),R_obj.T @ R_obj_fwkin - np.eye(3)) - objective_inverse_kin = ocp.sum(e_pos + e_rot + 0.001*cas.dot(qdot,qdot),include_last = True) - # ocp.subject_to(p_obj == p_obj_fwkin) - # ocp.subject_to(tril_vec_no_diag(R_obj.T @ R_obj_fwkin - np.eye(3)) == 0.) - # objective_inverse_kin = ocp.sum(0.001*cas.dot(qdot,qdot),include_last = True) + # e_pos = cas.dot(p_obj_fwkin - p_obj,p_obj_fwkin - p_obj) + # e_rot = cas.dot(R_obj.T @ R_obj_fwkin - np.eye(3),R_obj.T @ R_obj_fwkin - np.eye(3)) + # objective_inverse_kin = ocp.sum(e_pos + e_rot + 0.001*cas.dot(qdot,qdot),include_last = True) + ocp.subject_to(p_obj - p_obj_fwkin == 0) + ocp.subject_to(tril_vec_no_diag(R_obj.T @ R_obj_fwkin - np.eye(3)) == 0.) + objective_inverse_kin = ocp.sum(0.001*cas.dot(qdot,qdot),include_last = True) objective = ocp.sum(objective_invariants + objective_inverse_kin, include_last = True) else: objective = ocp.sum(1/window_len*cas.dot(w_invars*(invars - invars_demo),w_invars*(invars - invars_demo)),include_last=True) @@ -152,7 +152,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False ocp._method.set_name("/codegen/generation_pose") else: ocp.method(rockit.MultipleShooting(N=window_len-1)) - ocp.solver('ipopt', {'expand':True}) + ocp.solver('ipopt', {'expand':True, 'ipopt.max_iter': max_iters}) # ocp.solver('ipopt',{"print_time":True,"expand":True},{'tol':1e-4,'print_level':0,'ma57_automatic_scaling':'no','linear_solver':'mumps','max_iter':100}) # Solve already once with dummy values for code generation (TODO: can this step be avoided somehow?) @@ -176,7 +176,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False if "position" in boundary_constraints and "initial" in boundary_constraints["position"]: ocp.set_value(p_obj_start, p_obj_dummy) if "position" in boundary_constraints and "final" in boundary_constraints["position"]: - ocp.set_value(p_obj_end, p_obj_dummy + np.array([0.01,0,0])) + ocp.set_value(p_obj_end, p_obj_dummy + np.array([0.5,0,0])) if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]: ocp.set_value(R_obj_start, np.eye(3)) if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]: diff --git a/invariants_py/initialization.py b/invariants_py/initialization.py index a1c332d..192bb68 100644 --- a/invariants_py/initialization.py +++ b/invariants_py/initialization.py @@ -311,7 +311,7 @@ def estimate_initial_frames(vector_traj): #TODO this is not correct yet, ex not perpendicular to ey + not robust for singularities, these parts must still be transferred from Matlab - ex = vector_traj / np.linalg.norm(vector_traj,axis=1).reshape(N,1) + ex = vector_traj / (np.linalg.norm(vector_traj,axis=1).reshape(N,1)+0.000001) ez = np.tile( np.array((0,0,1)), (N,1) ) ey = np.array([ np.cross(ez[i,:],ex[i,:]) for i in range(N) ])