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))
