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)