def main(): """ Calculates the x, y, z coefficients for the four segments of the trajectory """ # all B (0 -> 0), all alpha (0 -> 2pi/3) a_ans = (2*np.pi)/3 q_start = np.array([0.0001, 0.0001, 0.0001, a_ans, a_ans, a_ans]) q_end = np.array([0.0001, 0.0001, 0.0001, a_ans + 0.2, a_ans + 0.2, a_ans + 0.2]) uz_0 = np.array([[0, 0, 0]]).transpose() (r1,r2,r3,Uz) = moving_CTR(q_start, uz_0) x_cur_pos = r1[-1] (r1e,r2e,r3e,Uze) = moving_CTR(q_end, uz_0) x_end_pos = r1e[-1] # waypoints = [[0.0, 0.0, 0.0], [a_ans, a_ans, a_ans]] waypoints = [x_cur_pos, x_end_pos] a1_coeffs = [] a2_coeffs = [] a3_coeffs = [] total_time = 5 for x in range(len(waypoints)): traj = TrajectoryGenerator(waypoints[x], waypoints[(x + 1) % len(waypoints)], total_time) traj.solve() a1_coeffs.append(traj.x_c) a2_coeffs.append(traj.y_c) a3_coeffs.append(traj.z_c) print('START x_cur_pos:', x_cur_pos) print('END x_end_pos:', x_end_pos) CTR_sim(a1_coeffs, a2_coeffs, a3_coeffs, total_time, q_start, q_end) print('START q_start:', q_start) print('END q_end:', q_end)
def main(): """ Calculates the x, y, z coefficients for the four segments of the trajectory """ # all B (0 -> 0), all alpha (0 -> 2pi/3) a_ans = (2*np.pi)/3 # q_start = np.array([0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001]) # a_ans, a_ans, a_ans # q_end = np.array([0.0001, 0.0001, 0.0001, a_ans + 0.2, a_ans + 0.2, a_ans + 0.2]) # ([1.0001, -1.0001, 0.7001, a_ans + 0.2, a_ans + 0.2, a_ans + 0.2]) q_start = np.array([-0.06235794, -0.00409771, 0.02960726, 0.14837708, 0.22618857, 0.09228618]) q_end = np.array([-0.19746493, -0.00637689, 0.00991869, 0.17226557, 1.68673423, -0.22740581]) uz_0 = np.array([[0, 0, 0]]).transpose() (r1,r2,r3,Uz) = moving_CTR(q_start, uz_0) x_cur_pos = r1[-1] (r1e,r2e,r3e,Uze) = moving_CTR(q_end, uz_0) x_end_pos = r1e[-1] # x_cur_pos = [0.0, -0.07, 0.1] # x_end_pos = [0.05, 0.05, 0.1] # waypoints = [[0.0, 0.0, 0.0], [a_ans, a_ans, a_ans]] waypoints = [x_cur_pos, x_end_pos] a1_coeffs = [] a2_coeffs = [] a3_coeffs = [] for x in range(len(waypoints)): traj = TrajectoryGenerator(waypoints[x], waypoints[(x + 1) % len(waypoints)], total_time) traj.solve() a1_coeffs.append(traj.x_c) a2_coeffs.append(traj.y_c) a3_coeffs.append(traj.z_c) print('START des x_cur_pos:', x_cur_pos) print('END des x_end_pos:', x_end_pos) CTR_sim(a1_coeffs, a2_coeffs, a3_coeffs, q_start, x_end_pos) print('START des q_start:', q_start) print('END des q_end:', q_end)
def __init__(self, q, uz0=0, Kp_Uz=5, total_time=1, dt=0.1, model=lambda q,uz:moving_CTR(q,uz), jac_delta_uz=1e-1, plot=False, x_dim = np.zeros(3), parallel=False): self.q = q self.model = model self.Kp_Uz = np.eye(3) * Kp_Uz self.total_time = total_time self.dt = dt self.jac_delta_uz = np.ones(3) * jac_delta_uz self.t_steps = int(self.total_time/self.dt) self.plot = plot self.x_dim = x_dim # just for size TODO: change to just integer self.parallel = parallel
def __init__(self, Kp_x=5, Ki_x=0, Kd_x=0, total_time=1, dt=0.05, sim=False, model=lambda q,uz:moving_CTR(q,uz), plot=False, vanilla_model=None, jac_del_q=1e-1, damped_lsq=0, pertubed_model=None, parallel=False, helical=None): self.model = model self.vanilla_model = vanilla_model self.Kp_x = np.eye(3) * Kp_x self.Ki_x = np.eye(3) * Ki_x self.Kd_x = np.eye(3) * Kd_x self.total_time = total_time self.dt = dt self.jac_del_q = np.ones(6) * jac_del_q self.t_steps = int(self.total_time/self.dt) self.result = {} self.plot = plot self.sim = sim self.damped_lsq = damped_lsq self.parallel = parallel self.helical = helical if pertubed_model: self.pertubed_model = pertubed_model print('Using Pertubed Model!') else: self.pertubed_model = model print('NOT Using Pertubed Model!')
def CTR_sim(a1_c, a2_c, a3_c, total_time, q_start, q_end): """ Calculates the necessary thrust and torques for the quadrotor to follow the trajectory described by the sets of coefficients x_c, y_c, and z_c. """ runtime = time.time() total_time = 5 # (seconds) dt = 0.1 time_stamp = int(total_time/dt) t = dt i = 1 jac_del_q = np.ones(6) * 1e-1 uz_0 = np.array([[0, 0, 0]]).transpose() model = lambda q, uz_0: moving_CTR(q, uz_0) q_des_pos = np.zeros((6, time_stamp)) # [BBBaaa] x_des_pos = np.zeros((3, time_stamp)) # [r] x_cur_pos = np.zeros((3, time_stamp)) # [r] delta_q = np.zeros(6) # [BBBaaa] delta_x = np.zeros(3) # [r] quintic = TrajectoryRetreiver() q_des_pos[:, 0] = q_start while i < time_stamp: # runtime = time.time() x = np.zeros(3) # just for size TODO: change to just integer x_des_pos[0, i] = quintic.calculate_position(a1_c[0], t) x_des_pos[1, i] = quintic.calculate_position(a2_c[0], t) x_des_pos[2, i] = quintic.calculate_position(a3_c[0], t) # print('t:', t) # print('i:', i) # print(alpha_position(t, total_time)) delta_x = x_des_pos[:, i] - x_cur_pos[:, i-1] # print('delta_x', delta_x) # get trajectory from Jacobian r_jac = Jacobian(jac_del_q, x, q_des_pos[:, i-1].flatten(), uz_0, model) J_inv = r_jac.p_inv() delta_q = J_inv @ delta_x # print('delta_q', delta_q) q_des_pos[:, i] = q_des_pos[:, i-1] + delta_q * dt (r1,r2,r3,Uz) = model(q_des_pos[:, i], uz_0) # FORWARD KINEMATICS x_cur_pos[:, i] = r1[-1] # print(i, time.time()-runtime) t += dt i += 1 print("Done", time.time()-runtime) print('x_des_pos[:, 1]:', x_des_pos[:, 1]) print('x_des_pos[:, -1]:', x_des_pos[:, -1]) print('q_des_pos[:, 0]:', q_des_pos[:, 0]) print('q_des_pos[:, -1]:', q_des_pos[:, -1]) fig = plt.figure() ax = plt.axes(projection='3d') colors = cm.rainbow(np.linspace(0, 1, len(x_cur_pos.transpose()))) for y, c in zip(x_cur_pos.transpose(), colors): # plt.scatter(x, y, color=c) ax.scatter(y[0], y[1], y[2], linewidth=1, color=c) # ax.plot3D(x_des_pos[0], x_des_pos[1], x_des_pos[2], linewidth=1, label='x_des_pos') ax.scatter(x_des_pos[0], x_des_pos[1], x_des_pos[2], linewidth=1, label='x_des_pos', marker='.') (r1,r2,r3,Uz) = moving_CTR(q_des_pos[:, 1], uz_0) plot_3D(ax, r1, r2, r3, 'start position') (r1,r2,r3,Uz) = moving_CTR(q_des_pos[:, -1], uz_0) plot_3D(ax, r1, r2, r3, 'final position') ax.legend() ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z ') # plt.axis('equal') # ax.set_aspect('equal') # # Create cubic bounding box to simulate equal aspect ratio # max_range = 0.2 # np.array([X.max()-X.min(), Y.max()-Y.min(), Z.max()-Z.min()]).max() # Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(0) # X.max()+X.min()) # Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(0) # Y.max()+Y.min()) # Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(0.3) # Z.max()+Z.min()) # # Comment or uncomment following both lines to test the fake bounding box: # for xb, yb, zb in zip(Xb, Yb, Zb): # ax.plot([xb], [yb], [zb], 'w') plt.show()
# def calculate_position(self, c, t): # return t + c[0] # def calculate_velocity(self, c, t): # return 0.2 if __name__ == "__main__": a_ans = (2 * np.pi) / 3 q_static = np.array([0.001, 0.001, 0.001, 0.001, a_ans, a_ans]) uz0_start = np.array([[0.0, 0.0, 0.0]]).transpose() ax = plt.axes(projection='3d') runtime = time.time() (r1, r2, r3, Uz) = moving_CTR(q_static, uz0_start) print("\nNormalRuntime:", time.time() - runtime) plot_3D(ax, r1, r2, r3, 'init position') print('Uz before: ', Uz) # uz0_cont = UzController(q_static) # uz0_new = uz0_cont.run() runtime = time.time() (r1, r2, r3, Uz) = UzController( q_static, uz0_start, plot=True, dt=0.5, parallel=True,
if __name__ == "__main__": import matplotlib.pyplot as plt import matplotlib.cm as cm from mpl_toolkits.mplot3d import Axes3D from CTRmodel import moving_CTR, plot_3D a_ans = (2 * np.pi) / 4 total_time = 1 dt = 0.01 Uzdt = 0.05 UzControl = False jac_del_q = 1e-3 Kp_x = 13 model = lambda q, uz: moving_CTR(q, uz) q_start = np.array( [0.0001, 0.0001, 0.0001, a_ans / 2, a_ans / 2, a_ans / 2]) # a_ans, a_ans, a_ans q_end = np.array([ 0.0001, 0.0001, 0.0001, a_ans, a_ans, a_ans ]) # ([1.0001, -1.0001, 0.7001, a_ans + 0.2, a_ans + 0.2, a_ans + 0.2]) # q_start = np.array([-0.06235794, -0.00409771, 0.02960726, 0.14837708, 0.22618857, 0.09228618]) # q_end = np.array([-0.19746493, -0.00637689, 0.00991869, 0.17226557, 1.68673423, -0.22740581]) uz_0 = np.array([[0, 0, 0]]).transpose() (r1, r2, r3, Uz) = model(q_start, uz_0) x_cur_pos = r1[-1] (r1e, r2e, r3e, Uze) = model(q_end, uz_0)
def main(): """ Calculates and compare forward kinematics from model directly and Jacobian technique. """ a_ans = (2 * np.pi) / 3 runtime = time.time() total_time = 1 # (seconds) dt = 0.1 time_stamp = int(total_time / dt) t = dt i = 1 jac_delta_uz = np.ones(3) * 1e-1 model = lambda q, uz: moving_CTR(q, uz) q_static = np.array([0.001, 0.001, 0.001, 0.001, a_ans, a_ans]) Uz_traj_pos = np.zeros((3, time_stamp)) # Uz_end_model_pos = np.zeros((3, time_stamp)) uz0_start = np.array([[0.0, 0.0, 0.0]]).transpose() uz0_cur_pos = np.zeros((3, time_stamp)) Uz_end_cur_pos = np.zeros((3, time_stamp)) Uz_traj_vel = np.zeros((3, time_stamp)) delta_uz0 = np.zeros((3, time_stamp)) delta_Uz = np.zeros((3, time_stamp)) (r1, r2, r3, Uz_end) = moving_CTR(q_static, uz0_start) Uz_end_cur_pos[:, 0] = Uz_end.flatten() if quintic_fn: a1_coeffs = [[], [], []] a2_coeffs = [[], [], []] a3_coeffs = [[], [], []] waypoints = [Uz_end_cur_pos[:, 0], [0.0, 0.0, 0.0]] for x in range(len(waypoints)): traj = TrajectoryGenerator(waypoints[x], waypoints[(x + 1) % len(waypoints)], total_time) traj.solve_cubic() a1_coeffs[x] = traj.x_c a2_coeffs[x] = traj.y_c a3_coeffs[x] = traj.z_c quintic = TrajectoryRetreiverCubic() while i <= time_stamp - 1: # runtime = time.time() x_dim = np.zeros(3) # just for size TODO: change to just integer if quintic_fn: Uz_traj_pos[0, i] = quintic.calculate_position(a1_coeffs[0], t) # FOR INTEGRAL Uz_traj_pos[1, i] = quintic.calculate_position(a2_coeffs[0], t) Uz_traj_pos[2, i] = quintic.calculate_position(a3_coeffs[0], t) else: Uz_traj_pos[:, i] = alpha_position(t, total_time) if quintic_fn: Uz_traj_vel[0, i] = quintic.calculate_velocity(a1_coeffs[0], t) Uz_traj_vel[1, i] = quintic.calculate_velocity(a2_coeffs[0], t) Uz_traj_vel[2, i] = quintic.calculate_velocity(a3_coeffs[0], t) else: Uz_traj_vel[:, i] = alpha_velocity(t, total_time) delta_Uz[:, i] = Uz_traj_pos[:, i] - Uz_end_cur_pos[:, i - 1] # get trajectory from Jacobian r_jac = UzJacobian(jac_delta_uz, x_dim, q_static, uz0_cur_pos[:, i - 1], model) J_inv = r_jac.p_inv() delta_uz0[:, i] = J_inv @ (Uz_traj_vel[:, i] + Kp_Uz @ delta_Uz[:, i]) uz0_cur_pos[:, i] = uz0_cur_pos[:, i - 1] + delta_uz0[:, i].copy() * dt # Uz_end_cur_pos += Uz_traj_vel[:, i].copy() * dt # TODO: change to Forward Kinematics (r1, r2, r3, Uz) = model(q_static, uz0_cur_pos[:, i]) Uz_end_cur_pos[:, i] = Uz.flatten() # print(i, time.time()-runtime) t += dt i += 1 print("Done", time.time() - runtime) # fig = plt.figure() # ax = plt.axes(projection='3d') # colors = cm.rainbow(np.linspace(0, 1, len(Uz_end_cur_pos.transpose()))) # for y, c in zip(Uz_end_cur_pos.transpose(), colors): # # plt.scatter(x, y, color=c) # ax.scatter(y[0], y[1], y[2], linewidth=1, color=c) # ax.scatter(Uz_traj_pos[0], Uz_traj_pos[1], Uz_traj_pos[2], linewidth=1, label='Uz_traj_pos', marker='x') # # ax.plot3D(Uz_end_cur_pos[0], Uz_end_cur_pos[1], Uz_end_cur_pos[2], linewidth=1, linestyle='--', label='Uz_end_jac') # ax.scatter(Uz_end_cur_pos[0, -1], Uz_end_cur_pos[1, -1], Uz_end_cur_pos[2, -1], label=Uz_end_cur_pos[:, -1], marker='o') # print('Final Uz:', Uz_end_cur_pos[:, -1]) # # (r1,r2,r3,Uz) = moving_CTR(q_ans, uz_0) # # plot_3D(ax, r1, r2, r3, 'final position') # ax.legend() # ax.set_xlabel('tube1') # ax.set_ylabel('tube2') # ax.set_zlabel('tube3') # # plt.axis('equal') # # ax.set_aspect('equal') # # Create cubic bounding box to simulate equal aspect ratio # max_range = 0.2 # np.array([X.max()-X.min(), Y.max()-Y.min(), Z.max()-Z.min()]).max() # Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(0) # X.max()+X.min()) # Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(0) # Y.max()+Y.min()) # Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(0.3) # Z.max()+Z.min()) # # Comment or uncomment following both lines to test the fake bounding box: # for xb, yb, zb in zip(Xb, Yb, Zb): # ax.plot([xb], [yb], [zb], 'w') plt.subplots(1) tt = np.arange(0.0, total_time, dt) plt.plot(tt, Uz_end_cur_pos[0], label='1') plt.plot(tt, Uz_end_cur_pos[1], label='2') plt.plot(tt, Uz_end_cur_pos[2], label='3') plt.title('Uz_end_cur_pos') plt.legend() plt.subplots(1) tt = np.arange(0.0, total_time, dt) if quintic_fn: plt.plot(tt, quintic.calculate_position(a1_coeffs[0], tt)) else: plt.plot(tt, alpha_position(tt, total_time)) plt.title('uz0 pos trajectory') plt.subplots(1) if quintic_fn: plt.plot(tt, quintic.calculate_velocity(a1_coeffs[0], tt)) else: plt.plot(tt, alpha_velocity(tt, total_time)) plt.title('uz0 vel trajectory') plt.show()
def CTR_sim(a1_c, a2_c, a3_c, q_start, x_end_pos): """ Calculates the necessary thrust and torques for the quadrotor to follow the trajectory described by the sets of coefficients x_c, y_c, and z_c. """ runtime = time.time() # total_time = # (seconds) dt = 0.1 time_stamp = int(total_time/dt) t = dt i = 1 jac_del_q = np.ones(6) * 1e-1 uz_0 = np.array([[0, 0, 0]]).transpose() model = lambda q, uz_0: moving_CTR(q, uz_0) q_des_pos = np.zeros((6, time_stamp)) # [BBBaaa] x_des_pos = np.zeros((3, time_stamp)) # [r] x_des_vel = np.zeros((3, time_stamp)) # [r] x_cur_pos = np.zeros((3, time_stamp)) # [r] delta_x = np.zeros((3, time_stamp)) # [r] delta_v = np.zeros((3, time_stamp)) # [r] delta_q = np.zeros((6, time_stamp)) # [BBBaaa] # delta_x = np.zeros(3) # [r] integral = 0.0 quintic = TrajectoryRetreiver() q_des_pos[:, 0] = q_start x_des_pos[0, 0] = quintic.calculate_position(a1_c[0], t) x_des_pos[1, 0] = quintic.calculate_position(a2_c[0], t) x_des_pos[2, 0] = quintic.calculate_position(a3_c[0], t) x_cur_pos[:, 0] = x_des_pos[:, 0] while i < time_stamp: # runtime = time.time() x = np.zeros(3) # just for size TODO: change to just integer x_des_pos[0, i] = quintic.calculate_position(a1_c[0], t) x_des_pos[1, i] = quintic.calculate_position(a2_c[0], t) x_des_pos[2, i] = quintic.calculate_position(a3_c[0], t) x_des_vel[0, i] = quintic.calculate_velocity(a1_c[0], t) x_des_vel[1, i] = quintic.calculate_velocity(a2_c[0], t) x_des_vel[2, i] = quintic.calculate_velocity(a3_c[0], t) # print('t:', t) # print('i:', i) # print(alpha_position(t, total_time)) delta_x[:, i] = x_des_pos[:, i] - x_cur_pos[:, i-1] # delta_x[:, i] = x_end_pos - x_cur_pos[:, i-1] # delta_v[:, i] = x_des_vel[:, i] - x_des_vel[:, i-1] # delta_v[:, i] = (delta_x[:, i] - delta_x[:, i-1])/dt integral += delta_x[:, i] * dt # print('delta_x', delta_x) if search: # add iterations for solution searching/clipping else just 1 pass else: # get trajectory from Jacobian r_jac = Jacobian(jac_del_q, x, q_des_pos[:, i-1].flatten(), uz_0, model) J_inv = r_jac.p_inv() delta_q[:, i] = J_inv @ (x_des_vel[:, i] + Kp_x@(delta_x[:, i]) + Ki_x@integral + Kd_x@(delta_v[:, i])) # print('delta_q', delta_q) q_des_pos[:, i] = q_des_pos[:, i-1] + delta_q[:, i] * dt (r1,r2,r3,Uz) = model(q_des_pos[:, i], uz_0) # FORWARD KINEMATICS x_cur_pos[:, i] = r1[-1] # print(i, time.time()-runtime) t += dt i += 1 print("Done", time.time()-runtime) print('x_des_vel[:, 1]:', x_des_vel[:, 1]) print('x_des_vel[:, -1]:', x_des_vel[:, -1]) print('q_des_pos[:, 0]:', q_des_pos[:, 0]) print('q_des_pos[:, 1]:', q_des_pos[:, 1]) print('q_des_pos[:, -1]:', q_des_pos[:, -1]) fig = plt.figure() ax = plt.axes(projection='3d') # colors = cm.rainbow(np.linspace(0, 1, len(x_cur_pos.transpose()))) # for y, c in zip(x_cur_pos.transpose(), colors): # # plt.scatter(x, y, color=c) # ax.scatter(y[0], y[1], y[2], linewidth=1, color=c) ax.plot3D(x_cur_pos[0], x_cur_pos[1], x_cur_pos[2], linewidth=1, label='x_cur_pos', color='red') ax.scatter(x_des_pos[0], x_des_pos[1], x_des_pos[2], linewidth=1, label='x_des_traj', marker='.') # (r1,r2,r3,Uz) = moving_CTR(q_des_pos[:, 0], uz_0) # plot_3D(ax, r1, r2, r3, 'initial pos') # (r1,r2,r3,Uz) = moving_CTR(q_des_pos[:, -1], uz_0) # plot_3D(ax, r1, r2, r3, 'final pos - q w/ controller') ax.legend() ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z ') # plt.axis('equal') # ax.set_aspect('equal') # # Create cubic bounding box to simulate equal aspect ratio # max_range = 0.2 # np.array([X.max()-X.min(), Y.max()-Y.min(), Z.max()-Z.min()]).max() # Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(0) # X.max()+X.min()) # Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(0) # Y.max()+Y.min()) # Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(0.3) # Z.max()+Z.min()) # # Comment or uncomment following both lines to test the fake bounding box: # for xb, yb, zb in zip(Xb, Yb, Zb): # ax.plot([xb], [yb], [zb], 'w') plt.subplots(1) tt = np.arange(0.0, total_time, dt) plt.plot(tt, q_des_pos[0], label='q_B1') plt.plot(tt, q_des_pos[1], label='q_B2') plt.plot(tt, q_des_pos[2], label='q_B3') plt.plot(tt, q_des_pos[3], label='q_a1') plt.plot(tt, q_des_pos[4], label='q_a2') plt.plot(tt, q_des_pos[5], label='q_a3') plt.title('q inputs') plt.legend() plt.subplots(1) tt = np.arange(0.0, total_time, dt) plt.plot(tt, delta_x[0], label='x') plt.plot(tt, delta_x[1], label='y') plt.plot(tt, delta_x[2], label='z') plt.title('delta_x') plt.legend() plt.subplots(1) tt = np.arange(0.0, total_time, dt) plt.plot(tt, delta_q[0], label='del_q.B1') plt.plot(tt, delta_q[1], label='del_q.B2') plt.plot(tt, delta_q[2], label='del_q.B3') plt.plot(tt, delta_q[3], label='del_q.a1') plt.plot(tt, delta_q[4], label='del_q.a2') plt.plot(tt, delta_q[5], label='del_q.a3') plt.title('delta_q') plt.legend() plt.show()