def __init__(self, quad, ctrlType, trajSelect):
self.ctrlType = ctrlType
self.xyzType = trajSelect[0]
self.yawType = trajSelect[1]
self.averVel = trajSelect[2]
t_wps, wps, y_wps, v_wp = makeWaypoints()
self.t_wps = t_wps
self.wps = wps
self.y_wps = y_wps
self.v_wp = v_wp
self.end_reached = 0
if (self.ctrlType == "xyz_pos"):
self.T_segment = np.diff(self.t_wps)
if (self.averVel == 1):
distance_segment = self.wps[1:] - self.wps[:-1]
self.T_segment = np.sqrt(distance_segment[:,0]**2 + distance_segment[:,1]**2 + distance_segment[:,2]**2)/self.v_wp
self.t_wps = np.zeros(len(self.T_segment) + 1)
self.t_wps[1:] = np.cumsum(self.T_segment)
if (self.yawType == 4):
self.y_wps = np.zeros(len(self.t_wps))
# Get initial heading
self.current_heading = quad.psi
# Initialize "arrived" and "omit_yaw" bool
self.arrived = 0
self.omit_yaw_follow = 0
# Initialize trajectory setpoint
self.desPos = np.zeros(3) # Desired position (x, y, z)
self.desVel = np.zeros(3) # Desired velocity (xdot, ydot, zdot)
self.desAcc = np.zeros(3) # Desired acceleration (xdotdot, ydotdot, zdotdot)
self.desThr = np.zeros(3) # Desired thrust in N-E-D directions (or E-N-U, if selected)
self.desEul = np.zeros(3) # Desired orientation in the world frame (phi, theta, psi)
self.desPQR = np.zeros(3) # Desired angular velocity in the body frame (p, q, r)
self.desYawRate = 0. # Desired yaw speed
self.sDes = np.hstack((self.desPos, self.desVel, self.desAcc, self.desThr, self.desEul, self.desPQR, self.desYawRate)).astype(float)
def __init__(self, quad, ctrlType, trajSelect, config):
self.ctrlType = ctrlType
self.xyzType = trajSelect[0]
self.yawType = trajSelect[1]
self.averVel = trajSelect[2]
t_wps, wps, y_wps, v_wp = makeWaypoints(config)
self.t_wps = t_wps
self.wps = wps
self.y_wps = y_wps
self.v_wp = v_wp
self.end_reached = 0
if (self.ctrlType == "xyz_pos"):
self.T_segment = np.diff(self.t_wps)
if (self.averVel == 1):
distance_segment = self.wps[1:] - self.wps[:-1]
self.T_segment = np.sqrt(distance_segment[:, 0]**2 +
distance_segment[:, 1]**2 +
distance_segment[:, 2]**2) / self.v_wp
self.t_wps = np.zeros(len(self.T_segment) + 1)
self.t_wps[1:] = np.cumsum(self.T_segment)
if (self.xyzType >= 3 and self.xyzType <= 6):
self.deriv_order = int(
self.xyzType - 2
) # Looking to minimize which derivative order (eg: Minimum velocity -> first order)
# Calculate coefficients
self.coeff_x = minSomethingTraj(self.wps[:, 0], self.T_segment,
self.deriv_order)
self.coeff_y = minSomethingTraj(self.wps[:, 1], self.T_segment,
self.deriv_order)
self.coeff_z = minSomethingTraj(self.wps[:, 2], self.T_segment,
self.deriv_order)
elif (self.xyzType >= 7 and self.xyzType <= 9):
self.deriv_order = int(
self.xyzType - 5
) # Looking to minimize which derivative order (eg: Minimum accel -> second order)
# Calculate coefficients
self.coeff_x = minSomethingTraj_stop(self.wps[:, 0],
self.T_segment,
self.deriv_order)
self.coeff_y = minSomethingTraj_stop(self.wps[:, 1],
self.T_segment,
self.deriv_order)
self.coeff_z = minSomethingTraj_stop(self.wps[:, 2],
self.T_segment,
self.deriv_order)
elif (self.xyzType >= 10 and self.xyzType <= 11):
self.deriv_order = int(
self.xyzType - 7
) # Looking to minimize which derivative order (eg: Minimum jerk -> third order)
# Calculate coefficients
self.coeff_x = minSomethingTraj_faststop(
self.wps[:, 0], self.T_segment, self.deriv_order)
self.coeff_y = minSomethingTraj_faststop(
self.wps[:, 1], self.T_segment, self.deriv_order)
self.coeff_z = minSomethingTraj_faststop(
self.wps[:, 2], self.T_segment, self.deriv_order)
if (self.yawType == 4):
self.y_wps = np.zeros(len(self.t_wps))
# Get initial heading
self.current_heading = quad.psi
# Initialize trajectory setpoint
self.desPos = np.zeros(3) # Desired position (x, y, z)
self.desVel = np.zeros(3) # Desired velocity (xdot, ydot, zdot)
self.desAcc = np.zeros(
3) # Desired acceleration (xdotdot, ydotdot, zdotdot)
self.desThr = np.zeros(
3) # Desired thrust in N-E-D directions (or E-N-U, if selected)
self.desEul = np.zeros(
3) # Desired orientation in the world frame (phi, theta, psi)
self.desPQR = np.zeros(
3) # Desired angular velocity in the body frame (p, q, r)
self.desYawRate = 0. # Desired yaw speed
self.sDes = np.hstack(
(self.desPos, self.desVel, self.desAcc, self.desThr, self.desEul,
self.desPQR, self.desYawRate)).astype(float)