def __init__(self):
self.x = np.zeros(13)
self.x[6] = 1.0 # identity quaternion
#self.u = np.zeros(5)
self.q_dot_prev = np.zeros(4)
# Grab parameters:
self.alt_KP = rospy.get_param("~alt_KP", 600.0)
self.alt_KI = rospy.get_param("~alt_KI", 400.0)
self.alt_KD = rospy.get_param("~alt_KD", 600.0)
#self.alt_KDD = rospy.get_param("~alt_KDD",50.0)
self.alt_Ilimit = rospy.get_param("~alt_Ilimit", 400.0)
#self.yaw_KP = rospy.get_param("~yaw_KP", 4.0)
#self.yaw_KI = rospy.get_param("~yaw_KI", 0.0)
#self.yaw_KD = rospy.get_param("~yaw_KD", 0.0)
#self.yaw_Ilimit = rospy.get_param("~yaw_Ilimit", 50.0)
self.nominal_thrust = rospy.get_param("~nominal_thrust", 1500)
self.thrust_mult = rospy.get_param("~thrust_mult", 1.0)
self.max_thrust = rospy.get_param("~max_thrust", 2100)
temp = rospy.get_param("~periodic_accel_disturbance/vector", None)
if temp is None:
self.periodic_accel_disturbance_vector = np.zeros(3)
else:
self.periodic_accel_disturbance_vector = np.array(temp)
self.periodic_accel_disturbance_period = rospy.get_param("~periodic_accel_disturbance/period", 10.0)
self.periodic_accel_disturbance_duration = rospy.get_param("~periodic_accel_disturbance/duration", 1.0)
self.pitch_bias = rospy.get_param("~pitch_bias", 0.0) # degrees
self.roll_bias = rospy.get_param("~roll_bias", 0.0) # degrees
self.roll_slew_rate_limit = rospy.get_param("~roll_slew_rate_limit", 10.0) # deg/s
self.pitch_slew_rate_limit = rospy.get_param("~pitch_slew_rate_limit", 10.0) # deg/s
temp = rospy.get_param("~gaussian_accel_noise_covariance", None) #[0.0]*9)
if temp is not None:
self.gaussian_accel_noise_covariance = np.array(temp).reshape(3,3)
else:
self.gaussian_accel_noise_covariance = None
self.prev_orientation = (0.0, 0.0, 0.0, 1.0)
self.prev_roll_cmd = 0.0
self.prev_pitch_cmd = 0.0
self.alt_pid = PidController(self.alt_KP, self.alt_KI, self.alt_KD, self.alt_Ilimit)
self.t = 0
self.debug_publisher = rospy.Publisher('~debug', SimDebug)
def __init__(self):
self.x = np.zeros(13)
self.x[6] = 1.0 # identity quaternion
self.u = np.zeros(5)
# Grab parameters:
self.alt_KP = rospy.get_param("~alt_KP", 600.0)
self.alt_KI = rospy.get_param("~alt_KI", 400.0)
self.alt_KD = rospy.get_param("~alt_KD", 600.0)
#self.alt_KDD = rospy.get_param("~alt_KDD",50.0)
self.alt_Ilimit = rospy.get_param("~alt_Ilimit", 400.0)
#self.yaw_KP = rospy.get_param("~yaw_KP", 4.0)
#self.yaw_KI = rospy.get_param("~yaw_KI", 0.0)
#self.yaw_KD = rospy.get_param("~yaw_KD", 0.0)
#self.yaw_Ilimit = rospy.get_param("~yaw_Ilimit", 50.0)
self.nominal_thrust = rospy.get_param("~nominal_thrust", 1500)
self.thrust_mult = rospy.get_param("~thrust_mult", 1.0)
self.max_thrust = rospy.get_param("~max_thrust", 2100)
self.prev_orientation = (0.0, 0.0, 0.0, 1.0)
self.alt_pid = PidController(self.alt_KP, self.alt_KI, self.alt_KD, self.alt_Ilimit)
class SimpleNonlinearModel(ModelBase):
"""
Simple nonlinear model of arbitrary quadrotor type vehicle.
13 states:
index quantity units
0 x position m
1 y position m
2 z position m
3 x velocity m/s
4 y velocity m/s
5 z velocity m/s
6 orientation quaternion (w)
7 orientation quaternion (x)
8 orientation quaternion (y)
9 orientation quaternion (z)
10 angular velocity (x) rad/s
11 angular velocity (y) rad/s
12 angular velocity (z) rad/s
4 inputs:
index quantity units
0 yaw deg
1 pitch deg
2 roll deg
3 altitude m
4 enable (bool, True if motors are on)
NOTE: transformations.py (tf.transformations) represents quaternions
as (x, y, z, w) tuples!
"""
def __init__(self):
self.x = np.zeros(13)
self.x[6] = 1.0 # identity quaternion
self.u = np.zeros(5)
# Grab parameters:
self.alt_KP = rospy.get_param("~alt_KP", 600.0)
self.alt_KI = rospy.get_param("~alt_KI", 400.0)
self.alt_KD = rospy.get_param("~alt_KD", 600.0)
#self.alt_KDD = rospy.get_param("~alt_KDD",50.0)
self.alt_Ilimit = rospy.get_param("~alt_Ilimit", 400.0)
#self.yaw_KP = rospy.get_param("~yaw_KP", 4.0)
#self.yaw_KI = rospy.get_param("~yaw_KI", 0.0)
#self.yaw_KD = rospy.get_param("~yaw_KD", 0.0)
#self.yaw_Ilimit = rospy.get_param("~yaw_Ilimit", 50.0)
self.nominal_thrust = rospy.get_param("~nominal_thrust", 1500)
self.thrust_mult = rospy.get_param("~thrust_mult", 1.0)
self.max_thrust = rospy.get_param("~max_thrust", 2100)
self.prev_orientation = (0.0, 0.0, 0.0, 1.0)
self.alt_pid = PidController(self.alt_KP, self.alt_KI, self.alt_KD, self.alt_Ilimit)
def set_input(self, u, dt):
self.u = u
self.thrust_cmd = self.nominal_thrust + \
self.alt_pid.update(-self.get_position()[2] - u[3], dt)
def update(self, dt):
# The following model is completely arbitrary and should not be taken to be representative of
# real vehicle performance!
# But, it should be good enough to test out control modes etc.
yaw_cmd, pitch_cmd, roll_cmd, alt_cmd, motor_enable = self.u
cur_yaw, cur_pitch, cur_roll = tft.euler_from_quaternion(self.get_orientation(), 'rzyx')
# Feedback control: attitude angles track inputs exactly; altitude is input to
# PID controller for thrust
if motor_enable:
thrust = self.thrust_cmd
# accelerations due to pitch/roll in body frame:
pitch_clipped, pwas_clipped = clip(pitch_cmd, -50, 50)
roll_clipped, rwas_clipped = clip(roll_cmd, -50, 50)
yaw_cmd = yaw_cmd
else:
thrust = 0.0
yaw_cmd = degrees(cur_yaw)
pitch_clipped = degrees(cur_pitch)
roll_clipped = degrees(cur_roll)
pwas_clipped = rwas_clipped = False
# Propagate dynamics
# Velocity
accel_thrust = thrust*THRUST_SCALING/MASS
if pwas_clipped or rwas_clipped:
rospy.logwarn('Pitch and/or roll clipped! Pre-clip values %f %f' % (pitch_cmd, roll_cmd))
accel_body = np.array([-sin(radians(pitch_clipped))*accel_thrust,
sin(radians(roll_clipped))*accel_thrust,
0.0]) # vertical accel will be added in inertial frame
# rotate body frame accelerations into inertial frame, and add drag and vertical forces:
Ryaw = tft.euler_matrix(cur_yaw, 0, 0, 'rzyx')
accel_inertial_lateral = np.dot(Ryaw[:3,:3], accel_body)
cur_velocity = self.get_velocity()
accel_inertial = accel_inertial_lateral + \
np.array([- LINEAR_DRAG*cur_velocity[0]
- QUADRATIC_DRAG*np.sign(cur_velocity[0])*(cur_velocity[0]**2),
- LINEAR_DRAG*cur_velocity[1]
- QUADRATIC_DRAG*np.sign(cur_velocity[1])*(cur_velocity[1]**2),
GRAVITY - accel_thrust*cos(radians(pitch_clipped))*cos(radians(roll_clipped))])
velocity = cur_velocity + accel_inertial*dt
# Position
position = self.get_position() + velocity*dt
position[2] = min(0, position[2]) # can't go lower than the ground
# Angles
if position[2] == 0.0 and velocity[2] > 0:
# On the ground..
velocity = np.zeros(3)
orientation = tft.quaternion_from_euler(radians(yaw_cmd), 0, 0, 'rzyx')
else:
# assume perfect yaw rate, and pitch and roll tracking:
orientation = tft.quaternion_from_euler(radians(yaw_cmd), radians(pitch_clipped), radians(roll_clipped), 'rzyx')
# Angular rate
# from J. Diebel, "Representing attitude: Euler angles, unit quaternions, and rotation vectors," 2006
q_cur = np.array((orientation[3],) + tuple(orientation[:3])) # convert to wxyz
q_prev = np.array((self.prev_orientation[3],) + tuple(self.prev_orientation[:3])) # convert to wxyz
if dt > EPS:
q_dot = (q_cur - q_prev)/dt
else:
q_dot = np.zeros(4)
Wprime = np.array([[-q_cur[1], q_cur[0], q_cur[3], -q_cur[2]],
[-q_cur[2], -q_cur[3], q_cur[0], q_cur[1]],
[-q_cur[3], q_cur[2], -q_cur[1], q_cur[0]]])
ang_vel_body = 2*np.dot(Wprime,q_dot)
# Assemble new state vector
self.x = np.concatenate([position, velocity, q_cur, ang_vel_body])
# Save for future finite differences
self.prev_orientation = orientation[:] # be sure to copy not reference...
def get_position(self):
return self.x[0:3]
def get_velocity(self):
return self.x[3:6]
def get_orientation(self):
"""
Return orientation quaternion in (x, y, z, w) convention
"""
return tuple(self.x[7:10]) + (self.x[6],)
def get_angular_velocity(self):
return self.x[10:13]
class SimpleNonlinearModel(ModelBase):
"""
Simple nonlinear model of arbitrary quadrotor type vehicle.
13 states:
index quantity units
0 x position m
1 y position m
2 z position m
3 x velocity m/s
4 y velocity m/s
5 z velocity m/s
6 orientation quaternion (w)
7 orientation quaternion (x)
8 orientation quaternion (y)
9 orientation quaternion (z)
10 angular velocity (x) rad/s
11 angular velocity (y) rad/s
12 angular velocity (z) rad/s
4 inputs:
index quantity units
0 yaw deg
1 pitch deg
2 roll deg
3 altitude m
4 enable (bool, True if motors are on)
NOTE: transformations.py (tf.transformations) represents quaternions
as (x, y, z, w) tuples!
"""
def __init__(self):
self.x = np.zeros(13)
self.x[6] = 1.0 # identity quaternion
#self.u = np.zeros(5)
self.q_dot_prev = np.zeros(4)
# Grab parameters:
self.alt_KP = rospy.get_param("~alt_KP", 600.0)
self.alt_KI = rospy.get_param("~alt_KI", 400.0)
self.alt_KD = rospy.get_param("~alt_KD", 600.0)
#self.alt_KDD = rospy.get_param("~alt_KDD",50.0)
self.alt_Ilimit = rospy.get_param("~alt_Ilimit", 400.0)
#self.yaw_KP = rospy.get_param("~yaw_KP", 4.0)
#self.yaw_KI = rospy.get_param("~yaw_KI", 0.0)
#self.yaw_KD = rospy.get_param("~yaw_KD", 0.0)
#self.yaw_Ilimit = rospy.get_param("~yaw_Ilimit", 50.0)
self.nominal_thrust = rospy.get_param("~nominal_thrust", 1500)
self.thrust_mult = rospy.get_param("~thrust_mult", 1.0)
self.max_thrust = rospy.get_param("~max_thrust", 2100)
temp = rospy.get_param("~periodic_accel_disturbance/vector", None)
if temp is None:
self.periodic_accel_disturbance_vector = np.zeros(3)
else:
self.periodic_accel_disturbance_vector = np.array(temp)
self.periodic_accel_disturbance_period = rospy.get_param("~periodic_accel_disturbance/period", 10.0)
self.periodic_accel_disturbance_duration = rospy.get_param("~periodic_accel_disturbance/duration", 1.0)
self.pitch_bias = rospy.get_param("~pitch_bias", 0.0) # degrees
self.roll_bias = rospy.get_param("~roll_bias", 0.0) # degrees
self.roll_slew_rate_limit = rospy.get_param("~roll_slew_rate_limit", 10.0) # deg/s
self.pitch_slew_rate_limit = rospy.get_param("~pitch_slew_rate_limit", 10.0) # deg/s
temp = rospy.get_param("~gaussian_accel_noise_covariance", None) #[0.0]*9)
if temp is not None:
self.gaussian_accel_noise_covariance = np.array(temp).reshape(3,3)
else:
self.gaussian_accel_noise_covariance = None
self.prev_orientation = (0.0, 0.0, 0.0, 1.0)
self.prev_roll_cmd = 0.0
self.prev_pitch_cmd = 0.0
self.alt_pid = PidController(self.alt_KP, self.alt_KI, self.alt_KD, self.alt_Ilimit)
self.t = 0
self.debug_publisher = rospy.Publisher('~debug', SimDebug)
def set_input(self, motors_on, roll_cmd, pitch_cmd, direct_yaw_rate_commands,
yaw_cmd, yaw_rate_cmd, direct_thrust_commands, alt_cmd, thrust_cmd):
self.motors_on = motors_on # bool
self.roll_cmd = roll_cmd # deg
self.pitch_cmd = pitch_cmd # deg
self.direct_yaw_rate_commands = direct_yaw_rate_commands # bool
self.yaw_cmd = yaw_cmd # deg
self.yaw_rate_cmd = yaw_rate_cmd # deg/s
self.direct_thrust_commands = direct_thrust_commands # bool
self.alt_cmd = alt_cmd # m
self.thrust_cmd = thrust_cmd # N
def update(self, dt):
# The following model is completely arbitrary and should not be taken to be representative of
# real vehicle performance!
# But, it should be good enough to test out control modes etc.
self.t += dt
#yaw_cmd, pitch_cmd, roll_cmd, alt_cmd, motor_enable = self.u
cur_yaw, cur_pitch, cur_roll = tft.euler_from_quaternion(self.get_orientation(), 'rzyx')
prev_yaw, prev_pitch, prev_roll = tft.euler_from_quaternion(self.prev_orientation, 'rzyx')
# Feedback control: attitude angles track inputs exactly; altitude is input to
# PID controller for thrust
sim_debug_msg = SimDebug()
sim_debug_msg.header.stamp = rospy.Time.now()
if self.motors_on:
if self.direct_thrust_commands:
thrust = self.thrust_mult*self.thrust_cmd/THRUST_SCALING
else:
thrust = self.thrust_mult*(self.nominal_thrust + \
self.alt_pid.update(-self.get_position()[2] - self.alt_cmd, dt))
thrust = min(self.max_thrust, thrust)
ang_vel_inertial = self.x[10:13]
R_inertial_body = tft.quaternion_matrix(self.get_orientation())[:3,:3]
ang_vel_body = np.dot(R_inertial_body.T, ang_vel_inertial)
# accelerations due to pitch/roll in body frame:
A = np.array([[-d1, 1.0],[-d0, 0.0]])
B = np.array([0, n0])
# Roll:
roll_cmd_slew_limited = limitSlewRate(self.roll_cmd, self.prev_roll_cmd, dt, self.roll_slew_rate_limit)
self.prev_roll_cmd = roll_cmd_slew_limited
roll_state_cur = np.array([cur_roll, ang_vel_body[0]])
xdot = np.dot(A,roll_state_cur) + B*radians(roll_cmd_slew_limited)
roll_state_new = roll_state_cur + xdot*dt #+ radians(self.roll_bias)
# Pitch:
pitch_cmd_slew_limited = limitSlewRate(self.pitch_cmd, self.prev_pitch_cmd, dt, self.pitch_slew_rate_limit)
self.prev_pitch_cmd = pitch_cmd_slew_limited
pitch_state_cur = np.array([cur_pitch, ang_vel_body[1]])
xdot = np.dot(A,pitch_state_cur) + B*radians(pitch_cmd_slew_limited)
pitch_state_new = pitch_state_cur + xdot*dt #+ radians(self.pitch_bias)
pitch_clipped, pwas_clipped = clip(degrees(pitch_state_new[0]), -50, 50)
roll_clipped, rwas_clipped = clip(degrees(roll_state_new[0]), -50, 50)
# Yaw (no dynamics here for now -- tracks perfectly)
yaw_cmd = self.yaw_cmd
else:
thrust = 0.0
yaw_cmd = degrees(cur_yaw)
pitch_clipped = degrees(cur_pitch)
roll_clipped = degrees(cur_roll)
roll_state_new = np.array([radians(roll_clipped), 0])
pitch_state_new = np.array([radians(pitch_clipped), 0])
pwas_clipped = rwas_clipped = False
sim_debug_msg.thrust_in_counts = thrust
# Propagate dynamics
# Velocity
accel_thrust = thrust*THRUST_SCALING/MASS
#rospy.loginfo("accel_thrust = " + str(accel_thrust))
if pwas_clipped or rwas_clipped:
rospy.logwarn('Pitch and/or roll clipped! Pre-clip (commanded) values %f %f' % (self.pitch_cmd, self.roll_cmd))
accel_body = np.array([-sin(radians(pitch_clipped))*accel_thrust,
sin(radians(roll_clipped))*accel_thrust,
0.0]) # vertical accel will be added in inertial frame
# rotate body frame accelerations into inertial frame, and add drag and vertical forces:
Ryaw = tft.euler_matrix(cur_yaw, 0, 0, 'rzyx')
accel_inertial_lateral = np.dot(Ryaw[:3,:3], accel_body)
cur_velocity = self.get_velocity()
# add periodic disturbance:
tmod = self.t % self.periodic_accel_disturbance_period
if tmod <= self.periodic_accel_disturbance_duration:
accel_disturbance = self.periodic_accel_disturbance_vector
else:
accel_disturbance = np.zeros(3)
if self.direct_thrust_commands:
tfactor = cos(radians(pitch_clipped))*cos(radians(roll_clipped))
else:
tfactor = 1.0 # asctec_adapter altitude controller accounts for cosine losses
accel_inertial = accel_disturbance + \
accel_inertial_lateral + \
np.array([# X:
- LINEAR_DRAG*cur_velocity[0]
- QUADRATIC_DRAG*np.sign(cur_velocity[0])*(cur_velocity[0]**2),
# Y:
- LINEAR_DRAG*cur_velocity[1]
- QUADRATIC_DRAG*np.sign(cur_velocity[1])*(cur_velocity[1]**2),
# Z:
GRAVITY - accel_thrust*tfactor
- LINEAR_DRAG*cur_velocity[2]
- QUADRATIC_DRAG*np.sign(cur_velocity[2])*(cur_velocity[2]**2)])
#rospy.loginfo('accel_inertial: ' + str(accel_inertial))
# Add Gaussian noise
if self.gaussian_accel_noise_covariance is not None:
accel_inertial += np.random.multivariate_normal((0,0,0), self.gaussian_accel_noise_covariance)
sim_debug_msg.accel_inertial = accel_inertial
velocity = cur_velocity + accel_inertial*dt
#rospy.loginfo('velocity: ' + str(velocity))
# Angles
if self.get_position()[2] >= -IMU_HEIGHT and velocity[2] > 0:
# On the ground..
velocity = np.zeros(3)
orientation = tft.quaternion_from_euler(radians(yaw_cmd), 0, 0, 'rzyx')
else:
# assume perfect yaw rate, and pitch and roll tracking:
orientation = tft.quaternion_from_euler(radians(yaw_cmd), radians(pitch_clipped), radians(roll_clipped), 'rzyx')
# Position
position = self.get_position() + velocity*dt
position[2] = min(-IMU_HEIGHT, position[2]) # can't go lower than the ground
sim_debug_msg.position = position
# Angular rate
# from J. Diebel, "Representing attitude: Euler angles, unit quaternions, and rotation vectors," 2006
q_cur = np.array((orientation[3],) + tuple(orientation[:3])) # convert to wxyz
q_prev = np.array((self.prev_orientation[3],) + tuple(self.prev_orientation[:3])) # convert to wxyz
if dt > EPS:
if q_cur[0]*q_prev[0] < 0:
mult = -1
else:
mult = 1
q_dot = QUAT_FILT_A*(mult*q_cur - q_prev)/dt + QUAT_FILT_B*self.q_dot_prev
else:
q_dot = np.zeros(4)
self.q_dot_prev = q_dot
Wprime = np.array([[-q_cur[1], q_cur[0], q_cur[3], -q_cur[2]],
[-q_cur[2], -q_cur[3], q_cur[0], q_cur[1]],
[-q_cur[3], q_cur[2], -q_cur[1], q_cur[0]]])
ang_vel_body = 2*np.dot(Wprime,q_dot)
ang_vel_body[0] = roll_state_new[1]
ang_vel_body[1] = pitch_state_new[1]
R_inertial_body = tft.quaternion_matrix(orientation)[:3,:3]
ang_vel_inertial = np.dot(R_inertial_body, ang_vel_body)
# Assemble new state vector
self.x = np.concatenate([position, velocity, q_cur, ang_vel_inertial])
# Save for future finite differences
self.prev_orientation = orientation[:] # be sure to copy not reference...
self.debug_publisher.publish(sim_debug_msg)
def get_position(self):
return self.x[0:3]
def get_velocity(self):
return self.x[3:6]
def get_orientation(self):
"""
Return orientation quaternion in (x, y, z, w) convention
"""
return tuple(self.x[7:10]) + (self.x[6],)
def get_angular_velocity(self):
return self.x[10:13]
