def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ # acceleration_cmd = np.array([0.0, 0.0]) if thrust_cmd <= 0.0: return np.array([0.0, 0.0]) thrust_acc = thrust_cmd / DRONE_MASS_KG R13_cmd = np.clip(acceleration_cmd[0] / thrust_acc, -MAX_ROLL_PITCH, MAX_ROLL_PITCH) R23_cmd = np.clip(acceleration_cmd[1] / thrust_acc, -MAX_ROLL_PITCH, MAX_ROLL_PITCH) R = euler2RM(attitude[0], attitude[1], attitude[2]) rot = np.array([[-R[1, 0], R[0, 0]], [-R[1, 1], R[0, 1]]]) control = np.array([self.k_p_roll * (R[0, 2] - R13_cmd), self.k_p_pitch * (R[1, 2] - R23_cmd)]) # return np.matmul(rot, control) / R[2, 2] return np.array([0.0, 0.0])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll,pitch,yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ c = -thrust_cmd / DRONE_MASS_KG R = euler2RM(*attitude) b = R[0:2, 2] b_c = acceleration_cmd / c b_err = b_c - b b_dot_c = self.k_p_euler_angles[:2][::-1] * b_err r = np.array([[R[1, 0], -R[0, 0]], [R[1, 1], -R[0, 1]]], dtype=np.float) pq_c = np.dot(r, b_dot_c) / R[2, 2] return pq_c
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ z_err = altitude_cmd - altitude z_err_dot = vertical_velocity_cmd - vertical_velocity b_z = euler2RM(*attitude)[2, 2] u_1 = self.altitude_k_p * z_err + self.altitude_k_d * z_err_dot + acceleration_ff acc = (u_1 - GRAVITY) / b_z thrust = DRONE_MASS_KG * acc if thrust > MAX_THRUST: thrust = MAX_THRUST else: if thrust < 0.: thurst = 0. return thrust
def altitude_control(self, z_c, z_dot_c, z, z_dot, attitude, z_dot_dot_c=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ # z_c = 3 # z_dot_c = 0 z_err = z_c - z z_err_dot = z_dot_c - z_dot p_term = self.kp_z * z_err d_term = self.kd_z * z_err_dot R = euler2RM(*attitude) b_z = R[2, 2] u_1_bar = p_term + d_term + z_dot_dot_c # thrust = (u_1_bar - self.g) / b_z thrust = u_1_bar * self.m / b_z return bounded_thrust(thrust)
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ b_z = euler2RM(*attitude)[2, 2] z_err = altitude_cmd - altitude z_dot_err = vertical_velocity_cmd - vertical_velocity z_dot_dot_c = self.k_p_z * z_err + self.k_d_z * z_dot_err + acceleration_ff c = (z_dot_dot_c - GRAVITY) / b_z thrust = c * DRONE_MASS_KG thrust = np.clip(thrust, 0.1, MAX_THRUST) return thrust
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=1.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) u1_bar = self.z_k_p * (altitude_cmd - altitude) + self.z_k_d * ( vertical_velocity_cmd - vertical_velocity) + acceleration_ff c = (u1_bar * DRONE_MASS_KG) / rot_mat[2, 2] if c > MAX_THRUST: c = MAX_THRUST elif c < 0.0: c = 0.0 return c
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) z_err = altitude_cmd - altitude z_err_dot = vertical_velocity_cmd - vertical_velocity b_z = rot_mat[2, 2] p_term = self.z_k_p * z_err d_term = self.z_k_d * z_err_dot u_1_bar = p_term + d_term + acceleration_ff c = (u_1_bar - self.g) / b_z return c
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ c = -thrust_cmd / DRONE_MASS_KG rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_x = rot_mat[0, 2] b_x_err = (acceleration_cmd[0] / c) - b_x b_x_p_term = self.k_p_roll * b_x_err b_y = rot_mat[1, 2] b_y_err = (acceleration_cmd[1] / c) - b_y b_y_p_term = self.k_p_pitch * b_y_err b_x_commanded_dot = b_x_p_term b_y_commanded_dot = b_y_p_term rot_mat1 = np.array([[rot_mat[1, 0], -rot_mat[0, 0]], [rot_mat[1, 1], -rot_mat[0, 1]]]) / rot_mat[2, 2] rot_rate = np.matmul( rot_mat1, np.array([b_x_commanded_dot, b_y_commanded_dot]).T) p_c = rot_rate[0] q_c = rot_rate[1] return np.array([p_c, q_c])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): #acceleration_cmd[0] = 0.0 #acceleration_cmd[1] = 0.0 """ Generate the rollrate and pitchrate commands in the body frame Args: acceleration_cmd: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll,pitch,yaw) in radians thrust_cmd: vehicle thrust command in newtons Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ # Calculate rotation matrix R = euler2RM(attitude[0], attitude[1], attitude[2]) # Account for drone mass (factor out for acceleration) accel = thrust_cmd / DRONE_MASS_KG # N/kg -> m/s^2 # Gate the tilt angle as a percentage of total acceleration (approximately) bx_targ = -np.clip(acceleration_cmd[0] / accel, -self.maxTiltAngle, self.maxTiltAngle) by_targ = -np.clip(acceleration_cmd[1] / accel, -self.maxTiltAngle, self.maxTiltAngle) # Calculate angular velocity bxd = self.kpBank * (bx_targ - R[0, 2]) byd = self.kpBank * (by_targ - R[1, 2]) p_cmd = (R[1, 0] * bxd - R[1, 1] * byd) / R[2, 2] q_cmd = (R[0, 0] * bxd - R[0, 1] * byd) / R[2, 2] return np.array([p_cmd, q_cmd])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ pc_qc = np.array([0.0,0.0]) if thrust_cmd > 0: c = -1 * thrust_cmd/DRONE_MASS_KG b_x_c_target, b_y_c_target = np.clip(acceleration_cmd/c, -1, 1) rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_x_term = self.k_p_roll * (b_x_c_target - rot_mat[0][2]) b_y_term = self.k_p_pitch * (b_y_c_target - rot_mat[1][2]) M = np.array([[rot_mat[1][0], -rot_mat[0][0]], [rot_mat[1][1], -rot_mat[0][1]]]) / rot_mat[2][2] pc_qc = np.matmul(M, np.array([b_x_term, b_y_term]).T) return pc_qc
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ #return 0.0 rot_mat = euler2RM(*attitude) u1_bar = self.kpPosZ * (altitude_cmd - altitude) + self.kpVelZ * ( vertical_velocity_cmd - vertical_velocity) + acceleration_ff c = (u1_bar - 0.0 * GRAVITY) / rot_mat[2, 2] * DRONE_MASS_KG c = np.clip(c, 0.0, MAX_THRUST) return c
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ if thrust_cmd > 0: b_x_c_target = -np.clip( acceleration_cmd[0] * DRONE_MASS_KG / thrust_cmd, -1, 1) b_y_c_target = -np.clip( acceleration_cmd[1] * DRONE_MASS_KG / thrust_cmd, -1, 1) rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_x_c_dot = self.k_p_roll * (b_x_c_target - rot_mat[0, 2]) b_y_c_dot = self.k_p_pitch * (b_y_c_target - rot_mat[1, 2]) r_mat = np.array([[ rot_mat[1, 0] / rot_mat[2, 2], -rot_mat[0, 0] / rot_mat[2, 2] ], [rot_mat[1, 1] / rot_mat[2, 2], -rot_mat[0, 1] / rot_mat[2, 2]]]) b_dot_mat = np.array([b_x_c_dot, b_y_c_dot]).T res = np.matmul(r_mat, b_dot_mat) else: res = [0., 0.] return res
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the roll rate and pitch rate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd, east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thrusts command in Newton Returns: 2-element numpy array, desired roll rate (p_cmd) and pitch rate (q_cmd) commands in radians/s """ if thrust_cmd > 0: # Calculate rotation matrix R = euler2RM(*attitude) # Calculate b_x and b_y b_xy = R[0:2, 2] # Calculate b_x_cmd and b_y_cmd (value of the rotation matrix should be between -1 and 1) c_cmd = -thrust_cmd / DRONE_MASS_KG # (m/s^2) b_xy_cmd = np.clip(acceleration_cmd/c_cmd, -1, 1) # Calculate error terms error = b_xy_cmd - b_xy # Calculate b_x_dot_cmd and b_y_dot_cmd (1/s) b_xy_dot_cmd = np.array([self.roll_controller.control(error[0]), self.pitch_controller.control(error[1])]) # Calculate and return p_cmd and q_cmd (rad/s) rotation = np.array([[R[1,0], -R[0,0]], [R[1,1], -R[0,1]]]) / R[2,2] return rotation @ b_xy_dot_cmd else: return np.array([0, 0])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thrust command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ e = euler2RM(attitude[0], attitude[1], attitude[2]) b_x_a = e[0][2] b_y_a = e[1][2] R33 = e[2][2] R21 = e[1][0] R22 = e[1][1] R12 = e[0][1] R11 = e[0][0] b_x_c_target = acceleration_cmd[0] * DRONE_MASS_KG / thrust_cmd b_y_c_target = acceleration_cmd[1] * DRONE_MASS_KG / thrust_cmd b_dot_x_c = self.k_p_roll * (b_x_c_target - b_x_a) b_dot_y_c = self.k_p_pitch * (b_y_c_target - b_y_a) [[p_c], [q_c]] = (1.0 / R33) * np.matmul(np.array([[R21, -R11], [R22, -R12]]), np.array([[b_dot_x_c], [b_dot_y_c]])) return np.array([p_c, q_c])
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ rot_mat = euler2RM(*attitude) b_z = rot_mat[2, 2] e_z = altitude_cmd - altitude e_z_dot = vertical_velocity_cmd - vertical_velocity z_dot_dot_c = self.k_p_z * e_z + self.k_d_z * e_z_dot + acceleration_ff #print("AltC: {}, Alt: {}, Vc: {}, V: {}, Ez: {}, Ezd: {}, Zddc: {}".format( # altitude_cmd, altitude, vertical_velocity_cmd, vertical_velocity, # e_z, e_z_dot, z_dot_dot_c)) c_c = z_dot_dot_c / b_z thrust = c_c * DRONE_MASS_KG thrust = np.clip(thrust, 0.1, MAX_THRUST) # print("target alt: {}, current alt: {}, z_dd_c: {}, thrust:{}".format(altitude_cmd, altitude, z_dot_dot_c, # thrust)) #print("Bz: {}, ThrustO: {}, thrust: {}, attitude: {}".format(b_z, c_c * DRONE_MASS_KG, thrust, attitude)) return thrust
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll,pitch,yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ R = euler2RM(attitude[0], attitude[1], attitude[2]) c_d = thrust_cmd / DRONE_MASS_KG if thrust_cmd > 0.0: accel_p_term = -min(max(acceleration_cmd[0].item() / c_d, -1.0), 1.0) accel_q_term = -min(max(acceleration_cmd[1].item() / c_d, -1.0), 1.0) p_cmd = (1 / R[2, 2]) * (-R[1, 0] * self.Kp_gain_roll * (R[0, 2] - accel_p_term) + R[0, 0] * self.Kp_gain_pitch * (R[1, 2] - accel_q_term)) q_cmd = (1 / R[2, 2]) * (-R[1, 1] * self.Kp_gain_pitch * (R[0, 2] - accel_p_term) + R[0, 1] * self.Kp_gain_pitch * (R[1, 2] - accel_q_term)) else: # Otherwise command no rate p_cmd = 0.0 q_cmd = 0.0 thrust_cmd = 0.0 return np.array([p_cmd, q_cmd])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: acceleration_cmd: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ # #TODO move to __init__ (b_x_c, b_y_c) = acceleration_cmd * self.m / -thrust_cmd R = euler2RM(*attitude) b_x = R[0, 2] b_x_err = b_x_c - b_x b_x_c_dot = self.kp_roll * b_x_err b_y = R[1, 2] b_y_err = b_y_c - b_y b_y_c_dot = self.kp_pitch * b_y_err rot_mat1 = np.array([[R[1, 0], -R[0, 0]], [R[1, 1], -R[0, 1]]]) / R[2, 2] rot_rate = np.matmul(rot_mat1, np.array([b_x_c_dot, b_y_c_dot]).T) # print(rot_rate) return rot_rate
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ #return np.array([0.0, 0.0]) if thrust_cmd > 0: MAX_TILT = 1.0 c = -thrust_cmd / DRONE_MASS_KG # why -ve? did not understand fully but followed comments in forum bxc, byc = np.clip(acceleration_cmd / c, -MAX_TILT, MAX_TILT) rot_mat = euler2RM(*attitude) R13 = rot_mat[0, 2] R23 = rot_mat[1, 2] R33 = rot_mat[2, 2] R11 = rot_mat[0, 0] R22 = rot_mat[1, 1] R12 = rot_mat[0, 1] R21 = rot_mat[1, 0] b_x_c_dot = self.kpBank * (bxc - R13) b_y_c_dot = self.kpBank * (byc - R23) p_c, q_c = 1. / R33 * np.matmul( np.array([[R21, -R11], [R22, -R12]]), np.array([b_x_c_dot, b_y_c_dot])) return np.array([p_c, q_c]) else: return np.array([0.0, 0.0])
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ err = altitude_cmd - altitude vel_err = vertical_velocity_cmd - vertical_velocity R = euler2RM(attitude[0], attitude[1], attitude[2]) u1_cmd = ((self.alt_Kp * err + self.alt_Kd * vel_err + DRONE_MASS_KG * acceleration_ff) / R[2, 2]) return np.clip(u1_cmd, 0.0, MAX_THRUST)
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ e = euler2RM(attitude[0], attitude[1], attitude[2]) b_z = e[2][2] u_bar_1 = self.z_k_p * (altitude_cmd - altitude) + self.z_k_d * ( vertical_velocity_cmd - vertical_velocity) + acceleration_ff c = (u_bar_1 + GRAVITY) / b_z return np.clip(c * DRONE_MASS_KG, -MAX_THRUST, MAX_THRUST)
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll,pitch,yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ rot_mat = euler2RM(*attitude) b = rot_mat[0:2, 2] c_c = -thrust_cmd / DRONE_MASS_KG b_c = acceleration_cmd / c_c e_b = b_c - b # notice the gain is named as k_p_pr here. the first element in e_b # is b_x_c, which need pitching. b_y_c on the other hand, need rolling b_c_dot = self.k_p_pr * e_b r = np.array([[rot_mat[1, 0], -rot_mat[0, 0]], [rot_mat[1, 1], -rot_mat[0, 1]]], dtype=np.float) pq_c = np.dot(r, b_c_dot) / rot_mat[2, 2] # print(acceleration_cmd, b_c_dot) return pq_c
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ c = -thrust_cmd / DRONE_MASS_KG rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_x = rot_mat[0, 2] #TBD if Thrust command is correct here b_x_err = (acceleration_cmd[0] / c) - b_x b_x_p_term = self.k_p_roll * b_x_err b_y = rot_mat[1, 2] #TBD if Thrust command is correct here b_y_err = (acceleration_cmd[1] / c) - b_y b_y_p_term = self.k_p_pitch * b_y_err b_x_commanded_dot = b_x_p_term b_y_commanded_dot = b_y_p_term rot_mat1 = np.array([[rot_mat[1, 0], -rot_mat[0, 0]], [rot_mat[1, 1], -rot_mat[0, 1]]]) / rot_mat[2, 2] rot_rate = np.matmul( rot_mat1, np.array([b_x_commanded_dot, b_y_commanded_dot]).T) p_c = rot_rate[0] q_c = rot_rate[1] # if p_c > 3.0: # print (p_c,acceleration_cmd[0]) # if q_c > 2: # print (q_c,acceleration_cmd[1]) # print (acceleration_cmd) # Calculate rotation matrix first from attitude # Rx_phi = np.array([ [1,0,0],\ # [0,np.cos(attitude[0]),-np.sin(attitude[0])],\ # [0,np.sin(attitude[0]),np.cos(attitude[0])] ]) # Ry_theta = np.array([ [np.cos(attitude[1]),0,np.sin(attitude[1])],\ # [0,1,0],\ # [-np.sin(attitude[1]),0,np.cos(attitude[1])] ]) # Rz_psi = np.array([ [np.cos(attitude[2]),-np.sin(attitude[2]),0],\ # [np.sin(attitude[2]),np.cos(attitude[2]), 0],\ # [0,0,1] ]) # A = np.matmul(Rz_psi,Ry_theta) # rotation_mat = np.matmul(A,Rx_phi) # b_dot_x_c = self.k_p_roll*((-acceleration_cmd[0]/thrust_cmd)-rotation_mat[0][2]) # b_dot_y_c = self.k_p_pitch*((-acceleration_cmd[1]/thrust_cmd)-rotation_mat[1][2]) # A = np.array([ [rotation_mat[1][0],-rotation_mat[0][0]],[rotation_mat[1][1],-rotation_mat[0][1] ] ]) # B = (1/rotation_mat[2][2])*A # C = np.matmul(B,np.array([b_dot_x_c,b_dot_y_c])) # p_c = C[0] # q_c = C[1] return np.array([p_c, q_c])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ if thrust_cmd > 0: c = -1 * thrust_cmd / DRONE_MASS_KG # Find R13 (Target_X) and R23 (Target_Y) b_x_c_target, b_y_c_target = np.clip(acceleration_cmd / c, -1, 1) # min & max tilt (rad) #Calculate Rotation Matrix rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_x = rot_mat[0, 2] # R13 (Actual) b_x_err = b_x_c_target - b_x b_x_p_term = self.Kp_roll * b_x_err b_y = rot_mat[1, 2] # R23 (Actual) b_y_err = b_y_c_target - b_y b_y_p_term = self.Kp_pitch * b_y_err b_x_cmd_dot = b_x_p_term b_y_cmd_dot = b_y_p_term rot_mat1 = np.array([[rot_mat[1, 0], -rot_mat[0, 0]], [rot_mat[1, 1], -rot_mat[0, 1]]]) / rot_mat[2, 2] rot_rate = np.matmul(rot_mat1, np.array([b_x_cmd_dot, b_y_cmd_dot]).T) p_c = rot_rate[0] q_c = rot_rate[1] else: p_c = 0 q_c = 0 thrust_cmd = 0 return np.array([p_c, q_c])
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) z_err = altitude_cmd - altitude z_err_dot = vertical_velocity_cmd - vertical_velocity b_z = rot_mat[2, 2] p_term = self.z_k_p * z_err d_term = self.z_k_d * z_err_dot u_1_bar = p_term + d_term + acceleration_ff c = (u_1_bar - self.g) / b_z # # Calculate rotation matrix first from attitude # Rx_phi = np.array([ [1,0,0],\ # [0,np.cos(attitude[0]),-np.sin(attitude[0])],\ # [0,np.sin(attitude[0]),np.cos(attitude[0])] ]) # Ry_theta = np.array([ [np.cos(attitude[1]),0,np.sin(attitude[1])],\ # [0,1,0],\ # [-np.sin(attitude[1]),0,np.cos(attitude[1])] ]) # Rz_psi = np.array([ [np.cos(attitude[2]),-np.sin(attitude[2]),0],\ # [np.sin(attitude[2]),np.cos(attitude[2]), 0],\ # [0,0,1] ]) # A = np.matmul(Rz_psi,Ry_theta) # rotation_matrix = np.matmul(A,Rx_phi) # u1_bar = self.z_k_p*(altitude_cmd-altitude) + self.z_k_d*(vertical_velocity_cmd-vertical_velocity) + acceleration_ff # c = (u1_bar-self.g)/rotation_matrix[2][2] # if c > MAX_THRUST: # c = MAX_THRUST return c
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s """ b_x_c_target = -np.clip( acceleration_cmd[0] / (thrust_cmd / DRONE_MASS_KG), -MAX_TILT_ANGLE, MAX_TILT_ANGLE) b_y_c_target = -np.clip( acceleration_cmd[1] / (thrust_cmd / DRONE_MASS_KG), -MAX_TILT_ANGLE, MAX_TILT_ANGLE) #b_x_c_target = -acceleration_cmd[0] / (thrust_cmd / DRONE_MASS_KG) #b_y_c_target = -acceleration_cmd[1] / (thrust_cmd / DRONE_MASS_KG) # Calculate rotation matrix rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) # Roll P Controller b_x = rot_mat[0, 2] b_x_err = b_x_c_target - b_x b_x_p_term = self.k_p_roll * b_x_err # Pitch P Controller b_y = rot_mat[1, 2] b_y_err = b_y_c_target - b_y b_y_p_term = self.k_p_pitch * b_y_err b_x_commanded_dot = b_x_p_term b_y_commanded_dot = b_y_p_term # Account for non-linear transformations from local accelerations to body rates rot_mat1 = np.array([[rot_mat[1, 0], -rot_mat[0, 0]], [rot_mat[1, 1], -rot_mat[0, 1]]]) / rot_mat[2, 2] rot_rate = np.matmul( rot_mat1, np.array([b_x_commanded_dot, b_y_commanded_dot]).T) p_c = rot_rate[0] q_c = rot_rate[1] return np.array([p_c, q_c])
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd): """ Generate the rollrate and pitchrate commands in the body frame Args: target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2 attitude: 3-element numpy array (roll, pitch, yaw) in radians thrust_cmd: vehicle thruts command in Newton Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s Hints: - Note that the last command parameter is thrust, not acceleration. If you plan on using acceleration in your controller, make sure to account for the drone's mass! """ if thrust_cmd > 0.: c = -thrust_cmd / DRONE_MASS_KG b_x_c, b_y_c = np.clip(acceleration_cmd / c, -1., 1) rot_mat = euler2RM(*attitude) b_x = rot_mat[0, 2] b_x_err = b_x_c - b_x b_x_p_term = self.roll_pitch_k_p_roll * b_x_err b_y = rot_mat[1, 2] b_y_err = b_y_c - b_y b_y_p_term = self.roll_pitch_k_p_pitch * b_y_err b_x_commanded_dot = b_x_p_term b_y_commanded_dot = b_y_p_term rot_mat1 = np.array([[rot_mat[1, 0], -rot_mat[0, 0]], [rot_mat[1, 1], -rot_mat[0, 1]]]) / rot_mat[2, 2] rot_rate = np.matmul( rot_mat1, np.array([b_x_commanded_dot, b_y_commanded_dot]).T) p_c = rot_rate[0] q_c = rot_rate[1] print("b_x_err={0}, b_y_err={1}".format(b_x_err, b_y_err)) return np.array([p_c, q_c]) else: return np.array([0.0, 0.0])
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ print("Altitude_cmd", altitude_cmd) print("Altitude", altitude) print("vertical_velocity_cmd", vertical_velocity_cmd) print("vertical_velocity", vertical_velocity) z_target = altitude_cmd z_dot_target = vertical_velocity_cmd z_actual = altitude z_dot_actual = vertical_velocity z_dot_dot_target = acceleration_ff rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) u_bar_1 = self.z_k_p * (z_target - z_actual) + self.z_k_d * ( z_dot_target - z_dot_actual) + z_dot_dot_target b = rot_mat[2, 2] c = (u_bar_1 - self.g) / b print("C", c) thrust = c * self.m print("Thrust", thrust) a = np.clip(thrust, 0.0, 10) print("App", a) return a
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ # Extract roll, pitch, yaw variables from attitude parameter [roll, pitch, yaw] = attitude # calculate rotation matrix from roll pitch yaw rot_mat = euler2RM(roll, pitch, yaw) # desired vertical acceleration in inertial frame is given by the following PD controller formula u_1_bar = self.z_k_p * (altitude_cmd - altitude) + self.z_k_d * ( vertical_velocity_cmd - vertical_velocity) + acceleration_ff # calculate vertical acceleration in body frame, rot_mat[2][2] is z scaling factor in inertial frame c = (u_1_bar - 9.81) / rot_mat[2][2] # since thrust is in Newton, from F=ma we need to multiply by drone's mass thrust = DRONE_MASS_KG * c # ensure thrust stays below the MAX_THRUST limit thrust_limited = np.clip(thrust, 0.0, MAX_THRUST) return thrust_limited # thrust command in Newton
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ # Get rotation matrix from attitude rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_z = rot_mat[2, 2] altitude_err = altitude_cmd - altitude integral_err = self.Kp_gain_alt * altitude_err + vertical_velocity_cmd # Margin the ascending and descending rate if integral_err > self.max_ascending_rate: integral_err = self.max_ascending_rate elif integral_err < -self.max_descending_rate: integral_err = -self.max_descending_rate acceleration_cmd = DRONE_MASS_KG * acceleration_ff + self.Kp_gain_i * (integral_err - vertical_velocity) thrust = acceleration_cmd / b_z if thrust > MAX_THRUST: thrust = MAX_THRUST elif thrust < 0.0: thrust = 0.0 return thrust
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0): """Generate vertical acceleration (thrust) command Args: altitude_cmd: desired vertical position (+up) vertical_velocity_cmd: desired vertical velocity (+up) altitude: vehicle vertical position (+up) vertical_velocity: vehicle vertical velocity (+up) attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians acceleration_ff: feedforward acceleration command (+up) Returns: thrust command for the vehicle (+up) """ # Vertical position and velocity errors z_err = altitude_cmd - altitude z_err_dot = vertical_velocity_cmd - vertical_velocity p_term = self.z_k_p * z_err d_term = self.z_k_d * z_err_dot # PD altitude controller with feed forward acceleration u_1_bar = p_term + d_term + acceleration_ff rot_mat = euler2RM(attitude[0], attitude[1], attitude[2]) b_z = rot_mat[2, 2] #c = (u_1_bar + GRAVITY) / b_z #Convert linear acceleration to Thrust c = (u_1_bar) / b_z thrust = c * DRONE_MASS_KG return (np.clip(thrust, 0, MAX_THRUST))