From e168e8fe082c83335725aa4925ed9e832fc7e79a Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Fri, 23 Sep 2022 18:39:08 +0800 Subject: [PATCH 01/30] Initial commit for lspb_s_curve --- competition/edit_this.py | 345 ++++++++++++++++++++++++++++++--------- 1 file changed, 268 insertions(+), 77 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index a77d4b67b..68c9f40a9 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -25,6 +25,9 @@ """ import numpy as np +import scipy.interpolate +import scipy.integrate +from math import sqrt, sin, cos, atan2, pi from collections import deque @@ -94,48 +97,188 @@ def __init__(self, # REPLACE THIS (START) ## ######################### - # Example: harcode waypoints through the gates. + # hardcode gate yaw for testing + # TODO Replaced with more intelligent method of deciding direction to enter gate + self.NOMINAL_GATES[0][5] += pi + self.NOMINAL_GATES[2][5] += pi + + ### Spline fitting between waypoints ### + + length = len(self.NOMINAL_GATES) + 2 + # end goal if use_firmware: - waypoints = [(self.initial_obs[0], self.initial_obs[2], initial_info["gate_dimensions"]["tall"]["height"])] # Height is hardcoded scenario knowledge. + waypoints = [(self.initial_obs[0], self.initial_obs[2], initial_info["gate_dimensions"]["tall"]["height"], self.initial_obs[8])] # Height is hardcoded scenario knowledge. else: - waypoints = [(self.initial_obs[0], self.initial_obs[2], self.initial_obs[4])] + waypoints = [(self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8])] + for idx, g in enumerate(self.NOMINAL_GATES): - height = initial_info["gate_dimensions"]["tall"]["height"] if g[6] == 0 else initial_info["gate_dimensions"]["low"]["height"] - if g[5] > 0.75 or g[5] < 0: - if idx == 2: # Hardcoded scenario knowledge (direction in which to take gate 2). - waypoints.append((g[0]+0.3, g[1]-0.3, height)) - waypoints.append((g[0]-0.3, g[1]-0.3, height)) - else: - waypoints.append((g[0]-0.3, g[1], height)) - waypoints.append((g[0]+0.3, g[1], height)) - else: - if idx == 3: # Hardcoded scenario knowledge (correct how to take gate 3). - waypoints.append((g[0]+0.1, g[1]-0.3, height)) - waypoints.append((g[0]+0.1, g[1]+0.3, height)) - else: - waypoints.append((g[0], g[1]-0.3, height)) - waypoints.append((g[0], g[1]+0.3, height)) - waypoints.append([initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4]]) - - # Polynomial fit - self.waypoints = np.array(waypoints) - deg = 6 - t = np.arange(self.waypoints.shape[0]) - fx = np.poly1d(np.polyfit(t, self.waypoints[:,0], deg)) - fy = np.poly1d(np.polyfit(t, self.waypoints[:,1], deg)) - fz = np.poly1d(np.polyfit(t, self.waypoints[:,2], deg)) - duration = 15 - t_scaled = np.linspace(t[0], t[-1], int(duration*self.CTRL_FREQ)) - self.ref_x = fx(t_scaled) - self.ref_y = fy(t_scaled) - self.ref_z = fz(t_scaled) + [x, y, _, _, _, yaw, typ] = g + z = initial_info["gate_dimensions"]["tall"]["height"] if typ == 0 else initial_info["gate_dimensions"]["low"]["height"] + waypoints.append((x, y, z, yaw)) + # end goal + waypoints.append((initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4], initial_info["x_reference"][8])) + + # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve + grad_scale = 2 + + [x0, y0, _, yaw0] = waypoints[0] + dx0 = sin(yaw0) + dy0 = cos(yaw0) + x_coeffs = np.zeros((4,len(waypoints)-1)) + y_coeffs = np.zeros((4,len(waypoints)-1)) + + # "time" for each waypoint + # TODO replace with more intelligent time intervals + ts = np.arange(length) + for idx in range(1, length): + [xf, yf, _, yawf] = waypoints[idx] + inv_t = 1/(ts[idx] - ts[idx - 1]) + inv_t2 = inv_t*inv_t + dxf = sin(yawf) * grad_scale + dyf = cos(yawf) * grad_scale + dx = xf - x0 + dy = yf - y0 + x_a0 = x0 + x_a1 = dx0 + x_a2 = (3*dx*inv_t - 2*dx0 - dxf)*inv_t + x_a3 = (-2*dx*inv_t + (dxf + dx0))*inv_t2 + x_coeffs[:,idx-1] = (x_a3, x_a2, x_a1, x_a0) + y_a0 = y0 + y_a1 = dy0 + y_a2 = (3*dy*inv_t - 2*dy0 - dyf)*inv_t + y_a3 = (-2*dy*inv_t + (dyf + dy0))*inv_t2 + y_coeffs[:,idx-1] = (y_a3, y_a2, y_a1, y_a0) + [x0, y0] = [xf, yf] + [dx0, dy0] = [dxf, dyf] + waypoints = np.array(waypoints) + z_coeffs = scipy.interpolate.PchipInterpolator(ts, waypoints[:,2], 0).c # ascending at start, descending at end + + def df_idx(idx): + def df(t): + dx = (3*x_coeffs[1,idx]*t + 2*x_coeffs[2,idx])*t + x_coeffs[3,idx] + dy = (3*y_coeffs[1,idx]*t + 2*y_coeffs[2,idx])*t + y_coeffs[3,idx] + dz = (3*z_coeffs[1,idx]*t + 2*z_coeffs[2,idx])*t + z_coeffs[3,idx] + return sqrt(dx*dx+dy*dy+dz*dz) + return df + + def f_(coeffs): + def f(t): + for idx in range(length-1): + if (ts[idx+1] > t): + break + t -= ts[idx] + return ((coeffs[0,idx]*t + coeffs[1,idx])*t + coeffs[2,idx])*t + coeffs[3,idx] + return f + + def df_(coeffs): + def f(t): + for idx in range(length-1): + if (ts[idx+1] > t): + break + t -= ts[idx] + return (3*coeffs[0,idx]*t + 2*coeffs[1,idx])*t + coeffs[2,idx] + return f + + def d2f_(coeffs): + def f(t): + for idx in range(length-1): + if (ts[idx+1] > t): + break + t -= ts[idx] + return 6*coeffs[0,idx]*t + 2*coeffs[1,idx] + return f + + # Integrate to get pathlength + pathlength = 0 + for idx in range(length-1): + pathlength += scipy.integrate.quad(df_idx(idx), 0, ts[idx+1] - ts[idx])[0] + self.scaling_factor = ts[-1] / pathlength + + self.x = f_(x_coeffs) + self.y = f_(y_coeffs) + self.z = f_(z_coeffs) + + self.dx = df_(x_coeffs) + self.dy = df_(y_coeffs) + self.dz = df_(z_coeffs) + + self.d2x = d2f_(x_coeffs) + self.d2y = d2f_(y_coeffs) + self.d2z = d2f_(z_coeffs) + + duration = ts[-1] - ts[0] + t_scaled = np.linspace(ts[0], ts[-1], int(duration*self.CTRL_FREQ)) + x_scaled = np.array(tuple(map(self.x, t_scaled))) + y_scaled = np.array(tuple(map(self.y, t_scaled))) + z_scaled = np.array(tuple(map(self.z, t_scaled))) if self.VERBOSE: + print(x_coeffs) + print(y_coeffs) + print(z_coeffs) + print(waypoints) + print(t_scaled) + print(x_scaled) + print(y_scaled) + print(z_scaled) # Plot trajectory in each dimension and 3D. - plot_trajectory(t_scaled, self.waypoints, self.ref_x, self.ref_y, self.ref_z) - + plot_trajectory(t_scaled, waypoints, x_scaled, y_scaled, z_scaled) # Draw the trajectory on PyBullet's GUI - draw_trajectory(initial_info, self.waypoints, self.ref_x, self.ref_y, self.ref_z) + draw_trajectory(initial_info, waypoints, x_scaled, y_scaled, z_scaled) + + ### S-curve ### + sf = pathlength + + # Kinematic limits + # TODO determine better estimates from model + v_max = 2.e-1 + a_max = 2.e-1 + j_max = 2.e-1 + + s = np.zeros(8) + v = np.zeros(8) + a = np.zeros(8) + j = np.array((j_max, 0, -j_max, 0, -j_max, 0, j_max, 0)) + t = np.zeros(8) + T = np.zeros(8) + + def fill(): + for i in range(7): + dt = T[i+1] + t[i+1] = t[i] + dt + s[i+1] = ((j[i]/6*dt + a[i]/2)*dt + v[i])*dt+ s[i] + v[i+1] = (j[i]/2*dt + a[i] )*dt + v[i] + a[i+1] = j[i] *dt + a[i] + + T[1] = T[3] = T[5] = T[7] = min(sqrt(v_max/j_max), a_max/j_max) # min(wont't hit a_max, will hit a_max) + fill() + ds = sf - s[-1] + if ds < 0: + # T[2] = T[4] = T[6] = 0 + T[1] = T[3] = T[5] = T[7] = (xf/(2*j_max))**(1./3) + else: + T_2_max = (v_max - 2*v[1])/a_max + added_T_2 = (-v[1]+sqrt(v[1]*v[1]/2 + a_max*ds))/a_max + if added_T_2 > T_2_max: + T[2] = T[6] = T_2_max + ds -= (2*v[1] + a_max*T_2_max)*T_2_max + T[4] = ds/v_max + else: + # T[4] = 0 + T[2] = T[6] = added_T_2 + fill() + def s_curve(t_): + for i in range(7): + if t[i+1] > t_: + break + dt = t_ - t[i] + s_ = ((j[i]/6*dt + a[i]/2)*dt + v[i])*dt+ s[i] + v_ = (j[i]/2*dt + a[i] )*dt + v[i] + a_ = j[i] *dt + a[i] + return [s_, v_, a_] + self.s = s_curve + self.start_t = -1 + self.end_t = t[-1] ######################### # REPLACE THIS (END) #### @@ -176,60 +319,108 @@ def cmdFirmware(self, # REPLACE THIS (START) ## ######################### - # Handwritten solution for GitHub's getting_stated scenario. + t = time / self.scaling_factor + # # Handwritten solution for GitHub's getting_stated scenario. if iteration == 0: height = 1 duration = 2 command_type = Command(2) # Take-off. args = [height, duration] + elif iteration >= 3*self.CTRL_FREQ: + if self.start_t < 0: + self.start_t = t + t -= self.start_t + if t > self.end_t: + height = 0. + duration = 3 + command_type = Command(3) # Land. + args = [height, duration] + [curve_t, curve_v, curve_a] = self.s(t) + curve_t *= self.scaling_factor + curve_v *= self.scaling_factor + curve_a *= self.scaling_factor + x_c = self.x(curve_t) + y_c = self.y(curve_t) + z_c = self.z(curve_t) + target_pos = np.array([x_c, y_c, z_c]) + print(target_pos) + + dx_c = self.dx(curve_t) + dy_c = self.dy(curve_t) + dz_c = self.dz(curve_t) + d2x_c = self.d2x(curve_t) + d2y_c = self.d2y(curve_t) + d2z_c = self.d2z(curve_t) + tangent = np.array((dx_c,dy_c,dz_c)) + dtangent = np.array((d2x_c,d2y_c,d2z_c)) + den = np.linalg.norm(tangent[:2]) + if den < 1e-9: + w = 0 + else: + num = dx_c * d2y_c - dy_c * d2x_c + w = num/den + w *= curve_v - elif iteration >= 3*self.CTRL_FREQ and iteration < 20*self.CTRL_FREQ: - step = min(iteration-3*self.CTRL_FREQ, len(self.ref_x) -1) - target_pos = np.array([self.ref_x[step], self.ref_y[step], self.ref_z[step]]) - target_vel = np.zeros(3) - target_acc = np.zeros(3) - target_yaw = 0. + target_vel = tangent * curve_v + target_acc = tangent * curve_a + dtangent * curve_v**2 + target_yaw = atan2(dy_c, dx_c) target_rpy_rates = np.zeros(3) + # target_vel = np.zeros(3) + # target_acc = np.zeros(3) + # target_yaw = 0. + # target_rpy_rates = np.zeros(3) command_type = Command(1) # cmdFullState. args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] - - elif iteration == 20*self.CTRL_FREQ: - command_type = Command(6) # notify setpoint stop. - args = [] - - elif iteration == 20*self.CTRL_FREQ+1: - x = self.ref_x[-1] - y = self.ref_y[-1] - z = 1.5 - yaw = 0. - duration = 2.5 - - command_type = Command(5) # goTo. - args = [[x, y, z], yaw, duration, False] - - elif iteration == 23*self.CTRL_FREQ: - x = self.initial_obs[0] - y = self.initial_obs[2] - z = 1.5 - yaw = 0. - duration = 6 - - command_type = Command(5) # goTo. - args = [[x, y, z], yaw, duration, False] - - elif iteration == 30*self.CTRL_FREQ: - height = 0. - duration = 3 - - command_type = Command(3) # Land. - args = [height, duration] - - elif iteration == 33*self.CTRL_FREQ-1: - command_type = Command(-1) # Terminate command to be sent once trajectory is completed. - args = [] + + + # elif iteration >= 3*self.CTRL_FREQ and iteration < 20*self.CTRL_FREQ: + # step = min(iteration-3*self.CTRL_FREQ, len(self.ref_x) -1) + # target_pos = np.array([self.ref_x[step], self.ref_y[step], self.ref_z[step]]) + # target_vel = np.zeros(3) + # target_acc = np.zeros(3) + # target_yaw = 0. + # target_rpy_rates = np.zeros(3) + + # command_type = Command(1) # cmdFullState. + # args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] + + # elif iteration == 20*self.CTRL_FREQ: + # command_type = Command(6) # notify setpoint stop. + # args = [] + + # elif iteration == 20*self.CTRL_FREQ+1: + # x = self.ref_x[-1] + # y = self.ref_y[-1] + # z = 1.5 + # yaw = 0. + # duration = 2.5 + + # command_type = Command(5) # goTo. + # args = [[x, y, z], yaw, duration, False] + + # elif iteration == 23*self.CTRL_FREQ: + # x = self.initial_obs[0] + # y = self.initial_obs[2] + # z = 1.5 + # yaw = 0. + # duration = 6 + + # command_type = Command(5) # goTo. + # args = [[x, y, z], yaw, duration, False] + + # elif iteration == 30*self.CTRL_FREQ: + # height = 0. + # duration = 3 + + # command_type = Command(3) # Land. + # args = [height, duration] + + # elif iteration == 33*self.CTRL_FREQ-1: + # command_type = Command(-1) # Terminate command to be sent once trajectory is completed. + # args = [] else: command_type = Command(0) # None. From ead26af5bc6e0228b564239fc3f844c9dc647d69 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sat, 1 Oct 2022 21:30:12 +0800 Subject: [PATCH 02/30] Time delta btw gates --- competition/edit_this.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 68c9f40a9..037754f15 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -97,10 +97,11 @@ def __init__(self, # REPLACE THIS (START) ## ######################### - # hardcode gate yaw for testing - # TODO Replaced with more intelligent method of deciding direction to enter gate - self.NOMINAL_GATES[0][5] += pi - self.NOMINAL_GATES[2][5] += pi + # Kinematic limits + # TODO determine better estimates from model + v_max = 2 + a_max = .2 + j_max = .2 ### Spline fitting between waypoints ### @@ -128,8 +129,15 @@ def __init__(self, y_coeffs = np.zeros((4,len(waypoints)-1)) # "time" for each waypoint - # TODO replace with more intelligent time intervals - ts = np.arange(length) + # time interval determined by eulcidean distance between waypoints along xy plane + # ts = np.arange(length) + ts = np.zeros(length) + [x_prev, y_prev, _, _] = waypoints[0] + for idx in range(1,length): + [x_curr, y_curr, _, _] = waypoints[idx] + xy_norm = sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + ts[idx] = ts[idx-1] + xy_norm / v_max + for idx in range(1, length): [xf, yf, _, yawf] = waypoints[idx] inv_t = 1/(ts[idx] - ts[idx - 1]) @@ -229,12 +237,6 @@ def f(t): ### S-curve ### sf = pathlength - # Kinematic limits - # TODO determine better estimates from model - v_max = 2.e-1 - a_max = 2.e-1 - j_max = 2.e-1 - s = np.zeros(8) v = np.zeros(8) a = np.zeros(8) From 0183f51f108d3ba302a727c3c413cc9f5f32482f Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sat, 1 Oct 2022 21:32:26 +0800 Subject: [PATCH 03/30] Decision for whether to flip gate orientation --- competition/edit_this.py | 58 +++++++++++++++++++++++++++++++--------- 1 file changed, 45 insertions(+), 13 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 037754f15..ccf2205d2 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -102,29 +102,25 @@ def __init__(self, v_max = 2 a_max = .2 j_max = .2 + # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve + grad_scale = 1 ### Spline fitting between waypoints ### length = len(self.NOMINAL_GATES) + 2 # end goal if use_firmware: - waypoints = [(self.initial_obs[0], self.initial_obs[2], initial_info["gate_dimensions"]["tall"]["height"], self.initial_obs[8])] # Height is hardcoded scenario knowledge. + waypoints = [[self.initial_obs[0], self.initial_obs[2], initial_info["gate_dimensions"]["tall"]["height"], self.initial_obs[8]],] # Height is hardcoded scenario knowledge. else: - waypoints = [(self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8])] + waypoints = [[self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8]],] for idx, g in enumerate(self.NOMINAL_GATES): [x, y, _, _, _, yaw, typ] = g z = initial_info["gate_dimensions"]["tall"]["height"] if typ == 0 else initial_info["gate_dimensions"]["low"]["height"] - waypoints.append((x, y, z, yaw)) + waypoints.append([x, y, z, yaw]) # end goal - waypoints.append((initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4], initial_info["x_reference"][8])) - - # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve - grad_scale = 2 + waypoints.append([initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4], initial_info["x_reference"][8]]) - [x0, y0, _, yaw0] = waypoints[0] - dx0 = sin(yaw0) - dy0 = cos(yaw0) x_coeffs = np.zeros((4,len(waypoints)-1)) y_coeffs = np.zeros((4,len(waypoints)-1)) @@ -138,9 +134,45 @@ def __init__(self, xy_norm = sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) ts[idx] = ts[idx-1] + xy_norm / v_max + # Flip gates + # Selectively flip gate orientation based on vector from previous and to next waypoint, + # and current gate's orientation + [x_prev, y_prev, _, _] = waypoints[0] + [x_curr, y_curr, _, yaw_curr] = waypoints[1] + dt = ts[1] - ts[0] + for idx in range(1,length-1): + [x_next, y_next, _, yaw_next] = waypoints[idx+1] + dt_next = ts[idx+1] - ts[idx] + dxf = sin(yaw_curr) * grad_scale + dyf = cos(yaw_curr) * grad_scale + vx1 = np.array((x_curr - x_prev,dt)) + vx21 = np.array((dxf,1)) + vx22 = np.array((-dxf,1)) + vx3 = np.array((x_next - x_curr,dt_next)) + + vy1 = np.array((y_curr - y_prev,dt)) + vy21 = np.array((dyf,1)) + vy22 = np.array((-dyf,1)) + vy3 = np.array((y_next - y_curr,dt_next)) + choice_1 = vx1.dot(vx21)/np.linalg.norm(vx1)/np.linalg.norm(vx21)+vx21.dot(vx3)/np.linalg.norm(vx21)/np.linalg.norm(vx3) + choice_1 += vy1.dot(vy21)/np.linalg.norm(vy1)/np.linalg.norm(vy21)+vy21.dot(vy3)/np.linalg.norm(vy21)/np.linalg.norm(vy3) + choice_2 = vx1.dot(vx22)/np.linalg.norm(vx1)/np.linalg.norm(vx22)+vx22.dot(vx3)/np.linalg.norm(vx22)/np.linalg.norm(vx3) + choice_2 += vy1.dot(vy22)/np.linalg.norm(vy1)/np.linalg.norm(vy22)+vy22.dot(vy3)/np.linalg.norm(vy22)/np.linalg.norm(vy3) + if choice_2 > choice_1: + waypoints[idx][3] += pi + print("flipping", idx) + x_prev, y_prev = x_curr, y_curr + x_curr, y_curr = x_next, y_next + yaw_curr = yaw_next + dt = dt_next + + [x0, y0, _, yaw0] = waypoints[0] + dx0 = sin(yaw0) + dy0 = cos(yaw0) for idx in range(1, length): [xf, yf, _, yawf] = waypoints[idx] - inv_t = 1/(ts[idx] - ts[idx - 1]) + dt = ts[idx] - ts[idx - 1] + inv_t = 1/dt inv_t2 = inv_t*inv_t dxf = sin(yawf) * grad_scale dyf = cos(yawf) * grad_scale @@ -159,7 +191,7 @@ def __init__(self, [x0, y0] = [xf, yf] [dx0, dy0] = [dxf, dyf] waypoints = np.array(waypoints) - z_coeffs = scipy.interpolate.PchipInterpolator(ts, waypoints[:,2], 0).c # ascending at start, descending at end + z_coeffs = scipy.interpolate.PchipInterpolator(ts, waypoints[:,2], 0).c def df_idx(idx): def df(t): @@ -347,7 +379,7 @@ def cmdFirmware(self, y_c = self.y(curve_t) z_c = self.z(curve_t) target_pos = np.array([x_c, y_c, z_c]) - print(target_pos) + # print(obs[0] - x_c, obs[2] - y_c, obs[4] - z_c) dx_c = self.dx(curve_t) dy_c = self.dy(curve_t) From 5a95947775359c386911b94e7d734d1efa15a623 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sat, 1 Oct 2022 21:32:47 +0800 Subject: [PATCH 04/30] Slightly shift gate position due to turns --- competition/edit_this.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/competition/edit_this.py b/competition/edit_this.py index ccf2205d2..1d178b6ec 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -166,6 +166,30 @@ def __init__(self, yaw_curr = yaw_next dt = dt_next + # Slightly shift gate waypoint to deal with over/undershoot of PID controller + # E.g. for double left > shift gate to right + # for double right > shift gate to left + # for right then left > shift gate to right + # for left then right > shift gate to left + # For consecutive turns in same direction, distance shifted is larger + shift_dist = .1 + yaw_prev = waypoints[0][3] + yaw_curr = waypoints[1][3] + dyaw = yaw_curr - yaw_prev + for idx in range(1,length-1): + yaw_next = waypoints[idx+1][3] + dyaw_next = yaw_next - yaw_curr + scale = (((abs(dyaw) + abs(dyaw_next)) / 2. / pi + 1.) % 2.) - 1. + if dyaw > 0: + scale = -scale + if dyaw * dyaw_next < 0: + scale = -scale + waypoints[idx][0] += cos(yaw_curr) * shift_dist * scale + waypoints[idx][1] -= sin(yaw_curr) * shift_dist * scale + yaw_prev = yaw_curr + yaw_curr = yaw_next + dyaw = dyaw_next + [x0, y0, _, yaw0] = waypoints[0] dx0 = sin(yaw0) dy0 = cos(yaw0) From b921aa7ffe06688d83da5d605359c14a92557c01 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:16:27 +0800 Subject: [PATCH 05/30] Correct yaw representation --- competition/edit_this.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 1d178b6ec..063be25f5 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -114,8 +114,10 @@ def __init__(self, else: waypoints = [[self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8]],] + half_pi = 0.5*pi for idx, g in enumerate(self.NOMINAL_GATES): [x, y, _, _, _, yaw, typ] = g + yaw += half_pi z = initial_info["gate_dimensions"]["tall"]["height"] if typ == 0 else initial_info["gate_dimensions"]["low"]["height"] waypoints.append([x, y, z, yaw]) # end goal @@ -143,8 +145,8 @@ def __init__(self, for idx in range(1,length-1): [x_next, y_next, _, yaw_next] = waypoints[idx+1] dt_next = ts[idx+1] - ts[idx] - dxf = sin(yaw_curr) * grad_scale - dyf = cos(yaw_curr) * grad_scale + dxf = cos(yaw_curr) * grad_scale + dyf = sin(yaw_curr) * grad_scale vx1 = np.array((x_curr - x_prev,dt)) vx21 = np.array((dxf,1)) vx22 = np.array((-dxf,1)) @@ -184,22 +186,22 @@ def __init__(self, scale = -scale if dyaw * dyaw_next < 0: scale = -scale - waypoints[idx][0] += cos(yaw_curr) * shift_dist * scale - waypoints[idx][1] -= sin(yaw_curr) * shift_dist * scale + waypoints[idx][0] -= sin(yaw_curr) * shift_dist * scale + waypoints[idx][1] -= cos(yaw_curr) * shift_dist * scale yaw_prev = yaw_curr yaw_curr = yaw_next dyaw = dyaw_next [x0, y0, _, yaw0] = waypoints[0] - dx0 = sin(yaw0) - dy0 = cos(yaw0) + dx0 = cos(yaw0) + dy0 = sin(yaw0) for idx in range(1, length): [xf, yf, _, yawf] = waypoints[idx] dt = ts[idx] - ts[idx - 1] inv_t = 1/dt inv_t2 = inv_t*inv_t - dxf = sin(yawf) * grad_scale - dyf = cos(yawf) * grad_scale + dxf = cos(yawf) * grad_scale + dyf = sin(yawf) * grad_scale dx = xf - x0 dy = yf - y0 x_a0 = x0 From 10d5bf344bb7efdf875655a2b47f434a8c938cfa Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:21:16 +0800 Subject: [PATCH 06/30] Reference angular rates --- competition/edit_this.py | 61 ++++++++++++++++++++++++++++++++++------ 1 file changed, 52 insertions(+), 9 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 063be25f5..2491a8f2f 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -254,6 +254,15 @@ def f(t): return 6*coeffs[0,idx]*t + 2*coeffs[1,idx] return f + def d3f_(coeffs): + def f(t): + for idx in range(length-1): + if (ts[idx+1] > t): + break + t -= ts[idx] + return 6*coeffs[0,idx] + return f + # Integrate to get pathlength pathlength = 0 for idx in range(length-1): @@ -272,6 +281,10 @@ def f(t): self.d2y = d2f_(y_coeffs) self.d2z = d2f_(z_coeffs) + self.d3x = d3f_(x_coeffs) + self.d3y = d3f_(y_coeffs) + self.d3z = d3f_(z_coeffs) + duration = ts[-1] - ts[0] t_scaled = np.linspace(ts[0], ts[-1], int(duration*self.CTRL_FREQ)) x_scaled = np.array(tuple(map(self.x, t_scaled))) @@ -335,10 +348,24 @@ def s_curve(t_): s_ = ((j[i]/6*dt + a[i]/2)*dt + v[i])*dt+ s[i] v_ = (j[i]/2*dt + a[i] )*dt + v[i] a_ = j[i] *dt + a[i] - return [s_, v_, a_] + j_ = j[i] + return [s_, v_, a_, j_] self.s = s_curve self.start_t = -1 self.end_t = t[-1] + def rpy2rot(roll, pitch, yaw): + sr = sin(roll) + cr = cos(roll) + sp = sin(pitch) + cp = cos(pitch) + sy = sin(yaw) + cy = cos(yaw) + m = np.matrix(((cy*cp, -sy*cr+cy*sp*sr, sy*sr+cy*cr*sp), + (sy*cp, cy*cr+sy*sp*sr, -cy*sr+sp*sy*cr), + (-sp , cp*sr , cp*cr))) + return m + self.rpy2rot = rpy2rot + ######################### # REPLACE THIS (END) #### @@ -397,10 +424,11 @@ def cmdFirmware(self, duration = 3 command_type = Command(3) # Land. args = [height, duration] - [curve_t, curve_v, curve_a] = self.s(t) + [curve_t, curve_v, curve_a, curve_j] = self.s(self.scaled_t) curve_t *= self.scaling_factor curve_v *= self.scaling_factor curve_a *= self.scaling_factor + curve_j *= self.scaling_factor x_c = self.x(curve_t) y_c = self.y(curve_t) z_c = self.z(curve_t) @@ -413,20 +441,35 @@ def cmdFirmware(self, d2x_c = self.d2x(curve_t) d2y_c = self.d2y(curve_t) d2z_c = self.d2z(curve_t) + d3x_c = self.d3x(curve_t) + d3y_c = self.d3y(curve_t) + d3z_c = self.d3z(curve_t) tangent = np.array((dx_c,dy_c,dz_c)) dtangent = np.array((d2x_c,d2y_c,d2z_c)) + d2tangent = np.array((d3x_c,d3y_c,d3z_c)) + + target_yaw = atan2(dy_c, dx_c) + target_vel = tangent * curve_v + target_acc = tangent * curve_a + dtangent * curve_v**2 + + # Roll, pitch rate + # Small angle approximation + target_jerk = tangent * curve_j + d2tangent * curve_v**3 + 3 * dtangent * curve_v * curve_a + Jinv = self.rpy2rot(obs[6], obs[7], obs[8]).transpose() + body_jerk = np.matmul(Jinv, target_jerk.transpose()) + p = 0.027/9.8*body_jerk[0,1] # roll rate = mass / g * y_jerk + q = 0.027/9.8*body_jerk[0,0] # pitch rate = mass / g * x_jerk + + # Yaw rate den = np.linalg.norm(tangent[:2]) if den < 1e-9: - w = 0 + r = 0 else: num = dx_c * d2y_c - dy_c * d2x_c - w = num/den - w *= curve_v + r = num/den + r *= curve_v - target_vel = tangent * curve_v - target_acc = tangent * curve_a + dtangent * curve_v**2 - target_yaw = atan2(dy_c, dx_c) - target_rpy_rates = np.zeros(3) + target_rpy_rates = np.array((p,q,r)) # target_vel = np.zeros(3) # target_acc = np.zeros(3) # target_yaw = 0. From a60d7d5b84bddd1527ffc22459d9020c216bb210 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:24:55 +0800 Subject: [PATCH 07/30] Modified mission plan --- competition/edit_this.py | 73 ++++++++++++---------------------------- 1 file changed, 22 insertions(+), 51 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 2491a8f2f..8d710ea17 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -353,6 +353,10 @@ def s_curve(t_): self.s = s_curve self.start_t = -1 self.end_t = t[-1] + self.scaled_t = 0 + self.time_scale = 1 + self.takeoff_land_duration = 2 + def rpy2rot(roll, pitch, yaw): sr = sin(roll) cr = cos(roll) @@ -406,20 +410,17 @@ def cmdFirmware(self, # REPLACE THIS (START) ## ######################### - t = time / self.scaling_factor - # # Handwritten solution for GitHub's getting_stated scenario. if iteration == 0: height = 1 - duration = 2 + duration = self.takeoff_land_duration command_type = Command(2) # Take-off. args = [height, duration] - elif iteration >= 3*self.CTRL_FREQ: - if self.start_t < 0: - self.start_t = t - t -= self.start_t - if t > self.end_t: + elif iteration >= 2*self.CTRL_FREQ and iteration < (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ: + self.scaled_t += self.time_scale / self.CTRL_FREQ / self.scaling_factor + + if self.scaled_t > self.end_t: height = 0. duration = 3 command_type = Command(3) # Land. @@ -477,53 +478,21 @@ def cmdFirmware(self, command_type = Command(1) # cmdFullState. args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] - - - # elif iteration >= 3*self.CTRL_FREQ and iteration < 20*self.CTRL_FREQ: - # step = min(iteration-3*self.CTRL_FREQ, len(self.ref_x) -1) - # target_pos = np.array([self.ref_x[step], self.ref_y[step], self.ref_z[step]]) - # target_vel = np.zeros(3) - # target_acc = np.zeros(3) - # target_yaw = 0. - # target_rpy_rates = np.zeros(3) - - # command_type = Command(1) # cmdFullState. - # args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] - - # elif iteration == 20*self.CTRL_FREQ: - # command_type = Command(6) # notify setpoint stop. - # args = [] - # elif iteration == 20*self.CTRL_FREQ+1: - # x = self.ref_x[-1] - # y = self.ref_y[-1] - # z = 1.5 - # yaw = 0. - # duration = 2.5 - - # command_type = Command(5) # goTo. - # args = [[x, y, z], yaw, duration, False] - - # elif iteration == 23*self.CTRL_FREQ: - # x = self.initial_obs[0] - # y = self.initial_obs[2] - # z = 1.5 - # yaw = 0. - # duration = 6 - - # command_type = Command(5) # goTo. - # args = [[x, y, z], yaw, duration, False] + elif iteration == (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ: + command_type = Command(6) # notify setpoint stop. + args = [] - # elif iteration == 30*self.CTRL_FREQ: - # height = 0. - # duration = 3 + elif iteration == (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 1: + height = 0. + duration = 2 - # command_type = Command(3) # Land. - # args = [height, duration] + command_type = Command(3) # Land. + args = [height, duration] - # elif iteration == 33*self.CTRL_FREQ-1: - # command_type = Command(-1) # Terminate command to be sent once trajectory is completed. - # args = [] + elif iteration == (2*self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 1: + command_type = Command(-1) # Terminate command to be sent once trajectory is completed. + args = [] else: command_type = Command(0) # None. @@ -637,6 +606,8 @@ def interEpisodeLearn(self): _ = self.done_buffer _ = self.info_buffer + self.scaled_t = 0 + ######################### # REPLACE THIS (END) #### ######################### From 73fc7bc369fa39c5ddd6f211122c499f720bf636 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:25:50 +0800 Subject: [PATCH 08/30] TIme scaling function --- competition/edit_this.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 8d710ea17..cca44706f 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -27,7 +27,7 @@ import numpy as np import scipy.interpolate import scipy.integrate -from math import sqrt, sin, cos, atan2, pi +from math import sqrt, sin, cos, atan2, pi, log1p from collections import deque @@ -434,7 +434,11 @@ def cmdFirmware(self, y_c = self.y(curve_t) z_c = self.z(curve_t) target_pos = np.array([x_c, y_c, z_c]) - # print(obs[0] - x_c, obs[2] - y_c, obs[4] - z_c) + error = np.array((obs[0] - x_c, obs[2] - y_c, obs[4] - z_c)) # positional error (world frame) + # print(error) + self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.025 + self.time_scale = min(1.0, self.time_scale) + # print(self.time_scale) dx_c = self.dx(curve_t) dy_c = self.dy(curve_t) From 2ecf8af0abba1f30e502b7a7da0b1da5f997f74c Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:26:37 +0800 Subject: [PATCH 09/30] Commented out shifting of goal --- competition/edit_this.py | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index cca44706f..85ce890e8 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -174,23 +174,23 @@ def __init__(self, # for right then left > shift gate to right # for left then right > shift gate to left # For consecutive turns in same direction, distance shifted is larger - shift_dist = .1 - yaw_prev = waypoints[0][3] - yaw_curr = waypoints[1][3] - dyaw = yaw_curr - yaw_prev - for idx in range(1,length-1): - yaw_next = waypoints[idx+1][3] - dyaw_next = yaw_next - yaw_curr - scale = (((abs(dyaw) + abs(dyaw_next)) / 2. / pi + 1.) % 2.) - 1. - if dyaw > 0: - scale = -scale - if dyaw * dyaw_next < 0: - scale = -scale - waypoints[idx][0] -= sin(yaw_curr) * shift_dist * scale - waypoints[idx][1] -= cos(yaw_curr) * shift_dist * scale - yaw_prev = yaw_curr - yaw_curr = yaw_next - dyaw = dyaw_next + # shift_dist = .2 + # yaw_prev = waypoints[0][3] + # yaw_curr = waypoints[1][3] + # dyaw = yaw_curr - yaw_prev + # for idx in range(1,length-1): + # yaw_next = waypoints[idx+1][3] + # dyaw_next = yaw_next - yaw_curr + # scale = (((dyaw + dyaw_next) / 2. / pi + 1.) % 2.) - 1. + # if dyaw > 0: + # scale = -scale + # if dyaw * dyaw_next < 0: + # scale = -scale + # waypoints[idx][0] -= sin(yaw_curr) * shift_dist * scale + # waypoints[idx][1] -= cos(yaw_curr) * shift_dist * scale + # yaw_prev = yaw_curr + # yaw_curr = yaw_next + # dyaw = dyaw_next [x0, y0, _, yaw0] = waypoints[0] dx0 = cos(yaw0) From b6c25c45fe52e761e5802853970518cfaf3735f9 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:27:26 +0800 Subject: [PATCH 10/30] Reduce tangent magnitude for start and end --- competition/edit_this.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/competition/edit_this.py b/competition/edit_this.py index 85ce890e8..e4ad1a820 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -202,6 +202,9 @@ def __init__(self, inv_t2 = inv_t*inv_t dxf = cos(yawf) * grad_scale dyf = sin(yawf) * grad_scale + if idx == 0 or idx == length-1: # don't need to care about curving out of first and into last goal + dxf *= 0.01 + dyf *= 0.01 dx = xf - x0 dy = yf - y0 x_a0 = x0 From 7ec9089fb9ffd022cbaf9e298670b8c7b3bb6ea8 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Wed, 5 Oct 2022 21:27:51 +0800 Subject: [PATCH 11/30] Tuned kinematic limits --- competition/edit_this.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index e4ad1a820..277aad00b 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -99,9 +99,9 @@ def __init__(self, # Kinematic limits # TODO determine better estimates from model - v_max = 2 - a_max = .2 - j_max = .2 + v_max = 1.5 + a_max = 2 + j_max = 2 # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve grad_scale = 1 From 86bfa7658d72d0761e694289c90a5cdd1b858531 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Thu, 6 Oct 2022 00:15:02 +0800 Subject: [PATCH 12/30] Removed extraneous land command --- competition/edit_this.py | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 277aad00b..1c76d6000 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -420,14 +420,9 @@ def cmdFirmware(self, command_type = Command(2) # Take-off. args = [height, duration] - elif iteration >= 2*self.CTRL_FREQ and iteration < (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ: + elif iteration >= self.takeoff_land_duration*self.CTRL_FREQ and iteration < (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ: self.scaled_t += self.time_scale / self.CTRL_FREQ / self.scaling_factor - if self.scaled_t > self.end_t: - height = 0. - duration = 3 - command_type = Command(3) # Land. - args = [height, duration] [curve_t, curve_v, curve_a, curve_j] = self.s(self.scaled_t) curve_t *= self.scaling_factor curve_v *= self.scaling_factor @@ -492,12 +487,12 @@ def cmdFirmware(self, elif iteration == (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 1: height = 0. - duration = 2 + duration = self.takeoff_land_duration command_type = Command(3) # Land. args = [height, duration] - elif iteration == (2*self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 1: + elif iteration == (2*self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 2: command_type = Command(-1) # Terminate command to be sent once trajectory is completed. args = [] From 36fb43126683a8734dcbd8adb7bf7ba590c42ff9 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Fri, 7 Oct 2022 01:39:44 +0800 Subject: [PATCH 13/30] Quintic polynomial curve fitting for x&y --- competition/edit_this.py | 91 +++++++++++++++++++++++++++------------- 1 file changed, 62 insertions(+), 29 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 1c76d6000..06762ac03 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -100,10 +100,10 @@ def __init__(self, # Kinematic limits # TODO determine better estimates from model v_max = 1.5 - a_max = 2 - j_max = 2 + a_max = .5 + j_max = .5 # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve - grad_scale = 1 + grad_scale = .8 ### Spline fitting between waypoints ### @@ -123,9 +123,6 @@ def __init__(self, # end goal waypoints.append([initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4], initial_info["x_reference"][8]]) - x_coeffs = np.zeros((4,len(waypoints)-1)) - y_coeffs = np.zeros((4,len(waypoints)-1)) - # "time" for each waypoint # time interval determined by eulcidean distance between waypoints along xy plane # ts = np.arange(length) @@ -192,14 +189,23 @@ def __init__(self, # yaw_curr = yaw_next # dyaw = dyaw_next + x_coeffs = np.zeros((6,len(waypoints)-1)) + y_coeffs = np.zeros((6,len(waypoints)-1)) [x0, y0, _, yaw0] = waypoints[0] dx0 = cos(yaw0) dy0 = sin(yaw0) + d2x0 = 0 + d2y0 = 0 + d2xf = 0 + d2yf = 0 for idx in range(1, length): [xf, yf, _, yawf] = waypoints[idx] dt = ts[idx] - ts[idx - 1] inv_t = 1/dt inv_t2 = inv_t*inv_t + inv_t3 = inv_t2*inv_t + inv_t4 = inv_t2*inv_t2 + inv_t5 = inv_t2*inv_t3 dxf = cos(yawf) * grad_scale dyf = sin(yawf) * grad_scale if idx == 0 or idx == length-1: # don't need to care about curving out of first and into last goal @@ -209,34 +215,35 @@ def __init__(self, dy = yf - y0 x_a0 = x0 x_a1 = dx0 - x_a2 = (3*dx*inv_t - 2*dx0 - dxf)*inv_t - x_a3 = (-2*dx*inv_t + (dxf + dx0))*inv_t2 - x_coeffs[:,idx-1] = (x_a3, x_a2, x_a1, x_a0) + x_a2 = d2x0*0.5 + x_a3 = 10*dx*inv_t3 - (4*dxf+6*dx0)*inv_t2 - 0.5*(3*d2x0-d2xf)*inv_t + x_a4 = -15*dx*inv_t4 + (7*dxf+8*dx0)*inv_t3 + 0.5*(3*d2x0-2*d2xf)*inv_t2 + x_a5 = 6*dx*inv_t5 - 3*(dxf+dx0)*inv_t4 - 0.5*(d2x0-d2xf)*inv_t3 + x_coeffs[:,idx-1] = (x_a5, x_a4, x_a3, x_a2, x_a1, x_a0) y_a0 = y0 y_a1 = dy0 - y_a2 = (3*dy*inv_t - 2*dy0 - dyf)*inv_t - y_a3 = (-2*dy*inv_t + (dyf + dy0))*inv_t2 - y_coeffs[:,idx-1] = (y_a3, y_a2, y_a1, y_a0) + y_a2 = d2y0*0.5 + y_a3 = 10*dy*inv_t3 - (4*dyf+6*dy0)*inv_t2 - 0.5*(3*d2y0-d2yf)*inv_t + y_a4 = -15*dy*inv_t4 + (7*dyf+8*dy0)*inv_t3 + 0.5*(3*d2y0-2*d2yf)*inv_t2 + y_a5 = 6*dy*inv_t5 - 3*(dyf+dy0)*inv_t4 - 0.5*(d2y0-d2yf)*inv_t3 + y_coeffs[:,idx-1] = (y_a5, y_a4, y_a3, y_a2, y_a1, y_a0) [x0, y0] = [xf, yf] [dx0, dy0] = [dxf, dyf] + # [d2x0, d2y0] = [d2xf, d2yf] waypoints = np.array(waypoints) z_coeffs = scipy.interpolate.PchipInterpolator(ts, waypoints[:,2], 0).c - def df_idx(idx): - def df(t): - dx = (3*x_coeffs[1,idx]*t + 2*x_coeffs[2,idx])*t + x_coeffs[3,idx] - dy = (3*y_coeffs[1,idx]*t + 2*y_coeffs[2,idx])*t + y_coeffs[3,idx] - dz = (3*z_coeffs[1,idx]*t + 2*z_coeffs[2,idx])*t + z_coeffs[3,idx] - return sqrt(dx*dx+dy*dy+dz*dz) - return df - def f_(coeffs): def f(t): for idx in range(length-1): if (ts[idx+1] > t): break t -= ts[idx] - return ((coeffs[0,idx]*t + coeffs[1,idx])*t + coeffs[2,idx])*t + coeffs[3,idx] + val = 0 + for coeff in coeffs[:,idx]: + val *= t + val += coeff + return val return f def df_(coeffs): @@ -245,7 +252,13 @@ def f(t): if (ts[idx+1] > t): break t -= ts[idx] - return (3*coeffs[0,idx]*t + 2*coeffs[1,idx])*t + coeffs[2,idx] + coeffs_no = len(coeffs[:,idx]) + val = 0 + for i, coeff in enumerate(coeffs[:-1,idx]): + val *= t + factor = (coeffs_no - i - 1) + val += coeff * factor + return val return f def d2f_(coeffs): @@ -254,7 +267,13 @@ def f(t): if (ts[idx+1] > t): break t -= ts[idx] - return 6*coeffs[0,idx]*t + 2*coeffs[1,idx] + coeffs_no = len(coeffs[:,idx]) + val = 0 + for i, coeff in enumerate(coeffs[:-2,idx]): + val *= t + factor = (coeffs_no - i - 1) + val += coeff * factor * (factor - 1) + return val return f def d3f_(coeffs): @@ -263,14 +282,22 @@ def f(t): if (ts[idx+1] > t): break t -= ts[idx] - return 6*coeffs[0,idx] + coeffs_no = len(coeffs[:,idx]) + val = 0 + for i, coeff in enumerate(coeffs[:-3,idx]): + val *= t + factor = (coeffs_no - i - 1) + val += coeff * factor * (factor - 1) * (factor - 2) + return val return f - # Integrate to get pathlength - pathlength = 0 - for idx in range(length-1): - pathlength += scipy.integrate.quad(df_idx(idx), 0, ts[idx+1] - ts[idx])[0] - self.scaling_factor = ts[-1] / pathlength + def df_idx(idx): + def df(t): + dx = self.dx(t) + dy = self.dy(t) + dz = self.dz(t) + return sqrt(dx*dx+dy*dy+dz*dz) + return df self.x = f_(x_coeffs) self.y = f_(y_coeffs) @@ -288,6 +315,12 @@ def f(t): self.d3y = d3f_(y_coeffs) self.d3z = d3f_(z_coeffs) + # Integrate to get pathlength + pathlength = 0 + for idx in range(length-1): + pathlength += scipy.integrate.quad(df_idx(idx), 0, ts[idx+1] - ts[idx])[0] + self.scaling_factor = ts[-1] / pathlength + duration = ts[-1] - ts[0] t_scaled = np.linspace(ts[0], ts[-1], int(duration*self.CTRL_FREQ)) x_scaled = np.array(tuple(map(self.x, t_scaled))) From 2907a51604469edbd745e92811346f2c5a538753 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Fri, 7 Oct 2022 19:57:41 +0800 Subject: [PATCH 14/30] Streamline function calls for spline --- competition/edit_this.py | 237 ++++++++++++++++++++------------------- 1 file changed, 120 insertions(+), 117 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 06762ac03..940293f44 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -102,48 +102,51 @@ def __init__(self, v_max = 1.5 a_max = .5 j_max = .5 + self.mass = initial_info['nominal_physical_parameters']['quadrotor_mass'] # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve - grad_scale = .8 + self.grad_scale = .8 + + self.tall = initial_info["gate_dimensions"]["tall"]["height"] + self.low = initial_info["gate_dimensions"]["low"]["height"] ### Spline fitting between waypoints ### - length = len(self.NOMINAL_GATES) + 2 + self.length = len(self.NOMINAL_GATES) + 2 # end goal if use_firmware: waypoints = [[self.initial_obs[0], self.initial_obs[2], initial_info["gate_dimensions"]["tall"]["height"], self.initial_obs[8]],] # Height is hardcoded scenario knowledge. else: waypoints = [[self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8]],] - half_pi = 0.5*pi + self.half_pi = 0.5*pi for idx, g in enumerate(self.NOMINAL_GATES): [x, y, _, _, _, yaw, typ] = g - yaw += half_pi - z = initial_info["gate_dimensions"]["tall"]["height"] if typ == 0 else initial_info["gate_dimensions"]["low"]["height"] + yaw += self.half_pi + z = self.tall if typ == 0 else self.low waypoints.append([x, y, z, yaw]) # end goal waypoints.append([initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4], initial_info["x_reference"][8]]) # "time" for each waypoint # time interval determined by eulcidean distance between waypoints along xy plane - # ts = np.arange(length) - ts = np.zeros(length) + self.ts = np.zeros(self.length) [x_prev, y_prev, _, _] = waypoints[0] - for idx in range(1,length): + for idx in range(1,self.length): [x_curr, y_curr, _, _] = waypoints[idx] xy_norm = sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) - ts[idx] = ts[idx-1] + xy_norm / v_max + self.ts[idx] = self.ts[idx-1] + xy_norm / v_max # Flip gates # Selectively flip gate orientation based on vector from previous and to next waypoint, # and current gate's orientation [x_prev, y_prev, _, _] = waypoints[0] [x_curr, y_curr, _, yaw_curr] = waypoints[1] - dt = ts[1] - ts[0] - for idx in range(1,length-1): + dt = self.ts[1] - self.ts[0] + for idx in range(1,self.length-1): [x_next, y_next, _, yaw_next] = waypoints[idx+1] - dt_next = ts[idx+1] - ts[idx] - dxf = cos(yaw_curr) * grad_scale - dyf = sin(yaw_curr) * grad_scale + dt_next = self.ts[idx+1] - self.ts[idx] + dxf = cos(yaw_curr) * self.grad_scale + dyf = sin(yaw_curr) * self.grad_scale vx1 = np.array((x_curr - x_prev,dt)) vx21 = np.array((dxf,1)) vx22 = np.array((-dxf,1)) @@ -175,7 +178,7 @@ def __init__(self, # yaw_prev = waypoints[0][3] # yaw_curr = waypoints[1][3] # dyaw = yaw_curr - yaw_prev - # for idx in range(1,length-1): + # for idx in range(1,self.length-1): # yaw_next = waypoints[idx+1][3] # dyaw_next = yaw_next - yaw_curr # scale = (((dyaw + dyaw_next) / 2. / pi + 1.) % 2.) - 1. @@ -189,8 +192,22 @@ def __init__(self, # yaw_curr = yaw_next # dyaw = dyaw_next - x_coeffs = np.zeros((6,len(waypoints)-1)) - y_coeffs = np.zeros((6,len(waypoints)-1)) + self.cubic_interp = lambda inv_t, x0, xf, dx0, dxf:( + x0, + dx0, + (3*(xf-x0)*inv_t - 2*dx0 - dxf)*inv_t, + (-2*(xf-x0)*inv_t + (dxf + dx0))*inv_t*inv_t) + + self.quintic_interp = lambda inv_t, x0, xf, dx0, dxf, d2x0, d2xf:( + x0, + dx0, + d2x0*0.5, + ((10*(xf-x0)*inv_t - (4*dxf+6*dx0))*inv_t - 0.5*(3*d2x0-d2xf))*inv_t, + ((-15*(xf-x0)*inv_t + (7*dxf+8*dx0))*inv_t + 0.5*(3*d2x0-2*d2xf))*inv_t*inv_t, + ((6*(xf-x0)*inv_t - 3*(dxf+dx0))*inv_t - 0.5*(d2x0-d2xf))*inv_t*inv_t*inv_t) + + self.x_coeffs = np.zeros((6,len(waypoints)-1)) + self.y_coeffs = np.zeros((6,len(waypoints)-1)) [x0, y0, _, yaw0] = waypoints[0] dx0 = cos(yaw0) dy0 = sin(yaw0) @@ -198,47 +215,25 @@ def __init__(self, d2y0 = 0 d2xf = 0 d2yf = 0 - for idx in range(1, length): + for idx in range(1, self.length): [xf, yf, _, yawf] = waypoints[idx] - dt = ts[idx] - ts[idx - 1] + dt = self.ts[idx] - self.ts[idx - 1] inv_t = 1/dt - inv_t2 = inv_t*inv_t - inv_t3 = inv_t2*inv_t - inv_t4 = inv_t2*inv_t2 - inv_t5 = inv_t2*inv_t3 - dxf = cos(yawf) * grad_scale - dyf = sin(yawf) * grad_scale - if idx == 0 or idx == length-1: # don't need to care about curving out of first and into last goal + dxf = cos(yawf) * self.grad_scale + dyf = sin(yawf) * self.grad_scale + if idx == 0 or idx == self.length-1: # don't need to care about curving out of first and into last goal dxf *= 0.01 dyf *= 0.01 - dx = xf - x0 - dy = yf - y0 - x_a0 = x0 - x_a1 = dx0 - x_a2 = d2x0*0.5 - x_a3 = 10*dx*inv_t3 - (4*dxf+6*dx0)*inv_t2 - 0.5*(3*d2x0-d2xf)*inv_t - x_a4 = -15*dx*inv_t4 + (7*dxf+8*dx0)*inv_t3 + 0.5*(3*d2x0-2*d2xf)*inv_t2 - x_a5 = 6*dx*inv_t5 - 3*(dxf+dx0)*inv_t4 - 0.5*(d2x0-d2xf)*inv_t3 - x_coeffs[:,idx-1] = (x_a5, x_a4, x_a3, x_a2, x_a1, x_a0) - y_a0 = y0 - y_a1 = dy0 - y_a2 = d2y0*0.5 - y_a3 = 10*dy*inv_t3 - (4*dyf+6*dy0)*inv_t2 - 0.5*(3*d2y0-d2yf)*inv_t - y_a4 = -15*dy*inv_t4 + (7*dyf+8*dy0)*inv_t3 + 0.5*(3*d2y0-2*d2yf)*inv_t2 - y_a5 = 6*dy*inv_t5 - 3*(dyf+dy0)*inv_t4 - 0.5*(d2y0-d2yf)*inv_t3 - y_coeffs[:,idx-1] = (y_a5, y_a4, y_a3, y_a2, y_a1, y_a0) + self.x_coeffs[::-1,idx-1] = self.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) + self.y_coeffs[::-1,idx-1] = self.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) [x0, y0] = [xf, yf] [dx0, dy0] = [dxf, dyf] # [d2x0, d2y0] = [d2xf, d2yf] waypoints = np.array(waypoints) - z_coeffs = scipy.interpolate.PchipInterpolator(ts, waypoints[:,2], 0).c + self.z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c def f_(coeffs): - def f(t): - for idx in range(length-1): - if (ts[idx+1] > t): - break - t -= ts[idx] + def f(idx, t): val = 0 for coeff in coeffs[:,idx]: val *= t @@ -247,11 +242,7 @@ def f(t): return f def df_(coeffs): - def f(t): - for idx in range(length-1): - if (ts[idx+1] > t): - break - t -= ts[idx] + def f(idx, t): coeffs_no = len(coeffs[:,idx]) val = 0 for i, coeff in enumerate(coeffs[:-1,idx]): @@ -262,11 +253,7 @@ def f(t): return f def d2f_(coeffs): - def f(t): - for idx in range(length-1): - if (ts[idx+1] > t): - break - t -= ts[idx] + def f(idx, t): coeffs_no = len(coeffs[:,idx]) val = 0 for i, coeff in enumerate(coeffs[:-2,idx]): @@ -277,11 +264,7 @@ def f(t): return f def d3f_(coeffs): - def f(t): - for idx in range(length-1): - if (ts[idx+1] > t): - break - t -= ts[idx] + def f(idx, t): coeffs_no = len(coeffs[:,idx]) val = 0 for i, coeff in enumerate(coeffs[:-3,idx]): @@ -291,48 +274,50 @@ def f(t): return val return f - def df_idx(idx): + def df_idx(): def df(t): - dx = self.dx(t) - dy = self.dy(t) - dz = self.dz(t) + for idx in range(self.length-1): + if (self.ts[idx+1] > t): + break + t -= self.ts[idx] + dx = self.df(self.x_coeffs)(idx, t) + dy = self.df(self.y_coeffs)(idx, t) + dz = self.df(self.z_coeffs)(idx, t) return sqrt(dx*dx+dy*dy+dz*dz) return df - self.x = f_(x_coeffs) - self.y = f_(y_coeffs) - self.z = f_(z_coeffs) - - self.dx = df_(x_coeffs) - self.dy = df_(y_coeffs) - self.dz = df_(z_coeffs) - - self.d2x = d2f_(x_coeffs) - self.d2y = d2f_(y_coeffs) - self.d2z = d2f_(z_coeffs) - - self.d3x = d3f_(x_coeffs) - self.d3y = d3f_(y_coeffs) - self.d3z = d3f_(z_coeffs) + self.f = f_ + self.df = df_ + self.d2f = d2f_ + self.d3f = d3f_ # Integrate to get pathlength pathlength = 0 - for idx in range(length-1): - pathlength += scipy.integrate.quad(df_idx(idx), 0, ts[idx+1] - ts[idx])[0] - self.scaling_factor = ts[-1] / pathlength - - duration = ts[-1] - ts[0] - t_scaled = np.linspace(ts[0], ts[-1], int(duration*self.CTRL_FREQ)) - x_scaled = np.array(tuple(map(self.x, t_scaled))) - y_scaled = np.array(tuple(map(self.y, t_scaled))) - z_scaled = np.array(tuple(map(self.z, t_scaled))) + for idx in range(self.length-1): + pathlength += scipy.integrate.quad(df_idx(), 0, self.ts[idx+1] - self.ts[idx])[0] + self.scaling_factor = self.ts[-1] / pathlength if self.VERBOSE: - print(x_coeffs) - print(y_coeffs) - print(z_coeffs) + duration = self.ts[-1] - self.ts[0] + t_scaled = np.linspace(self.ts[0], self.ts[-1], int(duration*self.CTRL_FREQ)) + t_diff_scaled = np.zeros(t_scaled.shape) + gate_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) + gate = 0 + for i, t in enumerate(t_scaled): + if gate < self.length and t > self.ts[gate+1]: + gate += 1 + t_diff_scaled[i] = t_scaled[i] - self.ts[gate] + gate_scaled[i] = gate + x_scaled = np.array(tuple(map(self.f(self.x_coeffs), gate_scaled, t_diff_scaled))) + y_scaled = np.array(tuple(map(self.f(self.y_coeffs), gate_scaled, t_diff_scaled))) + z_scaled = np.array(tuple(map(self.f(self.z_coeffs), gate_scaled, t_diff_scaled))) + print(self.x_coeffs) + print(self.y_coeffs) + print(self.z_coeffs) print(waypoints) print(t_scaled) + print(t_diff_scaled) + print(gate_scaled) print(x_scaled) print(y_scaled) print(z_scaled) @@ -406,6 +391,11 @@ def rpy2rot(roll, pitch, yaw): return m self.rpy2rot = rpy2rot + self.gate_no = 0 + self.run_x_coeffs = np.copy(self.x_coeffs) + self.run_y_coeffs = np.copy(self.y_coeffs) + self.run_z_coeffs = np.copy(self.z_coeffs) + self.run_ts = np.copy(self.ts) ######################### # REPLACE THIS (END) #### @@ -453,7 +443,7 @@ def cmdFirmware(self, command_type = Command(2) # Take-off. args = [height, duration] - elif iteration >= self.takeoff_land_duration*self.CTRL_FREQ and iteration < (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ: + elif iteration >= self.takeoff_land_duration*self.CTRL_FREQ and self.scaled_t < self.end_t: self.scaled_t += self.time_scale / self.CTRL_FREQ / self.scaling_factor [curve_t, curve_v, curve_a, curve_j] = self.s(self.scaled_t) @@ -461,30 +451,37 @@ def cmdFirmware(self, curve_v *= self.scaling_factor curve_a *= self.scaling_factor curve_j *= self.scaling_factor - x_c = self.x(curve_t) - y_c = self.y(curve_t) - z_c = self.z(curve_t) - target_pos = np.array([x_c, y_c, z_c]) - error = np.array((obs[0] - x_c, obs[2] - y_c, obs[4] - z_c)) # positional error (world frame) + self.curve_t = curve_t + + if (self.gate_no < self.length-2 and self.run_ts[self.gate_no+1] <= curve_t): + self.gate_no += 1 + + t = curve_t - self.run_ts[self.gate_no] + + self.x_c = self.f(self.run_x_coeffs)(self.gate_no, t) + self.y_c = self.f(self.run_y_coeffs)(self.gate_no, t) + self.z_c = self.f(self.run_z_coeffs)(self.gate_no, t) + target_pos = np.array([self.x_c, self.y_c, self.z_c]) + error = np.array((obs[0] - self.x_c, obs[2] - self.y_c, obs[4] - self.z_c)) # positional error (world frame) # print(error) self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.025 self.time_scale = min(1.0, self.time_scale) # print(self.time_scale) - dx_c = self.dx(curve_t) - dy_c = self.dy(curve_t) - dz_c = self.dz(curve_t) - d2x_c = self.d2x(curve_t) - d2y_c = self.d2y(curve_t) - d2z_c = self.d2z(curve_t) - d3x_c = self.d3x(curve_t) - d3y_c = self.d3y(curve_t) - d3z_c = self.d3z(curve_t) - tangent = np.array((dx_c,dy_c,dz_c)) - dtangent = np.array((d2x_c,d2y_c,d2z_c)) - d2tangent = np.array((d3x_c,d3y_c,d3z_c)) - - target_yaw = atan2(dy_c, dx_c) + self.dx_c = self.df(self.run_x_coeffs)(self.gate_no, t) + self.dy_c = self.df(self.run_y_coeffs)(self.gate_no, t) + self.dz_c = self.df(self.run_z_coeffs)(self.gate_no, t) + self.d2x_c = self.d2f(self.run_x_coeffs)(self.gate_no, t) + self.d2y_c = self.d2f(self.run_y_coeffs)(self.gate_no, t) + self.d2z_c = self.d2f(self.run_z_coeffs)(self.gate_no, t) + self.d3x_c = self.d3f(self.run_x_coeffs)(self.gate_no, t) + self.d3y_c = self.d3f(self.run_y_coeffs)(self.gate_no, t) + self.d3z_c = self.d3f(self.run_z_coeffs)(self.gate_no, t) + tangent = np.array((self.dx_c,self.dy_c,self.dz_c)) + dtangent = np.array((self.d2x_c,self.d2y_c,self.d2z_c)) + d2tangent = np.array((self.d3x_c,self.d3y_c,self.d3z_c)) + + target_yaw = atan2(self.dy_c, self.dx_c) target_vel = tangent * curve_v target_acc = tangent * curve_a + dtangent * curve_v**2 @@ -493,15 +490,15 @@ def cmdFirmware(self, target_jerk = tangent * curve_j + d2tangent * curve_v**3 + 3 * dtangent * curve_v * curve_a Jinv = self.rpy2rot(obs[6], obs[7], obs[8]).transpose() body_jerk = np.matmul(Jinv, target_jerk.transpose()) - p = 0.027/9.8*body_jerk[0,1] # roll rate = mass / g * y_jerk - q = 0.027/9.8*body_jerk[0,0] # pitch rate = mass / g * x_jerk + p = self.mass/9.8*body_jerk[0,1] # roll rate = mass / g * y_jerk + q = self.mass/9.8*body_jerk[0,0] # pitch rate = mass / g * x_jerk # Yaw rate den = np.linalg.norm(tangent[:2]) if den < 1e-9: r = 0 else: - num = dx_c * d2y_c - dy_c * d2x_c + num = self.dx_c * self.d2y_c - self.dy_c * self.d2x_c r = num/den r *= curve_v @@ -642,6 +639,12 @@ def interEpisodeLearn(self): _ = self.info_buffer self.scaled_t = 0 + self.gate_no = 0 + self.run_x_coeffs = np.copy(self.x_coeffs) + self.run_y_coeffs = np.copy(self.y_coeffs) + self.run_z_coeffs = np.copy(self.z_coeffs) + self.run_ts = np.copy(self.ts) + self.length = len(self.NOMINAL_GATES) + 2 ######################### # REPLACE THIS (END) #### From 2dc05c480ca8cc5246d60825c408c185d7c22e64 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Fri, 7 Oct 2022 19:58:18 +0800 Subject: [PATCH 15/30] Local planning when actual gate pose known --- competition/edit_this.py | 72 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 71 insertions(+), 1 deletion(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 940293f44..d2d1ba2ad 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -390,7 +390,7 @@ def rpy2rot(roll, pitch, yaw): (-sp , cp*sr , cp*cr))) return m self.rpy2rot = rpy2rot - + self.at_gate = False self.gate_no = 0 self.run_x_coeffs = np.copy(self.x_coeffs) self.run_y_coeffs = np.copy(self.y_coeffs) @@ -444,6 +444,75 @@ def cmdFirmware(self, command_type = Command(2) # Take-off. args = [height, duration] elif iteration >= self.takeoff_land_duration*self.CTRL_FREQ and self.scaled_t < self.end_t: + if info['current_target_gate_in_range']: + if not self.at_gate: + # Local replan when near to goal + self.at_gate = True + [x, y, z, _, _, yaw] = info['current_target_gate_pos'] + yaw += self.half_pi + dt = self.run_ts[self.gate_no + 1] - self.curve_t + inv_t = 1/dt + dx_gate = cos(yaw) * self.grad_scale + dy_gate = sin(yaw) * self.grad_scale + self.run_x_coeffs = np.insert(self.run_x_coeffs, self.gate_no+1, np.flip(self.quintic_interp( + inv_t, + self.x_c, + x, + self.dx_c, + dx_gate, + self.d2x_c, + 0 + )), axis=1) + self.run_y_coeffs = np.insert(self.run_y_coeffs, self.gate_no+1, np.flip(self.quintic_interp( + inv_t, + self.y_c, + y, + self.dy_c, + dy_gate, + self.d2y_c, + 0 + )), axis=1) + self.run_z_coeffs = np.insert(self.run_z_coeffs, self.gate_no+1, np.flip(self.cubic_interp( + inv_t, + self.z_c, + z, + self.dz_c, + self.df(self.run_z_coeffs)(self.gate_no, dt) + )), axis=1) + dt = self.run_ts[self.gate_no + 2] - self.run_ts[self.gate_no + 1] + inv_t = 1/dt + self.run_x_coeffs[::-1,self.gate_no+2] = self.quintic_interp( + inv_t, + x, + self.f(self.run_x_coeffs)(self.gate_no+2, dt), + dx_gate, + self.df(self.run_x_coeffs)(self.gate_no+2, dt), + 0, + 0 + ) + self.run_y_coeffs[::-1,self.gate_no+2] = self.quintic_interp( + inv_t, + y, + self.f(self.run_y_coeffs)(self.gate_no+2, dt), + dy_gate, + self.df(self.run_y_coeffs)(self.gate_no+2, dt), + 0, + 0 + ) + self.run_z_coeffs[::-1,self.gate_no+2] = self.cubic_interp( + inv_t, + z, + self.f(self.run_z_coeffs)(self.gate_no+2, dt), + self.run_z_coeffs[-2, self.gate_no+2], + self.df(self.run_z_coeffs)(self.gate_no+2, dt) + ) + self.run_ts = np.insert(self.run_ts, self.gate_no+1, self.curve_t) + self.length += 1 + self.gate_no += 1 + # print(np.insert(self.run_ts, self.gate_no+1, self.curve_t)) + else: + self.at_gate = False + self.scaled_t += self.time_scale / self.CTRL_FREQ / self.scaling_factor [curve_t, curve_v, curve_a, curve_j] = self.s(self.scaled_t) @@ -639,6 +708,7 @@ def interEpisodeLearn(self): _ = self.info_buffer self.scaled_t = 0 + self.at_gate = False self.gate_no = 0 self.run_x_coeffs = np.copy(self.x_coeffs) self.run_y_coeffs = np.copy(self.y_coeffs) From 12398f83e0fcc39256c5131e8174d8a64fa4d7b6 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Fri, 7 Oct 2022 19:58:40 +0800 Subject: [PATCH 16/30] Time scaling for vel, acc, jerk --- competition/edit_this.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index d2d1ba2ad..820ea1a46 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -517,9 +517,9 @@ def cmdFirmware(self, [curve_t, curve_v, curve_a, curve_j] = self.s(self.scaled_t) curve_t *= self.scaling_factor - curve_v *= self.scaling_factor - curve_a *= self.scaling_factor - curve_j *= self.scaling_factor + curve_v *= self.scaling_factor * self.time_scale + curve_a *= self.scaling_factor * self.time_scale + curve_j *= self.scaling_factor * self.time_scale self.curve_t = curve_t if (self.gate_no < self.length-2 and self.run_ts[self.gate_no+1] <= curve_t): From b3aef4b9c715fe7d06d86919caf49cfeb6b33280 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sat, 8 Oct 2022 14:04:36 +0800 Subject: [PATCH 17/30] Modifications for dealing with final endpoint --- competition/edit_this.py | 63 ++++++++++++++++------------------------ 1 file changed, 25 insertions(+), 38 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 820ea1a46..42dc2c0c0 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -100,11 +100,11 @@ def __init__(self, # Kinematic limits # TODO determine better estimates from model v_max = 1.5 - a_max = .5 - j_max = .5 + a_max = .2 + j_max = 1 self.mass = initial_info['nominal_physical_parameters']['quadrotor_mass'] # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve - self.grad_scale = .8 + self.grad_scale = .6 self.tall = initial_info["gate_dimensions"]["tall"]["height"] self.low = initial_info["gate_dimensions"]["low"]["height"] @@ -113,10 +113,7 @@ def __init__(self, self.length = len(self.NOMINAL_GATES) + 2 # end goal - if use_firmware: - waypoints = [[self.initial_obs[0], self.initial_obs[2], initial_info["gate_dimensions"]["tall"]["height"], self.initial_obs[8]],] # Height is hardcoded scenario knowledge. - else: - waypoints = [[self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8]],] + waypoints = [[self.initial_obs[0], self.initial_obs[2], self.initial_obs[4], self.initial_obs[8]],] self.half_pi = 0.5*pi for idx, g in enumerate(self.NOMINAL_GATES): @@ -221,9 +218,12 @@ def __init__(self, inv_t = 1/dt dxf = cos(yawf) * self.grad_scale dyf = sin(yawf) * self.grad_scale - if idx == 0 or idx == self.length-1: # don't need to care about curving out of first and into last goal + if idx == 0: # don't need to care about curving out of first goal dxf *= 0.01 dyf *= 0.01 + if idx == self.length-1: # yaw of last goal not important + dxf = dx0 * 0.01 + dyf = dy0 * 0.01 self.x_coeffs[::-1,idx-1] = self.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) self.y_coeffs[::-1,idx-1] = self.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) [x0, y0] = [xf, yf] @@ -232,6 +232,14 @@ def __init__(self, waypoints = np.array(waypoints) self.z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c + # modify endpoint gradient for smooth z ending + self.z_coeffs[::-1,-1] = self.cubic_interp(1/(self.ts[-1]-self.ts[-2]), + waypoints[-2,2], + waypoints[-1,2], + self.z_coeffs[-2,-1], + 0 + ) + def f_(coeffs): def f(idx, t): val = 0 @@ -437,14 +445,8 @@ def cmdFirmware(self, ######################### # # Handwritten solution for GitHub's getting_stated scenario. - if iteration == 0: - height = 1 - duration = self.takeoff_land_duration - - command_type = Command(2) # Take-off. - args = [height, duration] - elif iteration >= self.takeoff_land_duration*self.CTRL_FREQ and self.scaled_t < self.end_t: - if info['current_target_gate_in_range']: + if self.scaled_t < self.end_t: + if iteration > 0 and info['current_target_gate_in_range']: if not self.at_gate: # Local replan when near to goal self.at_gate = True @@ -533,7 +535,7 @@ def cmdFirmware(self, target_pos = np.array([self.x_c, self.y_c, self.z_c]) error = np.array((obs[0] - self.x_c, obs[2] - self.y_c, obs[4] - self.z_c)) # positional error (world frame) # print(error) - self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.025 + self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.05 self.time_scale = min(1.0, self.time_scale) # print(self.time_scale) @@ -576,28 +578,13 @@ def cmdFirmware(self, # target_acc = np.zeros(3) # target_yaw = 0. # target_rpy_rates = np.zeros(3) - - command_type = Command(1) # cmdFullState. - args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] - - elif iteration == (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ: - command_type = Command(6) # notify setpoint stop. - args = [] - - elif iteration == (self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 1: - height = 0. - duration = self.takeoff_land_duration - - command_type = Command(3) # Land. - args = [height, duration] - - elif iteration == (2*self.takeoff_land_duration + self.end_t)*self.CTRL_FREQ + 2: - command_type = Command(-1) # Terminate command to be sent once trajectory is completed. - args = [] - + self.args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] else: - command_type = Command(0) # None. - args = [] + self.args[1] = np.zeros(3) + self.args[2] = np.zeros(3) + self.args[4] = np.zeros(3) + command_type = Command(1) # cmdFullState. + args = self.args ######################### # REPLACE THIS (END) #### From afb20e29ca884bd73fa781de79d001a41fada1b1 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sat, 8 Oct 2022 20:47:03 +0800 Subject: [PATCH 18/30] Move custom functions to custom_utils.py --- competition/custom_utils.py | 95 +++++++++++++ competition/edit_this.py | 257 ++++++++++++------------------------ 2 files changed, 179 insertions(+), 173 deletions(-) create mode 100644 competition/custom_utils.py diff --git a/competition/custom_utils.py b/competition/custom_utils.py new file mode 100644 index 000000000..9cef44d1f --- /dev/null +++ b/competition/custom_utils.py @@ -0,0 +1,95 @@ +"""Utility module. + +For the IROS competition + +""" + +from math import sqrt, sin, cos +import numpy as np + +def cubic_interp(inv_t, x0, xf, dx0, dxf): + dx = xf - x0 + return ( + x0, + dx0, + (3*dx*inv_t - 2*dx0 - dxf)*inv_t, + (-2*dx*inv_t + (dxf + dx0))*inv_t*inv_t) + +def quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf): + dx = xf - x0 + return ( + x0, + dx0, + d2x0*0.5, + ((10*dx*inv_t - (4*dxf+6*dx0))*inv_t - 0.5*(3*d2x0-d2xf))*inv_t, + ((-15*dx*inv_t + (7*dxf+8*dx0))*inv_t + 0.5*(3*d2x0-2*d2xf))*inv_t*inv_t, + ((6*dx*inv_t - 3*(dxf+dx0))*inv_t - 0.5*(d2x0-d2xf))*inv_t*inv_t*inv_t) + + +def f(coeffs, idx, t): + val = 0 + for coeff in coeffs[:,idx]: + val *= t + val += coeff + return val + +def df(coeffs, idx, t): + coeffs_no = len(coeffs[:,idx]) + val = 0 + for i, coeff in enumerate(coeffs[:-1,idx]): + val *= t + factor = (coeffs_no - i - 1) + val += coeff * factor + return val + +def d2f(coeffs, idx, t): + coeffs_no = len(coeffs[:,idx]) + val = 0 + for i, coeff in enumerate(coeffs[:-2,idx]): + val *= t + factor = (coeffs_no - i - 1) + val += coeff * factor * (factor - 1) + return val + +def d3f(coeffs, idx, t): + coeffs_no = len(coeffs[:,idx]) + val = 0 + for i, coeff in enumerate(coeffs[:-3,idx]): + val *= t + factor = (coeffs_no - i - 1) + val += coeff * factor * (factor - 1) * (factor - 2) + return val + +def df_idx(length, ts, x_coeffs, y_coeffs, z_coeffs): + def df_(t): + for idx in range(length-1): + if (ts[idx+1] > t): + break + t -= ts[idx] + dx = df(x_coeffs, idx, t) + dy = df(y_coeffs, idx, t) + dz = df(z_coeffs, idx, t) + return sqrt(dx*dx+dy*dy+dz*dz) + return df_ + +# Fill s, v, a, t arrays fro S-curve using j and T +def fill(T, t, s, v, a, j): + for i in range(7): + dt = T[i+1] + t[i+1] = t[i] + dt + s[i+1] = ((j[i]/6*dt + a[i]/2)*dt + v[i])*dt+ s[i] + v[i+1] = (j[i]/2*dt + a[i] )*dt + v[i] + a[i+1] = j[i] *dt + a[i] + +# RPY angles to 3x3 rotation matrix +def rpy2rot(roll, pitch, yaw): + sr = sin(roll) + cr = cos(roll) + sp = sin(pitch) + cp = cos(pitch) + sy = sin(yaw) + cy = cos(yaw) + m = np.matrix(((cy*cp, -sy*cr+cy*sp*sr, sy*sr+cy*cr*sp), + (sy*cp, cy*cr+sy*sp*sr, -cy*sr+sp*sy*cr), + (-sp , cp*sr , cp*cr))) + return m \ No newline at end of file diff --git a/competition/edit_this.py b/competition/edit_this.py index baa52b4c2..eea127705 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -25,9 +25,6 @@ """ import numpy as np -import scipy.interpolate -import scipy.integrate -from math import sqrt, sin, cos, atan2, pi, log1p from collections import deque @@ -41,13 +38,16 @@ # REPLACE THIS (START) ## ######################### -# Optionally, create and import modules you wrote. -# Please refrain from importing large or unstable 3rd party packages. +import scipy.interpolate +import scipy.integrate +from math import sqrt, sin, cos, atan2, log1p, pi +from copy import deepcopy + try: - import example_custom_utils as ecu + import custom_utils as utils except ImportError: # PyTest import. - from . import example_custom_utils as ecu + from . import custom_utils as utils ######################### # REPLACE THIS (END) #### @@ -204,22 +204,8 @@ def __init__(self, # yaw_curr = yaw_next # dyaw = dyaw_next - self.cubic_interp = lambda inv_t, x0, xf, dx0, dxf:( - x0, - dx0, - (3*(xf-x0)*inv_t - 2*dx0 - dxf)*inv_t, - (-2*(xf-x0)*inv_t + (dxf + dx0))*inv_t*inv_t) - - self.quintic_interp = lambda inv_t, x0, xf, dx0, dxf, d2x0, d2xf:( - x0, - dx0, - d2x0*0.5, - ((10*(xf-x0)*inv_t - (4*dxf+6*dx0))*inv_t - 0.5*(3*d2x0-d2xf))*inv_t, - ((-15*(xf-x0)*inv_t + (7*dxf+8*dx0))*inv_t + 0.5*(3*d2x0-2*d2xf))*inv_t*inv_t, - ((6*(xf-x0)*inv_t - 3*(dxf+dx0))*inv_t - 0.5*(d2x0-d2xf))*inv_t*inv_t*inv_t) - - self.x_coeffs = np.zeros((6,len(waypoints)-1)) - self.y_coeffs = np.zeros((6,len(waypoints)-1)) + x_coeffs = np.zeros((6,len(waypoints)-1)) + y_coeffs = np.zeros((6,len(waypoints)-1)) [x0, y0, _, yaw0] = waypoints[0] dx0 = cos(yaw0) dy0 = sin(yaw0) @@ -239,85 +225,29 @@ def __init__(self, if idx == self.length-1: # yaw of last goal not important dxf = dx0 * 0.01 dyf = dy0 * 0.01 - self.x_coeffs[::-1,idx-1] = self.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) - self.y_coeffs[::-1,idx-1] = self.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) + x_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) + y_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) [x0, y0] = [xf, yf] [dx0, dy0] = [dxf, dyf] # [d2x0, d2y0] = [d2xf, d2yf] waypoints = np.array(waypoints) - self.z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c + z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c # modify endpoint gradient for smooth z ending - self.z_coeffs[::-1,-1] = self.cubic_interp(1/(self.ts[-1]-self.ts[-2]), + z_coeffs[::-1,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), waypoints[-2,2], waypoints[-1,2], - self.z_coeffs[-2,-1], + z_coeffs[-2,-1], 0 ) - - def f_(coeffs): - def f(idx, t): - val = 0 - for coeff in coeffs[:,idx]: - val *= t - val += coeff - return val - return f - - def df_(coeffs): - def f(idx, t): - coeffs_no = len(coeffs[:,idx]) - val = 0 - for i, coeff in enumerate(coeffs[:-1,idx]): - val *= t - factor = (coeffs_no - i - 1) - val += coeff * factor - return val - return f - - def d2f_(coeffs): - def f(idx, t): - coeffs_no = len(coeffs[:,idx]) - val = 0 - for i, coeff in enumerate(coeffs[:-2,idx]): - val *= t - factor = (coeffs_no - i - 1) - val += coeff * factor * (factor - 1) - return val - return f - - def d3f_(coeffs): - def f(idx, t): - coeffs_no = len(coeffs[:,idx]) - val = 0 - for i, coeff in enumerate(coeffs[:-3,idx]): - val *= t - factor = (coeffs_no - i - 1) - val += coeff * factor * (factor - 1) * (factor - 2) - return val - return f - - def df_idx(): - def df(t): - for idx in range(self.length-1): - if (self.ts[idx+1] > t): - break - t -= self.ts[idx] - dx = self.df(self.x_coeffs)(idx, t) - dy = self.df(self.y_coeffs)(idx, t) - dz = self.df(self.z_coeffs)(idx, t) - return sqrt(dx*dx+dy*dy+dz*dz) - return df - - self.f = f_ - self.df = df_ - self.d2f = d2f_ - self.d3f = d3f_ + self.coeffs = [x_coeffs, y_coeffs, z_coeffs] # Integrate to get pathlength pathlength = 0 for idx in range(self.length-1): - pathlength += scipy.integrate.quad(df_idx(), 0, self.ts[idx+1] - self.ts[idx])[0] + pathlength += scipy.integrate.quad( + utils.df_idx(self.length, self.ts, x_coeffs, y_coeffs, z_coeffs), + 0, self.ts[idx+1] - self.ts[idx])[0] self.scaling_factor = self.ts[-1] / pathlength if self.VERBOSE: @@ -331,12 +261,18 @@ def df(t): gate += 1 t_diff_scaled[i] = t_scaled[i] - self.ts[gate] gate_scaled[i] = gate - x_scaled = np.array(tuple(map(self.f(self.x_coeffs), gate_scaled, t_diff_scaled))) - y_scaled = np.array(tuple(map(self.f(self.y_coeffs), gate_scaled, t_diff_scaled))) - z_scaled = np.array(tuple(map(self.f(self.z_coeffs), gate_scaled, t_diff_scaled))) - print(self.x_coeffs) - print(self.y_coeffs) - print(self.z_coeffs) + x_scaled = np.array(tuple(map( + lambda idx, t: utils.f(x_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + y_scaled = np.array(tuple(map( + lambda idx, t: utils.f(y_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + z_scaled = np.array(tuple(map( + lambda idx, t: utils.f(z_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + print(x_coeffs) + print(y_coeffs) + print(z_coeffs) print(waypoints) print(t_scaled) print(t_diff_scaled) @@ -359,16 +295,8 @@ def df(t): t = np.zeros(8) T = np.zeros(8) - def fill(): - for i in range(7): - dt = T[i+1] - t[i+1] = t[i] + dt - s[i+1] = ((j[i]/6*dt + a[i]/2)*dt + v[i])*dt+ s[i] - v[i+1] = (j[i]/2*dt + a[i] )*dt + v[i] - a[i+1] = j[i] *dt + a[i] - T[1] = T[3] = T[5] = T[7] = min(sqrt(v_max/j_max), a_max/j_max) # min(wont't hit a_max, will hit a_max) - fill() + utils.fill(T, t, s, v, a, j) ds = sf - s[-1] if ds < 0: # T[2] = T[4] = T[6] = 0 @@ -383,7 +311,7 @@ def fill(): else: # T[4] = 0 T[2] = T[6] = added_T_2 - fill() + utils.fill(T, t, s, v, a, j) def s_curve(t_): for i in range(7): if t[i+1] > t_: @@ -400,24 +328,9 @@ def s_curve(t_): self.scaled_t = 0 self.time_scale = 1 self.takeoff_land_duration = 2 - - def rpy2rot(roll, pitch, yaw): - sr = sin(roll) - cr = cos(roll) - sp = sin(pitch) - cp = cos(pitch) - sy = sin(yaw) - cy = cos(yaw) - m = np.matrix(((cy*cp, -sy*cr+cy*sp*sr, sy*sr+cy*cr*sp), - (sy*cp, cy*cr+sy*sp*sr, -cy*sr+sp*sy*cr), - (-sp , cp*sr , cp*cr))) - return m - self.rpy2rot = rpy2rot self.at_gate = False self.gate_no = 0 - self.run_x_coeffs = np.copy(self.x_coeffs) - self.run_y_coeffs = np.copy(self.y_coeffs) - self.run_z_coeffs = np.copy(self.z_coeffs) + self.run_coeffs = deepcopy(self.coeffs) self.run_ts = np.copy(self.ts) ######################### @@ -459,9 +372,8 @@ def cmdFirmware(self, # REPLACE THIS (START) ## ######################### - # # Handwritten solution for GitHub's getting_stated scenario. if self.scaled_t < self.end_t: - if iteration > 0 and info['current_target_gate_in_range']: + if info and info['current_target_gate_in_range']: if not self.at_gate: # Local replan when near to goal self.at_gate = True @@ -471,57 +383,57 @@ def cmdFirmware(self, inv_t = 1/dt dx_gate = cos(yaw) * self.grad_scale dy_gate = sin(yaw) * self.grad_scale - self.run_x_coeffs = np.insert(self.run_x_coeffs, self.gate_no+1, np.flip(self.quintic_interp( + self.run_coeffs[0] = np.insert(self.run_coeffs[0], self.gate_no+1, np.flip(utils.quintic_interp( inv_t, - self.x_c, + self.target_pose[0], x, - self.dx_c, + self.tangent[0], dx_gate, - self.d2x_c, + self.dtangent[0], 0 )), axis=1) - self.run_y_coeffs = np.insert(self.run_y_coeffs, self.gate_no+1, np.flip(self.quintic_interp( + self.run_coeffs[1] = np.insert(self.run_coeffs[1], self.gate_no+1, np.flip(utils.quintic_interp( inv_t, - self.y_c, + self.target_pose[1], y, - self.dy_c, + self.tangent[1], dy_gate, - self.d2y_c, + self.dtangent[1], 0 )), axis=1) - self.run_z_coeffs = np.insert(self.run_z_coeffs, self.gate_no+1, np.flip(self.cubic_interp( + self.run_coeffs[2] = np.insert(self.run_coeffs[2], self.gate_no+1, np.flip(utils.cubic_interp( inv_t, - self.z_c, + self.target_pose[2], z, - self.dz_c, - self.df(self.run_z_coeffs)(self.gate_no, dt) + self.tangent[2], + utils.df(self.run_coeffs[2], self.gate_no, dt) )), axis=1) dt = self.run_ts[self.gate_no + 2] - self.run_ts[self.gate_no + 1] inv_t = 1/dt - self.run_x_coeffs[::-1,self.gate_no+2] = self.quintic_interp( + self.run_coeffs[0][::-1,self.gate_no+2] = utils.quintic_interp( inv_t, x, - self.f(self.run_x_coeffs)(self.gate_no+2, dt), + utils.f(self.run_coeffs[0], self.gate_no+2, dt), dx_gate, - self.df(self.run_x_coeffs)(self.gate_no+2, dt), + utils.df(self.run_coeffs[0], self.gate_no+2, dt), 0, 0 ) - self.run_y_coeffs[::-1,self.gate_no+2] = self.quintic_interp( + self.run_coeffs[1][::-1,self.gate_no+2] = utils.quintic_interp( inv_t, y, - self.f(self.run_y_coeffs)(self.gate_no+2, dt), + utils.f(self.run_coeffs[1], self.gate_no+2, dt), dy_gate, - self.df(self.run_y_coeffs)(self.gate_no+2, dt), + utils.df(self.run_coeffs[1], self.gate_no+2, dt), 0, 0 ) - self.run_z_coeffs[::-1,self.gate_no+2] = self.cubic_interp( + self.run_coeffs[2][::-1,self.gate_no+2] = utils.cubic_interp( inv_t, z, - self.f(self.run_z_coeffs)(self.gate_no+2, dt), - self.run_z_coeffs[-2, self.gate_no+2], - self.df(self.run_z_coeffs)(self.gate_no+2, dt) + utils.f(self.run_coeffs[2], self.gate_no+2, dt), + self.run_coeffs[2][-2, self.gate_no+2], + utils.df(self.run_coeffs[2], self.gate_no+2, dt) ) self.run_ts = np.insert(self.run_ts, self.gate_no+1, self.curve_t) self.length += 1 @@ -544,47 +456,47 @@ def cmdFirmware(self, t = curve_t - self.run_ts[self.gate_no] - self.x_c = self.f(self.run_x_coeffs)(self.gate_no, t) - self.y_c = self.f(self.run_y_coeffs)(self.gate_no, t) - self.z_c = self.f(self.run_z_coeffs)(self.gate_no, t) - target_pos = np.array([self.x_c, self.y_c, self.z_c]) - error = np.array((obs[0] - self.x_c, obs[2] - self.y_c, obs[4] - self.z_c)) # positional error (world frame) + self.target_pose = np.array(tuple(map( + lambda coeffs: utils.f(coeffs, self.gate_no, t), + self.run_coeffs + ))) + self.tangent = np.array(tuple(map( + lambda coeffs: utils.df(coeffs, self.gate_no, t), + self.run_coeffs + ))) + self.dtangent = np.array(tuple(map( + lambda coeffs: utils.d2f(coeffs, self.gate_no, t), + self.run_coeffs + ))) + self.d2tangent = np.array(tuple(map( + lambda coeffs: utils.d3f(coeffs, self.gate_no, t), + self.run_coeffs + ))) + + error = np.array((obs[0] - self.target_pose[0], obs[2] - self.target_pose[1], obs[4] - self.target_pose[2])) # positional error (world frame) # print(error) self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.05 self.time_scale = min(1.0, self.time_scale) # print(self.time_scale) - self.dx_c = self.df(self.run_x_coeffs)(self.gate_no, t) - self.dy_c = self.df(self.run_y_coeffs)(self.gate_no, t) - self.dz_c = self.df(self.run_z_coeffs)(self.gate_no, t) - self.d2x_c = self.d2f(self.run_x_coeffs)(self.gate_no, t) - self.d2y_c = self.d2f(self.run_y_coeffs)(self.gate_no, t) - self.d2z_c = self.d2f(self.run_z_coeffs)(self.gate_no, t) - self.d3x_c = self.d3f(self.run_x_coeffs)(self.gate_no, t) - self.d3y_c = self.d3f(self.run_y_coeffs)(self.gate_no, t) - self.d3z_c = self.d3f(self.run_z_coeffs)(self.gate_no, t) - tangent = np.array((self.dx_c,self.dy_c,self.dz_c)) - dtangent = np.array((self.d2x_c,self.d2y_c,self.d2z_c)) - d2tangent = np.array((self.d3x_c,self.d3y_c,self.d3z_c)) - - target_yaw = atan2(self.dy_c, self.dx_c) - target_vel = tangent * curve_v - target_acc = tangent * curve_a + dtangent * curve_v**2 + target_yaw = atan2(self.tangent[1], self.tangent[0]) + target_vel = self.tangent * curve_v + target_acc = self.tangent * curve_a + self.dtangent * curve_v**2 # Roll, pitch rate # Small angle approximation - target_jerk = tangent * curve_j + d2tangent * curve_v**3 + 3 * dtangent * curve_v * curve_a - Jinv = self.rpy2rot(obs[6], obs[7], obs[8]).transpose() + target_jerk = self.tangent * curve_j + self.d2tangent * curve_v**3 + 3 * self.dtangent * curve_v * curve_a + Jinv = utils.rpy2rot(obs[6], obs[7], obs[8]).transpose() body_jerk = np.matmul(Jinv, target_jerk.transpose()) p = self.mass/9.8*body_jerk[0,1] # roll rate = mass / g * y_jerk q = self.mass/9.8*body_jerk[0,0] # pitch rate = mass / g * x_jerk # Yaw rate - den = np.linalg.norm(tangent[:2]) + den = np.linalg.norm(self.tangent[:2]) if den < 1e-9: r = 0 else: - num = self.dx_c * self.d2y_c - self.dy_c * self.d2x_c + num = self.tangent[0] * self.dtangent[1] - self.tangent[1] * self.dtangent[0] r = num/den r *= curve_v @@ -593,7 +505,7 @@ def cmdFirmware(self, # target_acc = np.zeros(3) # target_yaw = 0. # target_rpy_rates = np.zeros(3) - self.args = [target_pos, target_vel, target_acc, target_yaw, target_rpy_rates] + self.args = [self.target_pose, target_vel, target_acc, target_yaw, target_rpy_rates] else: self.args[1] = np.zeros(3) self.args[2] = np.zeros(3) @@ -709,12 +621,11 @@ def interEpisodeLearn(self): _ = self.done_buffer _ = self.info_buffer + # Reset run variables self.scaled_t = 0 self.at_gate = False self.gate_no = 0 - self.run_x_coeffs = np.copy(self.x_coeffs) - self.run_y_coeffs = np.copy(self.y_coeffs) - self.run_z_coeffs = np.copy(self.z_coeffs) + self.run_coeffs = deepcopy(self.coeffs) self.run_ts = np.copy(self.ts) self.length = len(self.NOMINAL_GATES) + 2 From 9a69f590c4254cbac9b6e14b9c6bf6ffa1cb38f3 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 9 Oct 2022 03:07:14 +0800 Subject: [PATCH 19/30] Changed z-axis iterpolator to quintic --- competition/edit_this.py | 160 ++++++++++++++------------------------- 1 file changed, 57 insertions(+), 103 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index eea127705..26b38ea48 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -38,7 +38,6 @@ # REPLACE THIS (START) ## ######################### -import scipy.interpolate import scipy.integrate from math import sqrt, sin, cos, atan2, log1p, pi from copy import deepcopy @@ -115,12 +114,12 @@ def __init__(self, # Kinematic limits # TODO determine better estimates from model v_max = 1.5 - a_max = .2 - j_max = 1 - self.mass = initial_info['nominal_physical_parameters']['quadrotor_mass'] + a_max = .5 + j_max = 2 # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve - self.grad_scale = .6 + self.grad_scale = .4 * v_max + self.mass = initial_info['nominal_physical_parameters']['quadrotor_mass'] self.tall = initial_info["gate_dimensions"]["tall"]["height"] self.low = initial_info["gate_dimensions"]["low"]["height"] @@ -140,13 +139,13 @@ def __init__(self, waypoints.append([initial_info["x_reference"][0], initial_info["x_reference"][2], initial_info["x_reference"][4], initial_info["x_reference"][8]]) # "time" for each waypoint - # time interval determined by eulcidean distance between waypoints along xy plane + # time interval determined by eulcidean distance between waypoints along xyz plane self.ts = np.zeros(self.length) - [x_prev, y_prev, _, _] = waypoints[0] + [x_prev, y_prev, z_prev, _] = waypoints[0] for idx in range(1,self.length): - [x_curr, y_curr, _, _] = waypoints[idx] - xy_norm = sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) - self.ts[idx] = self.ts[idx-1] + xy_norm / v_max + [x_curr, y_curr, z_curr, _] = waypoints[idx] + norm = sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2 + (z_curr-z_prev)**2) + self.ts[idx] = self.ts[idx-1] + norm / v_max # Flip gates # Selectively flip gate orientation based on vector from previous and to next waypoint, @@ -206,40 +205,43 @@ def __init__(self, x_coeffs = np.zeros((6,len(waypoints)-1)) y_coeffs = np.zeros((6,len(waypoints)-1)) - [x0, y0, _, yaw0] = waypoints[0] - dx0 = cos(yaw0) - dy0 = sin(yaw0) + z_coeffs = np.zeros((6,len(waypoints)-1)) + [x0, y0, z0, yaw0] = waypoints[0] + dx0 = cos(yaw0)*0.01 + dy0 = sin(yaw0)*0.01 + dz0 = v_max + dzf = 0 d2x0 = 0 d2y0 = 0 + d2z0 = a_max d2xf = 0 d2yf = 0 + d2zf = 0 for idx in range(1, self.length): - [xf, yf, _, yawf] = waypoints[idx] + [xf, yf, zf, yawf] = waypoints[idx] dt = self.ts[idx] - self.ts[idx - 1] inv_t = 1/dt dxf = cos(yawf) * self.grad_scale dyf = sin(yawf) * self.grad_scale - if idx == 0: # don't need to care about curving out of first goal - dxf *= 0.01 - dyf *= 0.01 if idx == self.length-1: # yaw of last goal not important dxf = dx0 * 0.01 dyf = dy0 * 0.01 x_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) y_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) - [x0, y0] = [xf, yf] - [dx0, dy0] = [dxf, dyf] - # [d2x0, d2y0] = [d2xf, d2yf] + z_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) + [x0, y0, z0] = [xf, yf, zf] + [dx0, dy0, dz0] = [dxf, dyf, dzf] + [d2x0, d2y0, d2z0] = [d2xf, d2yf, d2zf] waypoints = np.array(waypoints) - z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c - - # modify endpoint gradient for smooth z ending - z_coeffs[::-1,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), - waypoints[-2,2], - waypoints[-1,2], - z_coeffs[-2,-1], - 0 - ) + # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c + + # modify endpoint gradient for smooth z ending (pchip iterpolator) + # z_coeffs[::-1,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), + # waypoints[-2,2], + # waypoints[-1,2], + # z_coeffs[-2,-1], + # 0 + # ) self.coeffs = [x_coeffs, y_coeffs, z_coeffs] # Integrate to get pathlength @@ -274,12 +276,6 @@ def __init__(self, print(y_coeffs) print(z_coeffs) print(waypoints) - print(t_scaled) - print(t_diff_scaled) - print(gate_scaled) - print(x_scaled) - print(y_scaled) - print(z_scaled) # Plot trajectory in each dimension and 3D. plot_trajectory(t_scaled, waypoints, x_scaled, y_scaled, z_scaled) # Draw the trajectory on PyBullet's GUI @@ -383,62 +379,32 @@ def cmdFirmware(self, inv_t = 1/dt dx_gate = cos(yaw) * self.grad_scale dy_gate = sin(yaw) * self.grad_scale - self.run_coeffs[0] = np.insert(self.run_coeffs[0], self.gate_no+1, np.flip(utils.quintic_interp( - inv_t, - self.target_pose[0], - x, - self.tangent[0], - dx_gate, - self.dtangent[0], - 0 - )), axis=1) - self.run_coeffs[1] = np.insert(self.run_coeffs[1], self.gate_no+1, np.flip(utils.quintic_interp( - inv_t, - self.target_pose[1], - y, - self.tangent[1], - dy_gate, - self.dtangent[1], - 0 - )), axis=1) - self.run_coeffs[2] = np.insert(self.run_coeffs[2], self.gate_no+1, np.flip(utils.cubic_interp( - inv_t, - self.target_pose[2], - z, - self.tangent[2], - utils.df(self.run_coeffs[2], self.gate_no, dt) - )), axis=1) + dz_gate = 0 + for i, (val, dval) in enumerate(zip([x,y,z], [dx_gate, dy_gate, dz_gate])): + self.run_coeffs[i] = np.insert(self.run_coeffs[i], self.gate_no+1, np.flip(utils.quintic_interp( + inv_t, + self.target_pose[i], + val, + self.tangent[i], + dval, + self.dtangent[i], + 0 + )), axis=1) dt = self.run_ts[self.gate_no + 2] - self.run_ts[self.gate_no + 1] inv_t = 1/dt - self.run_coeffs[0][::-1,self.gate_no+2] = utils.quintic_interp( - inv_t, - x, - utils.f(self.run_coeffs[0], self.gate_no+2, dt), - dx_gate, - utils.df(self.run_coeffs[0], self.gate_no+2, dt), - 0, - 0 - ) - self.run_coeffs[1][::-1,self.gate_no+2] = utils.quintic_interp( - inv_t, - y, - utils.f(self.run_coeffs[1], self.gate_no+2, dt), - dy_gate, - utils.df(self.run_coeffs[1], self.gate_no+2, dt), - 0, - 0 - ) - self.run_coeffs[2][::-1,self.gate_no+2] = utils.cubic_interp( - inv_t, - z, - utils.f(self.run_coeffs[2], self.gate_no+2, dt), - self.run_coeffs[2][-2, self.gate_no+2], - utils.df(self.run_coeffs[2], self.gate_no+2, dt) - ) + for i, (val, dval) in enumerate(zip([x,y,z], [dx_gate, dy_gate, dz_gate])): + self.run_coeffs[i][::-1,self.gate_no+2] = utils.quintic_interp( + inv_t, + val, + utils.f(self.run_coeffs[i], self.gate_no+2, dt), + dval, + utils.df(self.run_coeffs[i], self.gate_no+2, dt), + 0, + 0 + ) self.run_ts = np.insert(self.run_ts, self.gate_no+1, self.curve_t) self.length += 1 self.gate_no += 1 - # print(np.insert(self.run_ts, self.gate_no+1, self.curve_t)) else: self.at_gate = False @@ -456,27 +422,15 @@ def cmdFirmware(self, t = curve_t - self.run_ts[self.gate_no] - self.target_pose = np.array(tuple(map( - lambda coeffs: utils.f(coeffs, self.gate_no, t), - self.run_coeffs - ))) - self.tangent = np.array(tuple(map( - lambda coeffs: utils.df(coeffs, self.gate_no, t), - self.run_coeffs - ))) - self.dtangent = np.array(tuple(map( - lambda coeffs: utils.d2f(coeffs, self.gate_no, t), - self.run_coeffs - ))) - self.d2tangent = np.array(tuple(map( - lambda coeffs: utils.d3f(coeffs, self.gate_no, t), - self.run_coeffs - ))) + self.target_pose = np.array([utils.f(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) + self.tangent = np.array([utils.df(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) + self.dtangent = np.array([utils.d2f(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) + self.d2tangent = np.array([utils.d3f(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) error = np.array((obs[0] - self.target_pose[0], obs[2] - self.target_pose[1], obs[4] - self.target_pose[2])) # positional error (world frame) # print(error) self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.05 - self.time_scale = min(1.0, self.time_scale) + self.time_scale = max(0.0, min(1.0, self.time_scale)) # print(self.time_scale) target_yaw = atan2(self.tangent[1], self.tangent[0]) From 00496cc2a92751053581a47b44105720722ec50a Mon Sep 17 00:00:00 2001 From: Hashir Zahir Date: Sat, 15 Oct 2022 22:13:44 -0700 Subject: [PATCH 20/30] Add initial obstacle avoidance --- competition/custom_utils.py | 55 +++++++++++- competition/edit_this.py | 162 +++++++++++++++++++----------------- 2 files changed, 138 insertions(+), 79 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index 9cef44d1f..5ba99596f 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -4,7 +4,7 @@ """ -from math import sqrt, sin, cos +from math import sqrt, sin, cos, atan2, pi import numpy as np def cubic_interp(inv_t, x0, xf, dx0, dxf): @@ -92,4 +92,55 @@ def rpy2rot(roll, pitch, yaw): m = np.matrix(((cy*cp, -sy*cr+cy*sp*sr, sy*sr+cy*cr*sp), (sy*cp, cy*cr+sy*sp*sr, -cy*sr+sp*sy*cr), (-sp , cp*sr , cp*cr))) - return m \ No newline at end of file + return m + +def is_intersect(waypoints, obstacle, low, high): + [x,y] = obstacle[0:2] + x_min, x_max = x + low, x + high + y_min, y_max = y + low, y + high + for idx, waypoint in enumerate(waypoints): + [x, y] = waypoint[0:2] + # print((x,y), obstacle, (x_min, y_min), (x_max, y_max)) + if x_min <= x and x <= x_max and y_min <= y and y <= y_max: + return True, idx + return False, None + +# Distance is distance away from obstacle that vector between spline_waypoint and obstacle is projected +# returns new waypoint [x,y,z,yaw] +def project_point(spline_waypoint, obstacle, distance): + [x1,y1] = spline_waypoint[0:2] + [x2,y2] = obstacle[0:2] + vec = np.array([x1-x2, y1-y2]) + norm_vec = vec * (1/np.linalg.norm(vec)) + new_waypoint = norm_vec * distance + np.array(obstacle[0:2]) + z = spline_waypoint[2] + yaw = pi/2. + atan2(new_waypoint[1], new_waypoint[0]) + new_waypoint = [new_waypoint[0], new_waypoint[1], z, yaw] + print("new_waypoint: ", new_waypoint) + return new_waypoint + +def update_waypoints_avoid_obstacles(spline_waypoints, waypoints, obstacles, initial_info): + is_collision = False + + ### Determine range around obstacle that is dangerous (low,high) + obstacle_distrib_dict = initial_info['gates_and_obs_randomization'] + tolerance = 0.1 # magic number to say how near we can be to the obs. includes drone size as well + low = -initial_info['obstacle_dimensions']['radius'] - tolerance + high = initial_info['obstacle_dimensions']['radius'] + tolerance + if 'obstacles' in obstacle_distrib_dict: + obstacle_distrib_dict = obstacle_distrib_dict['obstacles'] + low -= obstacle_distrib_dict['low'] + high += obstacle_distrib_dict['high'] + + # Check if obstacle position uncertainty intersects with waypoint path + for obstacle in obstacles: + collision, idx = is_intersect(spline_waypoints, obstacle, low, high) + if collision: + is_collision = True + print("Collision!") + dist = max(-low, high) + 0.5 + new_waypoint = project_point(spline_waypoints[idx], obstacle, dist) + # TODO: Fix indexing!! + waypoints = np.insert(waypoints, 1, new_waypoint, axis=0) + + return is_collision, waypoints diff --git a/competition/edit_this.py b/competition/edit_this.py index 26b38ea48..6ab6fad5c 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -28,6 +28,8 @@ from collections import deque +from competition.custom_utils import update_waypoints_avoid_obstacles + try: from competition_utils import Command, PIDController, timing_step, timing_ep, plot_trajectory, draw_trajectory except ImportError: @@ -203,83 +205,89 @@ def __init__(self, # yaw_curr = yaw_next # dyaw = dyaw_next - x_coeffs = np.zeros((6,len(waypoints)-1)) - y_coeffs = np.zeros((6,len(waypoints)-1)) - z_coeffs = np.zeros((6,len(waypoints)-1)) - [x0, y0, z0, yaw0] = waypoints[0] - dx0 = cos(yaw0)*0.01 - dy0 = sin(yaw0)*0.01 - dz0 = v_max - dzf = 0 - d2x0 = 0 - d2y0 = 0 - d2z0 = a_max - d2xf = 0 - d2yf = 0 - d2zf = 0 - for idx in range(1, self.length): - [xf, yf, zf, yawf] = waypoints[idx] - dt = self.ts[idx] - self.ts[idx - 1] - inv_t = 1/dt - dxf = cos(yawf) * self.grad_scale - dyf = sin(yawf) * self.grad_scale - if idx == self.length-1: # yaw of last goal not important - dxf = dx0 * 0.01 - dyf = dy0 * 0.01 - x_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) - y_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) - z_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) - [x0, y0, z0] = [xf, yf, zf] - [dx0, dy0, dz0] = [dxf, dyf, dzf] - [d2x0, d2y0, d2z0] = [d2xf, d2yf, d2zf] - waypoints = np.array(waypoints) - # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c - - # modify endpoint gradient for smooth z ending (pchip iterpolator) - # z_coeffs[::-1,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), - # waypoints[-2,2], - # waypoints[-1,2], - # z_coeffs[-2,-1], - # 0 - # ) - self.coeffs = [x_coeffs, y_coeffs, z_coeffs] - - # Integrate to get pathlength - pathlength = 0 - for idx in range(self.length-1): - pathlength += scipy.integrate.quad( - utils.df_idx(self.length, self.ts, x_coeffs, y_coeffs, z_coeffs), - 0, self.ts[idx+1] - self.ts[idx])[0] - self.scaling_factor = self.ts[-1] / pathlength - - if self.VERBOSE: - duration = self.ts[-1] - self.ts[0] - t_scaled = np.linspace(self.ts[0], self.ts[-1], int(duration*self.CTRL_FREQ)) - t_diff_scaled = np.zeros(t_scaled.shape) - gate_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) - gate = 0 - for i, t in enumerate(t_scaled): - if gate < self.length and t > self.ts[gate+1]: - gate += 1 - t_diff_scaled[i] = t_scaled[i] - self.ts[gate] - gate_scaled[i] = gate - x_scaled = np.array(tuple(map( - lambda idx, t: utils.f(x_coeffs, idx, t), - gate_scaled, t_diff_scaled))) - y_scaled = np.array(tuple(map( - lambda idx, t: utils.f(y_coeffs, idx, t), - gate_scaled, t_diff_scaled))) - z_scaled = np.array(tuple(map( - lambda idx, t: utils.f(z_coeffs, idx, t), - gate_scaled, t_diff_scaled))) - print(x_coeffs) - print(y_coeffs) - print(z_coeffs) - print(waypoints) - # Plot trajectory in each dimension and 3D. - plot_trajectory(t_scaled, waypoints, x_scaled, y_scaled, z_scaled) - # Draw the trajectory on PyBullet's GUI - draw_trajectory(initial_info, waypoints, x_scaled, y_scaled, z_scaled) + is_collision = True + while is_collision: + x_coeffs = np.zeros((6,len(waypoints)-1)) + y_coeffs = np.zeros((6,len(waypoints)-1)) + z_coeffs = np.zeros((6,len(waypoints)-1)) + [x0, y0, z0, yaw0] = waypoints[0] + dx0 = cos(yaw0)*0.01 + dy0 = sin(yaw0)*0.01 + dz0 = v_max + dzf = 0 + d2x0 = 0 + d2y0 = 0 + d2z0 = a_max + d2xf = 0 + d2yf = 0 + d2zf = 0 + for idx in range(1, self.length): + [xf, yf, zf, yawf] = waypoints[idx] + dt = self.ts[idx] - self.ts[idx - 1] + inv_t = 1/dt + dxf = cos(yawf) * self.grad_scale + dyf = sin(yawf) * self.grad_scale + if idx == self.length-1: # yaw of last goal not important + dxf = dx0 * 0.01 + dyf = dy0 * 0.01 + x_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) + y_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) + z_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) + [x0, y0, z0] = [xf, yf, zf] + [dx0, dy0, dz0] = [dxf, dyf, dzf] + [d2x0, d2y0, d2z0] = [d2xf, d2yf, d2zf] + waypoints = np.array(waypoints) + # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c + + # modify endpoint gradient for smooth z ending (pchip iterpolator) + # z_coeffs[::-1,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), + # waypoints[-2,2], + # waypoints[-1,2], + # z_coeffs[-2,-1], + # 0 + # ) + self.coeffs = [x_coeffs, y_coeffs, z_coeffs] + + # Integrate to get pathlength + pathlength = 0 + for idx in range(self.length-1): + pathlength += scipy.integrate.quad( + utils.df_idx(self.length, self.ts, x_coeffs, y_coeffs, z_coeffs), + 0, self.ts[idx+1] - self.ts[idx])[0] + self.scaling_factor = self.ts[-1] / pathlength + + if self.VERBOSE: + duration = self.ts[-1] - self.ts[0] + t_scaled = np.linspace(self.ts[0], self.ts[-1], int(duration*self.CTRL_FREQ)) + t_diff_scaled = np.zeros(t_scaled.shape) + gate_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) + gate = 0 + for i, t in enumerate(t_scaled): + if gate < self.length and t > self.ts[gate+1]: + gate += 1 + t_diff_scaled[i] = t_scaled[i] - self.ts[gate] + gate_scaled[i] = gate + x_scaled = np.array(tuple(map( + lambda idx, t: utils.f(x_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + y_scaled = np.array(tuple(map( + lambda idx, t: utils.f(y_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + z_scaled = np.array(tuple(map( + lambda idx, t: utils.f(z_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + print(x_coeffs) + print(y_coeffs) + print(z_coeffs) + print(waypoints) + # Plot trajectory in each dimension and 3D. + plot_trajectory(t_scaled, waypoints, x_scaled, y_scaled, z_scaled) + # Draw the trajectory on PyBullet's GUI + draw_trajectory(initial_info, waypoints, x_scaled, y_scaled, z_scaled) + + spline_waypoints = np.dstack((x_scaled, y_scaled, z_scaled))[0] + is_collision, waypoints = update_waypoints_avoid_obstacles(spline_waypoints, waypoints, self.NOMINAL_OBSTACLES, + initial_info) ### S-curve ### sf = pathlength From 51fac0884dc983ced0bd98b1c459d8e71d8ff765 Mon Sep 17 00:00:00 2001 From: huiyulhy Date: Sat, 15 Oct 2022 23:31:12 -0700 Subject: [PATCH 21/30] Fix indexing and change to single collision checking --- competition/custom_utils.py | 22 ++++++++++++++++++++-- competition/edit_this.py | 2 +- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index 5ba99596f..a42d98836 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -98,10 +98,12 @@ def is_intersect(waypoints, obstacle, low, high): [x,y] = obstacle[0:2] x_min, x_max = x + low, x + high y_min, y_max = y + low, y + high + print(waypoints.shape) for idx, waypoint in enumerate(waypoints): [x, y] = waypoint[0:2] # print((x,y), obstacle, (x_min, y_min), (x_max, y_max)) if x_min <= x and x <= x_max and y_min <= y and y <= y_max: + print(idx) return True, idx return False, None @@ -119,6 +121,22 @@ def project_point(spline_waypoint, obstacle, distance): print("new_waypoint: ", new_waypoint) return new_waypoint +# Get nearest path segment (in x y plane) +def get_nearest_path_segment(new_waypoint, waypoints): + if(len(waypoints) < 2): + return 0.0 + min_dist_to_path = np.Inf + min_idx = 0 + for idx in range(len(waypoints)-1): + vec = np.array(waypoints[idx+1] - waypoints[idx]) + main_vec = new_waypoint - waypoints[idx] + vec[2] = main_vec[2] = 0.0 + dist_to_path = np.dot(vec, main_vec) * (1/np.linalg.norm(vec)) + if(dist_to_path < min_dist_to_path): + min_dist_to_path = dist_to_path + min_idx = idx + return min_idx+1 + def update_waypoints_avoid_obstacles(spline_waypoints, waypoints, obstacles, initial_info): is_collision = False @@ -140,7 +158,7 @@ def update_waypoints_avoid_obstacles(spline_waypoints, waypoints, obstacles, ini print("Collision!") dist = max(-low, high) + 0.5 new_waypoint = project_point(spline_waypoints[idx], obstacle, dist) - # TODO: Fix indexing!! - waypoints = np.insert(waypoints, 1, new_waypoint, axis=0) + insertion_idx = get_nearest_path_segment(new_waypoint, waypoints) + waypoints = np.insert(waypoints, insertion_idx, new_waypoint, axis=0) return is_collision, waypoints diff --git a/competition/edit_this.py b/competition/edit_this.py index 6ab6fad5c..48a32815d 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -206,7 +206,7 @@ def __init__(self, # dyaw = dyaw_next is_collision = True - while is_collision: + if is_collision: x_coeffs = np.zeros((6,len(waypoints)-1)) y_coeffs = np.zeros((6,len(waypoints)-1)) z_coeffs = np.zeros((6,len(waypoints)-1)) From fdbdc784b0272cd3ee0ab41583a6b4546a207ecc Mon Sep 17 00:00:00 2001 From: huiyulhy Date: Sat, 15 Oct 2022 23:56:23 -0700 Subject: [PATCH 22/30] Move collision checking out of verbose --- competition/custom_utils.py | 2 +- competition/edit_this.py | 49 ++++++++++++++++++------------------- 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index a42d98836..0718e44e5 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -98,7 +98,6 @@ def is_intersect(waypoints, obstacle, low, high): [x,y] = obstacle[0:2] x_min, x_max = x + low, x + high y_min, y_max = y + low, y + high - print(waypoints.shape) for idx, waypoint in enumerate(waypoints): [x, y] = waypoint[0:2] # print((x,y), obstacle, (x_min, y_min), (x_max, y_max)) @@ -135,6 +134,7 @@ def get_nearest_path_segment(new_waypoint, waypoints): if(dist_to_path < min_dist_to_path): min_dist_to_path = dist_to_path min_idx = idx + print(idx) return min_idx+1 def update_waypoints_avoid_obstacles(spline_waypoints, waypoints, obstacles, initial_info): diff --git a/competition/edit_this.py b/competition/edit_this.py index 48a32815d..51e5e8d27 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -115,8 +115,8 @@ def __init__(self, # Kinematic limits # TODO determine better estimates from model - v_max = 1.5 - a_max = .5 + v_max = 2.0 + a_max = 0.75 j_max = 2 # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve self.grad_scale = .4 * v_max @@ -256,26 +256,29 @@ def __init__(self, 0, self.ts[idx+1] - self.ts[idx])[0] self.scaling_factor = self.ts[-1] / pathlength + duration = self.ts[-1] - self.ts[0] + t_scaled = np.linspace(self.ts[0], self.ts[-1], int(duration*self.CTRL_FREQ)) + t_diff_scaled = np.zeros(t_scaled.shape) + gate_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) + gate = 0 + for i, t in enumerate(t_scaled): + if gate < self.length and t > self.ts[gate+1]: + gate += 1 + t_diff_scaled[i] = t_scaled[i] - self.ts[gate] + gate_scaled[i] = gate + x_scaled = np.array(tuple(map( + lambda idx, t: utils.f(x_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + y_scaled = np.array(tuple(map( + lambda idx, t: utils.f(y_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + z_scaled = np.array(tuple(map( + lambda idx, t: utils.f(z_coeffs, idx, t), + gate_scaled, t_diff_scaled))) + spline_waypoints = np.dstack((x_scaled, y_scaled, z_scaled))[0] + is_collision, waypoints = update_waypoints_avoid_obstacles(spline_waypoints, waypoints, self.NOMINAL_OBSTACLES, + initial_info) if self.VERBOSE: - duration = self.ts[-1] - self.ts[0] - t_scaled = np.linspace(self.ts[0], self.ts[-1], int(duration*self.CTRL_FREQ)) - t_diff_scaled = np.zeros(t_scaled.shape) - gate_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) - gate = 0 - for i, t in enumerate(t_scaled): - if gate < self.length and t > self.ts[gate+1]: - gate += 1 - t_diff_scaled[i] = t_scaled[i] - self.ts[gate] - gate_scaled[i] = gate - x_scaled = np.array(tuple(map( - lambda idx, t: utils.f(x_coeffs, idx, t), - gate_scaled, t_diff_scaled))) - y_scaled = np.array(tuple(map( - lambda idx, t: utils.f(y_coeffs, idx, t), - gate_scaled, t_diff_scaled))) - z_scaled = np.array(tuple(map( - lambda idx, t: utils.f(z_coeffs, idx, t), - gate_scaled, t_diff_scaled))) print(x_coeffs) print(y_coeffs) print(z_coeffs) @@ -285,10 +288,6 @@ def __init__(self, # Draw the trajectory on PyBullet's GUI draw_trajectory(initial_info, waypoints, x_scaled, y_scaled, z_scaled) - spline_waypoints = np.dstack((x_scaled, y_scaled, z_scaled))[0] - is_collision, waypoints = update_waypoints_avoid_obstacles(spline_waypoints, waypoints, self.NOMINAL_OBSTACLES, - initial_info) - ### S-curve ### sf = pathlength From c3b63d754aaf70a6ce643b7adfe2c37b8f4d4461 Mon Sep 17 00:00:00 2001 From: huiyulhy Date: Sun, 16 Oct 2022 00:00:23 -0700 Subject: [PATCH 23/30] revert motion profile --- competition/custom_utils.py | 1 - competition/edit_this.py | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index 0718e44e5..b24c74dca 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -102,7 +102,6 @@ def is_intersect(waypoints, obstacle, low, high): [x, y] = waypoint[0:2] # print((x,y), obstacle, (x_min, y_min), (x_max, y_max)) if x_min <= x and x <= x_max and y_min <= y and y <= y_max: - print(idx) return True, idx return False, None diff --git a/competition/edit_this.py b/competition/edit_this.py index 51e5e8d27..3e18f05c3 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -115,8 +115,8 @@ def __init__(self, # Kinematic limits # TODO determine better estimates from model - v_max = 2.0 - a_max = 0.75 + v_max = 1.5 + a_max = 0.5 j_max = 2 # Affect curve radius around waypoints, higher value means larger curve, smaller value means tighter curve self.grad_scale = .4 * v_max From 516b7f4c8b479507629b93659c1231c7f78daf69 Mon Sep 17 00:00:00 2001 From: Hashir Zahir Date: Sun, 16 Oct 2022 01:08:46 -0700 Subject: [PATCH 24/30] add obstacle yaml --- competition/levelh.yaml | 182 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 182 insertions(+) create mode 100644 competition/levelh.yaml diff --git a/competition/levelh.yaml b/competition/levelh.yaml new file mode 100644 index 000000000..7396ddacf --- /dev/null +++ b/competition/levelh.yaml @@ -0,0 +1,182 @@ +# Level 0 + +# | Evaluation Scenario | Constraints | Rand. Inertial Properties | Randomized Obstacles, Gates | Rand. Between Episodes | Notes | +# | :-----------------: | :----------: | :-----------------------: | :-------------------------: | :--------------------: | :---------------: | +# | `level0.yaml` | **Yes** | *No* | *No* | *No* | Perfect knowledge | + +# Configuration +num_episodes: 2 # Customizable number of episodes (NOTE: solutions that require fewer episode will receive higher scores) +use_firmware: True # Whether to use pycffirmware or not (NOTE: only set to False for debugging purposes) +verbose: True # Boolean passed to the Controller class, can be used to turn on and off additional printouts + +# Note: DSL's flyable/usable space in Toronto extends from -3 to +3 meters in x and y and 0 and 2 meters in z +# Thus, only scenarios that meet these constraints are suitable for sim2real experiments + +# Environment +quadrotor_config: + seed: 2222 # Random seed + reseed_on_reset: True # Whether to re-seed the random number generator between episodes + gui: True # Whether to spawn PyBullet's GUI + camera_view: [5, -40, -40, 0.5, -1, 0.5] # Distance, yaw, pitch, x, y, z target position + + ctrl_freq: 30 # Frequency of the controller's decision making + pyb_freq: 500 # If `use_firmware` == True, must be multiple of 500, otherwise it must be multiple of ctrl_freq + episode_len_sec: 33 # Maximum episode duration, in seconds + + done_on_violation: True # Whether `done` becomes True when a constraint is violated + done_on_completion: True # Whether `done` becomes True when the task is completed (traveled all gates and 2'' hover at stabilization_goal) + done_on_collision: True # Whether `done` becomes True when a collision happens + cost: competition # Sparse reward function, if you desire, re-implement it in Quadrotor._get_reward() + # github.com/utiasDSL/safe-control-gym/blob/beta-iros-competition/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py + + task: stabilization + task_info: + stabilization_goal: [-0.5, 2.9, 0.75] # Final goal (target position in x-y-z) + stabilization_goal_tolerance: 0.15 + + # Nominal (pre-randomization) initial state + init_state: + init_x: -0.9 + init_x_dot: 0 + init_y: -2.9 + init_y_dot: 0 + init_z: 0.03 + init_z_dot: 0 + init_phi: 0 + init_theta: 0 + init_psi: 0 + init_p: 0 + init_q: 0 + init_r: 0 + + # Nominal (pre-randomization) positions of gates and obstacles + gates: + [ # x, y, z, r, p, y, type (0: `tall` obstacle, 1: `low` obstacle) + [0.5, -2.45, 0, 0, 0, -1.57, 0], + [2, -1.5, 0, 0, 0, 0, 1], + [0, 0.2, 0, 0, 0, 1.57, 1], + [-0.5, 1.5, 0, 0, 0, 0, 0] + ] + obstacles: + [ # x, y, z, r, p, y + [0, -2.5, 0, 0, 0, 0], + [1, -2, 0, 0, 0, 0], + [1.5, 0, 0, 0, 0, 0], + [-1, 0, 0, 0, 0, 0] + ] + + # Randomization of the initial state + randomized_init: False + # init_state_randomization_info: + # init_x: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + # init_y: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + # init_z: + # distrib: "uniform" + # low: 0.0 + # high: 0.02 + # init_phi: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + # init_theta: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + # init_psi: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + + # Randomization of the quadrotor inertial properties + randomized_inertial_prop: False + # inertial_prop_randomization_info: + # M: + # distrib: "uniform" + # low: -0.01 + # high: 0.01 + # Ixx: + # distrib: "uniform" + # low: -0.000001 + # high: 0.000001 + # Iyy: + # distrib: "uniform" + # low: -0.000001 + # high: 0.000001 + # Izz: + # distrib: "uniform" + # low: -0.000001 + # high: 0.000001 + + # Randomization of the gates and obstacles positions + randomized_gates_and_obstacles: False + # gates_and_obstacles_randomization_info: + # gates: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + # obstacles: + # distrib: "uniform" + # low: -0.1 + # high: 0.1 + + # Constraints + constraints: + # Input constraints + - constraint_form: default_constraint + constrained_variable: input + # State constraints + - constraint_form: bounded_constraint + constrained_variable: state + active_dims: [0, 2, 4] # The uncommented entries in the upper_bounds/lower_bounds vectors below + upper_bounds: + - 3 # x + # - 100 # x_dot + - 3 # y + # - 100 # y_dot + - 2 # z + # - 100 # z_dot + # - 3 # phi + # - 3 # theta + # - 3 # psi + # - 10 # p + # - 10 # q + # - 10 # r + lower_bounds: + - -3 # x + # - -100 # x_dot + - -3 # y + # - -100 # y_dot + - -0.1 # z + # - -100 # z_dot + # - -3 # phi + # - -3 # theta + # - -3 # psi + # - -10 # p + # - -10 # q + # - -10 # r + + # Disturbances + # disturbances: + # # Additive noise on the commanded input action + # action: + # - disturbance_func: white_noise + # std: 0.001 + # # Random external force applied to the quadrotor (e.g. a push or wind) + # dynamics: + # - disturbance_func: uniform + # low: [-0.1, -0.1, -0.1] + # high: [0.1, 0.1, 0.1] + + + + # Do NOT edit + quad_type: 3 # 3D quadrotor + info_in_reset: True # Info dictionary as second return value (after the initial observation) of env.reset() + normalized_rl_action_space: False + done_on_out_of_bound: False From fa1fd67db77d1c0a24e162f79e856b11f08d3a99 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 16 Oct 2022 20:00:56 +0800 Subject: [PATCH 25/30] SImplify poly functions --- competition/custom_utils.py | 36 ++++++++++++++++++------------------ competition/edit_this.py | 32 ++++++++++++++++---------------- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index b24c74dca..de7ae6f4f 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -9,52 +9,52 @@ def cubic_interp(inv_t, x0, xf, dx0, dxf): dx = xf - x0 - return ( + return np.flip(( x0, dx0, (3*dx*inv_t - 2*dx0 - dxf)*inv_t, - (-2*dx*inv_t + (dxf + dx0))*inv_t*inv_t) + (-2*dx*inv_t + (dxf + dx0))*inv_t*inv_t)) def quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf): dx = xf - x0 - return ( + return np.flip(( x0, dx0, d2x0*0.5, ((10*dx*inv_t - (4*dxf+6*dx0))*inv_t - 0.5*(3*d2x0-d2xf))*inv_t, ((-15*dx*inv_t + (7*dxf+8*dx0))*inv_t + 0.5*(3*d2x0-2*d2xf))*inv_t*inv_t, - ((6*dx*inv_t - 3*(dxf+dx0))*inv_t - 0.5*(d2x0-d2xf))*inv_t*inv_t*inv_t) + ((6*dx*inv_t - 3*(dxf+dx0))*inv_t - 0.5*(d2x0-d2xf))*inv_t*inv_t*inv_t)) -def f(coeffs, idx, t): +def f(coeffs, t): val = 0 - for coeff in coeffs[:,idx]: + for coeff in coeffs: val *= t val += coeff return val -def df(coeffs, idx, t): - coeffs_no = len(coeffs[:,idx]) +def df(coeffs, t): + coeffs_no = len(coeffs) val = 0 - for i, coeff in enumerate(coeffs[:-1,idx]): + for i, coeff in enumerate(coeffs[:-1]): val *= t factor = (coeffs_no - i - 1) val += coeff * factor return val -def d2f(coeffs, idx, t): - coeffs_no = len(coeffs[:,idx]) +def d2f(coeffs, t): + coeffs_no = len(coeffs) val = 0 - for i, coeff in enumerate(coeffs[:-2,idx]): + for i, coeff in enumerate(coeffs[:-2]): val *= t factor = (coeffs_no - i - 1) val += coeff * factor * (factor - 1) return val -def d3f(coeffs, idx, t): - coeffs_no = len(coeffs[:,idx]) +def d3f(coeffs, t): + coeffs_no = len(coeffs) val = 0 - for i, coeff in enumerate(coeffs[:-3,idx]): + for i, coeff in enumerate(coeffs[:-3]): val *= t factor = (coeffs_no - i - 1) val += coeff * factor * (factor - 1) * (factor - 2) @@ -66,9 +66,9 @@ def df_(t): if (ts[idx+1] > t): break t -= ts[idx] - dx = df(x_coeffs, idx, t) - dy = df(y_coeffs, idx, t) - dz = df(z_coeffs, idx, t) + dx = df(x_coeffs[:, idx], t) + dy = df(y_coeffs[:, idx], t) + dz = df(z_coeffs[:, idx], t) return sqrt(dx*dx+dy*dy+dz*dz) return df_ diff --git a/competition/edit_this.py b/competition/edit_this.py index 3e18f05c3..366370d02 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -230,9 +230,9 @@ def __init__(self, if idx == self.length-1: # yaw of last goal not important dxf = dx0 * 0.01 dyf = dy0 * 0.01 - x_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) - y_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) - z_coeffs[::-1,idx-1] = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) + x_coeffs[:,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) + y_coeffs[:,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) + z_coeffs[:,idx-1] = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) [x0, y0, z0] = [xf, yf, zf] [dx0, dy0, dz0] = [dxf, dyf, dzf] [d2x0, d2y0, d2z0] = [d2xf, d2yf, d2zf] @@ -240,7 +240,7 @@ def __init__(self, # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c # modify endpoint gradient for smooth z ending (pchip iterpolator) - # z_coeffs[::-1,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), + # z_coeffs[:,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), # waypoints[-2,2], # waypoints[-1,2], # z_coeffs[-2,-1], @@ -267,13 +267,13 @@ def __init__(self, t_diff_scaled[i] = t_scaled[i] - self.ts[gate] gate_scaled[i] = gate x_scaled = np.array(tuple(map( - lambda idx, t: utils.f(x_coeffs, idx, t), + lambda idx, t: utils.f(x_coeffs[:, idx], t), gate_scaled, t_diff_scaled))) y_scaled = np.array(tuple(map( - lambda idx, t: utils.f(y_coeffs, idx, t), + lambda idx, t: utils.f(y_coeffs[:, idx], t), gate_scaled, t_diff_scaled))) z_scaled = np.array(tuple(map( - lambda idx, t: utils.f(z_coeffs, idx, t), + lambda idx, t: utils.f(z_coeffs[:, idx], t), gate_scaled, t_diff_scaled))) spline_waypoints = np.dstack((x_scaled, y_scaled, z_scaled))[0] is_collision, waypoints = update_waypoints_avoid_obstacles(spline_waypoints, waypoints, self.NOMINAL_OBSTACLES, @@ -388,7 +388,7 @@ def cmdFirmware(self, dy_gate = sin(yaw) * self.grad_scale dz_gate = 0 for i, (val, dval) in enumerate(zip([x,y,z], [dx_gate, dy_gate, dz_gate])): - self.run_coeffs[i] = np.insert(self.run_coeffs[i], self.gate_no+1, np.flip(utils.quintic_interp( + self.run_coeffs[i] = np.insert(self.run_coeffs[i], self.gate_no+1, utils.quintic_interp( inv_t, self.target_pose[i], val, @@ -396,16 +396,16 @@ def cmdFirmware(self, dval, self.dtangent[i], 0 - )), axis=1) + ), axis=1) dt = self.run_ts[self.gate_no + 2] - self.run_ts[self.gate_no + 1] inv_t = 1/dt for i, (val, dval) in enumerate(zip([x,y,z], [dx_gate, dy_gate, dz_gate])): - self.run_coeffs[i][::-1,self.gate_no+2] = utils.quintic_interp( + self.run_coeffs[i][:,self.gate_no+2] = utils.quintic_interp( inv_t, val, - utils.f(self.run_coeffs[i], self.gate_no+2, dt), + utils.f(self.run_coeffs[i][:, self.gate_no+2], dt), dval, - utils.df(self.run_coeffs[i], self.gate_no+2, dt), + utils.df(self.run_coeffs[i][:, self.gate_no+2], dt), 0, 0 ) @@ -429,10 +429,10 @@ def cmdFirmware(self, t = curve_t - self.run_ts[self.gate_no] - self.target_pose = np.array([utils.f(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) - self.tangent = np.array([utils.df(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) - self.dtangent = np.array([utils.d2f(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) - self.d2tangent = np.array([utils.d3f(coeffs, self.gate_no, t) for coeffs in self.run_coeffs]) + self.target_pose = np.array([utils.f(coeffs[:, self.gate_no], t) for coeffs in self.run_coeffs]) + self.tangent = np.array([utils.df(coeffs[:, self.gate_no], t) for coeffs in self.run_coeffs]) + self.dtangent = np.array([utils.d2f(coeffs[:, self.gate_no], t) for coeffs in self.run_coeffs]) + self.d2tangent = np.array([utils.d3f(coeffs[:, self.gate_no], t) for coeffs in self.run_coeffs]) error = np.array((obs[0] - self.target_pose[0], obs[2] - self.target_pose[1], obs[4] - self.target_pose[2])) # positional error (world frame) # print(error) From 849b9ba00ba4512524a157ec897c812d8f6022e8 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 16 Oct 2022 22:56:25 +0800 Subject: [PATCH 26/30] Obstacle avoidance solved from polynomial --- competition/custom_utils.py | 48 +++++++++ competition/edit_this.py | 208 ++++++++++++++++++++++-------------- 2 files changed, 177 insertions(+), 79 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index de7ae6f4f..d8431a6fd 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -161,3 +161,51 @@ def update_waypoints_avoid_obstacles(spline_waypoints, waypoints, obstacles, ini waypoints = np.insert(waypoints, insertion_idx, new_waypoint, axis=0) return is_collision, waypoints + +def check_intersect_poly(x_coeff, y_coeff, dt, x, y, r): + x_poly = np.copy(x_coeff) + x_poly[-1] -= x + y_poly = np.copy(y_coeff) + y_poly[-1] -= y + LHS = np.polyadd(np.polymul(x_poly, x_poly), np.polymul(y_poly, y_poly)) + dLHS = np.polyder(LHS) + dLHS_roots = np.real_if_close(np.roots(dLHS)) + tolerance = 0.1 # magic number to say how near we can be to the obs. includes drone size as well + r += tolerance + r_2 = r*r + + selected = [] + for root in dLHS_roots: + if root.imag: + continue + root = root.real + if root < 0 or root > dt: + continue + LHS_curr = np.polyval(LHS, root) + if LHS_curr > r_2: + continue + dist = sqrt(LHS_curr) + selected.append((root, dist)) + + if not selected: + # no collision + return + + projected_points = [] + dx_coeff = np.polyder(x_coeff) + dy_coeff = np.polyder(y_coeff) + d2x_coeff = np.polyder(dx_coeff) + d2y_coeff = np.polyder(dy_coeff) + for t, dist in selected: + spline_x = np.polyval(x_coeff, t) + spline_y = np.polyval(y_coeff, t) + spline_dx = np.polyval(dx_coeff, t) + spline_dy = np.polyval(dy_coeff, t) + spline_d2x = np.polyval(d2x_coeff, t) + spline_d2y = np.polyval(d2y_coeff, t) + scale = (r + 0.0) / dist + # print("scale", scale) + projected_x = x + (spline_x - x) * scale + projected_y = y + (spline_y - y) * scale + projected_points.append((t, (projected_x, spline_dx, spline_d2x),(projected_y, spline_dy, spline_d2y))) + return projected_points \ No newline at end of file diff --git a/competition/edit_this.py b/competition/edit_this.py index 366370d02..047a1adef 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -142,22 +142,22 @@ def __init__(self, # "time" for each waypoint # time interval determined by eulcidean distance between waypoints along xyz plane - self.ts = np.zeros(self.length) + ts = np.zeros(self.length) [x_prev, y_prev, z_prev, _] = waypoints[0] for idx in range(1,self.length): [x_curr, y_curr, z_curr, _] = waypoints[idx] norm = sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2 + (z_curr-z_prev)**2) - self.ts[idx] = self.ts[idx-1] + norm / v_max + ts[idx] = ts[idx-1] + norm / v_max # Flip gates # Selectively flip gate orientation based on vector from previous and to next waypoint, # and current gate's orientation [x_prev, y_prev, _, _] = waypoints[0] [x_curr, y_curr, _, yaw_curr] = waypoints[1] - dt = self.ts[1] - self.ts[0] + dt = ts[1] - ts[0] for idx in range(1,self.length-1): [x_next, y_next, _, yaw_next] = waypoints[idx+1] - dt_next = self.ts[idx+1] - self.ts[idx] + dt_next = ts[idx+1] - ts[idx] dxf = cos(yaw_curr) * self.grad_scale dyf = sin(yaw_curr) * self.grad_scale vx1 = np.array((x_curr - x_prev,dt)) @@ -205,88 +205,138 @@ def __init__(self, # yaw_curr = yaw_next # dyaw = dyaw_next - is_collision = True - if is_collision: - x_coeffs = np.zeros((6,len(waypoints)-1)) - y_coeffs = np.zeros((6,len(waypoints)-1)) - z_coeffs = np.zeros((6,len(waypoints)-1)) - [x0, y0, z0, yaw0] = waypoints[0] - dx0 = cos(yaw0)*0.01 - dy0 = sin(yaw0)*0.01 - dz0 = v_max - dzf = 0 - d2x0 = 0 - d2y0 = 0 - d2z0 = a_max - d2xf = 0 - d2yf = 0 - d2zf = 0 - for idx in range(1, self.length): - [xf, yf, zf, yawf] = waypoints[idx] - dt = self.ts[idx] - self.ts[idx - 1] - inv_t = 1/dt - dxf = cos(yawf) * self.grad_scale - dyf = sin(yawf) * self.grad_scale - if idx == self.length-1: # yaw of last goal not important - dxf = dx0 * 0.01 - dyf = dy0 * 0.01 - x_coeffs[:,idx-1] = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) - y_coeffs[:,idx-1] = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) - z_coeffs[:,idx-1] = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) - [x0, y0, z0] = [xf, yf, zf] - [dx0, dy0, dz0] = [dxf, dyf, dzf] - [d2x0, d2y0, d2z0] = [d2xf, d2yf, d2zf] - waypoints = np.array(waypoints) - # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c - - # modify endpoint gradient for smooth z ending (pchip iterpolator) - # z_coeffs[:,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), - # waypoints[-2,2], - # waypoints[-1,2], - # z_coeffs[-2,-1], - # 0 - # ) - self.coeffs = [x_coeffs, y_coeffs, z_coeffs] - - # Integrate to get pathlength - pathlength = 0 - for idx in range(self.length-1): - pathlength += scipy.integrate.quad( - utils.df_idx(self.length, self.ts, x_coeffs, y_coeffs, z_coeffs), - 0, self.ts[idx+1] - self.ts[idx])[0] - self.scaling_factor = self.ts[-1] / pathlength - + # Obstacle radius (including randomness) + r = initial_info['obstacle_dimensions']['radius'] + obstacle_distrib_dict = initial_info['gates_and_obs_randomization'] + if 'obstacles' in obstacle_distrib_dict: + r += max(abs(obstacle_distrib_dict['obstacles']['low']), abs(obstacle_distrib_dict['obstacles']['high'])) + + x_coeffs = [] + y_coeffs = [] + z_coeffs = [] + [x0, y0, z0, yaw0] = waypoints[0] + dx0 = cos(yaw0)*0.01 + dy0 = sin(yaw0)*0.01 + dz0 = v_max + dzf = 0 + d2x0 = 0 + d2y0 = 0 + d2z0 = a_max + d2xf = 0 + d2yf = 0 + d2zf = 0 + self.ts = [0,] + for idx in range(1, self.length): + [xf, yf, zf, yawf] = waypoints[idx] + dt = ts[idx] - ts[idx - 1] + inv_t = 1/dt + dxf = cos(yawf) * self.grad_scale + dyf = sin(yawf) * self.grad_scale + if idx == self.length-1: # yaw of last goal not important + dxf = dx0 * 0.01 + dyf = dy0 * 0.01 + x_coeff = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) + y_coeff = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) + z_coeff = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) + + # Obstacle avoidance + projected_points = [] + for obstacle in self.NOMINAL_OBSTACLES: + x = obstacle[0] + y = obstacle[1] + projected_point = utils.check_intersect_poly(x_coeff, y_coeff, dt, x, y, r) + if projected_point: + print("projected points", projected_point) + projected_points += projected_point + if projected_points: + projected_points.sort(key = lambda x: x[0]) + prev_t = 0 + for t, (x, dx, d2x), (y, dy, d2y) in projected_points: + z = utils.f(z_coeff, t) + dz = utils.df(z_coeff, t) + d2z = utils.d2f(z_coeff, t) + inv_t = 1/(t - prev_t) + x_coeff = utils.quintic_interp(inv_t, x0, x, dx0, dx, d2x0, d2x) + y_coeff = utils.quintic_interp(inv_t, y0, y, dy0, dy, d2y0, d2y) + z_coeff = utils.quintic_interp(inv_t, z0, z, dz0, dz, d2z0, d2z) + x_coeffs.append(x_coeff) + y_coeffs.append(y_coeff) + z_coeffs.append(z_coeff) + self.ts.append(ts[idx - 1] + t) + + [x0, y0, z0] = [x, y, z] + [dx0, dy0, dz0] = [dx, dy, dz] + [d2x0, d2y0, d2z0] = [d2x, d2y, d2z] + prev_t = t + inv_t = 1/(ts[idx] - ts[idx - 1] - prev_t) + x_coeff = utils.quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf) + y_coeff = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) + z_coeff = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) + x_coeffs.append(x_coeff) + y_coeffs.append(y_coeff) + z_coeffs.append(z_coeff) + self.ts.append(ts[idx]) + + [x0, y0, z0] = [xf, yf, zf] + [dx0, dy0, dz0] = [dxf, dyf, dzf] + [d2x0, d2y0, d2z0] = [d2xf, d2yf, d2zf] + x_coeffs = np.array(x_coeffs).transpose() + y_coeffs = np.array(y_coeffs).transpose() + z_coeffs = np.array(z_coeffs).transpose() + waypoints = np.array(waypoints) + self.length = len(self.ts) + # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c + + # modify endpoint gradient for smooth z ending (pchip iterpolator) + # z_coeffs[:,-1] = utils.cubic_interp(1/(self.ts[-1]-self.ts[-2]), + # waypoints[-2,2], + # waypoints[-1,2], + # z_coeffs[-2,-1], + # 0 + # ) + self.coeffs = [x_coeffs, y_coeffs, z_coeffs] + + # Integrate to get pathlength + pathlength = 0 + for idx in range(self.length-1): + pathlength += scipy.integrate.quad( + utils.df_idx(self.length, self.ts, x_coeffs, y_coeffs, z_coeffs), + 0, self.ts[idx+1] - self.ts[idx])[0] + self.scaling_factor = self.ts[-1] / pathlength + + if self.VERBOSE: duration = self.ts[-1] - self.ts[0] t_scaled = np.linspace(self.ts[0], self.ts[-1], int(duration*self.CTRL_FREQ)) t_diff_scaled = np.zeros(t_scaled.shape) - gate_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) - gate = 0 + waypoint_scaled = np.zeros(t_scaled.shape, dtype=np.ushort) + waypoint = 0 + print(self.ts) + print(x_coeffs) + print(y_coeffs) + print(z_coeffs) + print(waypoints) for i, t in enumerate(t_scaled): - if gate < self.length and t > self.ts[gate+1]: - gate += 1 - t_diff_scaled[i] = t_scaled[i] - self.ts[gate] - gate_scaled[i] = gate + if waypoint < self.length and t > self.ts[waypoint+1]: + waypoint += 1 + t_diff_scaled[i] = t_scaled[i] - self.ts[waypoint] + waypoint_scaled[i] = waypoint x_scaled = np.array(tuple(map( lambda idx, t: utils.f(x_coeffs[:, idx], t), - gate_scaled, t_diff_scaled))) + waypoint_scaled, t_diff_scaled))) y_scaled = np.array(tuple(map( lambda idx, t: utils.f(y_coeffs[:, idx], t), - gate_scaled, t_diff_scaled))) + waypoint_scaled, t_diff_scaled))) z_scaled = np.array(tuple(map( lambda idx, t: utils.f(z_coeffs[:, idx], t), - gate_scaled, t_diff_scaled))) - spline_waypoints = np.dstack((x_scaled, y_scaled, z_scaled))[0] - is_collision, waypoints = update_waypoints_avoid_obstacles(spline_waypoints, waypoints, self.NOMINAL_OBSTACLES, - initial_info) - if self.VERBOSE: - print(x_coeffs) - print(y_coeffs) - print(z_coeffs) - print(waypoints) - # Plot trajectory in each dimension and 3D. - plot_trajectory(t_scaled, waypoints, x_scaled, y_scaled, z_scaled) - # Draw the trajectory on PyBullet's GUI - draw_trajectory(initial_info, waypoints, x_scaled, y_scaled, z_scaled) + waypoint_scaled, t_diff_scaled))) + # Plot trajectory in each dimension and 3D. + plot_trajectory(t_scaled, waypoints, x_scaled, y_scaled, z_scaled) + # Draw the trajectory on PyBullet's GUI + draw_trajectory(initial_info, waypoints, x_scaled, y_scaled, z_scaled) + + # spline_waypoints = np.dstack((x_scaled, y_scaled, z_scaled))[0] + # is_collision, waypoints = update_waypoints_avoid_obstacles(spline_waypoints, waypoints, self.NOMINAL_OBSTACLES, + # initial_info) ### S-curve ### sf = pathlength @@ -326,11 +376,9 @@ def s_curve(t_): j_ = j[i] return [s_, v_, a_, j_] self.s = s_curve - self.start_t = -1 self.end_t = t[-1] self.scaled_t = 0 self.time_scale = 1 - self.takeoff_land_duration = 2 self.at_gate = False self.gate_no = 0 self.run_coeffs = deepcopy(self.coeffs) @@ -583,12 +631,14 @@ def interEpisodeLearn(self): _ = self.info_buffer # Reset run variables + self.scaled_t = 0 + self.time_scale = 1 self.at_gate = False self.gate_no = 0 self.run_coeffs = deepcopy(self.coeffs) self.run_ts = np.copy(self.ts) - self.length = len(self.NOMINAL_GATES) + 2 + self.length = len(self.ts) ######################### # REPLACE THIS (END) #### From 9b9eab5cb29c242fa55ce08d6eeb086a8350d914 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 16 Oct 2022 22:59:28 +0800 Subject: [PATCH 27/30] Slowdown near gate --- competition/edit_this.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/competition/edit_this.py b/competition/edit_this.py index 047a1adef..f6b7b7f97 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -429,6 +429,12 @@ def cmdFirmware(self, # Local replan when near to goal self.at_gate = True [x, y, z, _, _, yaw] = info['current_target_gate_pos'] + prev_x = self.run_coeffs[0][-1,self.gate_no+1] + prev_y = self.run_coeffs[1][-1,self.gate_no+1] + prev_z = self.run_coeffs[2][-1,self.gate_no+1] + error = sqrt((x-prev_x)**2 + (y-prev_y)**2 + (z-prev_z)**2) + self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.05 + self.time_scale = max(0.0, min(1.0, self.time_scale)) yaw += self.half_pi dt = self.run_ts[self.gate_no + 1] - self.curve_t inv_t = 1/dt From 2f8460fd7b2b2abb5c273811ac16c3675b8f3a41 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 16 Oct 2022 23:06:14 +0800 Subject: [PATCH 28/30] Use numpy for polynomial evaluation --- competition/custom_utils.py | 30 ++++-------------------------- 1 file changed, 4 insertions(+), 26 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index d8431a6fd..bcdfed037 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -27,38 +27,16 @@ def quintic_interp(inv_t, x0, xf, dx0, dxf, d2x0, d2xf): def f(coeffs, t): - val = 0 - for coeff in coeffs: - val *= t - val += coeff - return val + return np.polyval(coeffs, t) def df(coeffs, t): - coeffs_no = len(coeffs) - val = 0 - for i, coeff in enumerate(coeffs[:-1]): - val *= t - factor = (coeffs_no - i - 1) - val += coeff * factor - return val + return np.polyval(np.polyder(coeffs), t) def d2f(coeffs, t): - coeffs_no = len(coeffs) - val = 0 - for i, coeff in enumerate(coeffs[:-2]): - val *= t - factor = (coeffs_no - i - 1) - val += coeff * factor * (factor - 1) - return val + return np.polyval(np.polyder(coeffs,2), t) def d3f(coeffs, t): - coeffs_no = len(coeffs) - val = 0 - for i, coeff in enumerate(coeffs[:-3]): - val *= t - factor = (coeffs_no - i - 1) - val += coeff * factor * (factor - 1) * (factor - 2) - return val + return np.polyval(np.polyder(coeffs,3), t) def df_idx(length, ts, x_coeffs, y_coeffs, z_coeffs): def df_(t): From 8a457c965b85541e0efc333d938f7f028ef78693 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 16 Oct 2022 23:16:06 +0800 Subject: [PATCH 29/30] Comments and slight changes --- competition/custom_utils.py | 28 ++++++++++++++-------------- competition/edit_this.py | 10 +++++++--- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/competition/custom_utils.py b/competition/custom_utils.py index bcdfed037..1b709a781 100644 --- a/competition/custom_utils.py +++ b/competition/custom_utils.py @@ -141,17 +141,20 @@ def update_waypoints_avoid_obstacles(spline_waypoints, waypoints, obstacles, ini return is_collision, waypoints def check_intersect_poly(x_coeff, y_coeff, dt, x, y, r): + # Function describing distance between spline and point (x,y) x_poly = np.copy(x_coeff) x_poly[-1] -= x y_poly = np.copy(y_coeff) y_poly[-1] -= y LHS = np.polyadd(np.polymul(x_poly, x_poly), np.polymul(y_poly, y_poly)) + + # Find stationary points dLHS = np.polyder(LHS) dLHS_roots = np.real_if_close(np.roots(dLHS)) - tolerance = 0.1 # magic number to say how near we can be to the obs. includes drone size as well - r += tolerance - r_2 = r*r + # Find stationarty points that lie within radius of obstacle + tolerance = 0.1 # parameter to say how near we can be to the obs. includes drone size as well + r_2 = (r + tolerance)**2 selected = [] for root in dLHS_roots: if root.imag: @@ -169,19 +172,16 @@ def check_intersect_poly(x_coeff, y_coeff, dt, x, y, r): # no collision return + # Collect all collisions projected_points = [] - dx_coeff = np.polyder(x_coeff) - dy_coeff = np.polyder(y_coeff) - d2x_coeff = np.polyder(dx_coeff) - d2y_coeff = np.polyder(dy_coeff) for t, dist in selected: - spline_x = np.polyval(x_coeff, t) - spline_y = np.polyval(y_coeff, t) - spline_dx = np.polyval(dx_coeff, t) - spline_dy = np.polyval(dy_coeff, t) - spline_d2x = np.polyval(d2x_coeff, t) - spline_d2y = np.polyval(d2y_coeff, t) - scale = (r + 0.0) / dist + spline_x = f(x_coeff, t) + spline_y = f(y_coeff, t) + spline_dx = df(x_coeff, t) + spline_dy = df(y_coeff, t) + spline_d2x = d2f(x_coeff, t) + spline_d2y = d2f(y_coeff, t) + scale = (r + 2*tolerance) / dist # print("scale", scale) projected_x = x + (spline_x - x) * scale projected_y = y + (spline_y - y) * scale diff --git a/competition/edit_this.py b/competition/edit_this.py index f6b7b7f97..2d48e926b 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -175,7 +175,7 @@ def __init__(self, choice_2 += vy1.dot(vy22)/np.linalg.norm(vy1)/np.linalg.norm(vy22)+vy22.dot(vy3)/np.linalg.norm(vy22)/np.linalg.norm(vy3) if choice_2 > choice_1: waypoints[idx][3] += pi - print("flipping", idx) + # print("flipping", idx) x_prev, y_prev = x_curr, y_curr x_curr, y_curr = x_next, y_next yaw_curr = yaw_next @@ -239,14 +239,15 @@ def __init__(self, y_coeff = utils.quintic_interp(inv_t, y0, yf, dy0, dyf, d2y0, d2yf) z_coeff = utils.quintic_interp(inv_t, z0, zf, dz0, dzf, d2z0, d2zf) - # Obstacle avoidance + # Obstacle avoidance by finding closest points on spline to obstacle + # Adds another waypoint that projects the spline outward from centre of obstacle projected_points = [] for obstacle in self.NOMINAL_OBSTACLES: x = obstacle[0] y = obstacle[1] projected_point = utils.check_intersect_poly(x_coeff, y_coeff, dt, x, y, r) if projected_point: - print("projected points", projected_point) + # print("projected points", projected_point) projected_points += projected_point if projected_points: projected_points.sort(key = lambda x: x[0]) @@ -429,12 +430,15 @@ def cmdFirmware(self, # Local replan when near to goal self.at_gate = True [x, y, z, _, _, yaw] = info['current_target_gate_pos'] + + # slow down based on distance jumped by gate prev_x = self.run_coeffs[0][-1,self.gate_no+1] prev_y = self.run_coeffs[1][-1,self.gate_no+1] prev_z = self.run_coeffs[2][-1,self.gate_no+1] error = sqrt((x-prev_x)**2 + (y-prev_y)**2 + (z-prev_z)**2) self.time_scale += -log1p(np.linalg.norm(error)-.25)*0.05 self.time_scale = max(0.0, min(1.0, self.time_scale)) + yaw += self.half_pi dt = self.run_ts[self.gate_no + 1] - self.curve_t inv_t = 1/dt From 92aafddfdbafee6492c5624148ae469690533f54 Mon Sep 17 00:00:00 2001 From: aoi3142 Date: Sun, 16 Oct 2022 23:33:15 +0800 Subject: [PATCH 30/30] Include added waypoints for visualisation --- competition/edit_this.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/competition/edit_this.py b/competition/edit_this.py index 2d48e926b..a17a07009 100644 --- a/competition/edit_this.py +++ b/competition/edit_this.py @@ -226,8 +226,11 @@ def __init__(self, d2yf = 0 d2zf = 0 self.ts = [0,] + waypoints_added = [] for idx in range(1, self.length): - [xf, yf, zf, yawf] = waypoints[idx] + waypoint = waypoints[idx] + [xf, yf, zf, yawf] = waypoint + waypoints_added.append(waypoint) dt = ts[idx] - ts[idx - 1] inv_t = 1/dt dxf = cos(yawf) * self.grad_scale @@ -264,6 +267,7 @@ def __init__(self, y_coeffs.append(y_coeff) z_coeffs.append(z_coeff) self.ts.append(ts[idx - 1] + t) + waypoints_added.append((x, y, z, 0)) [x0, y0, z0] = [x, y, z] [dx0, dy0, dz0] = [dx, dy, dz] @@ -284,7 +288,7 @@ def __init__(self, x_coeffs = np.array(x_coeffs).transpose() y_coeffs = np.array(y_coeffs).transpose() z_coeffs = np.array(z_coeffs).transpose() - waypoints = np.array(waypoints) + waypoints = np.array(waypoints_added) self.length = len(self.ts) # z_coeffs = scipy.interpolate.PchipInterpolator(self.ts, waypoints[:,2], 0).c