class PositionControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
/morus/pose - used to extract z-position of the vehicle
/morus/velocity - used to extract vz of the vehicle
/morus/pos_ref - used to set the reference for z-position
/morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
/morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
/morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
# Params
self.rate = rospy.get_param('~rate', 100)
# 0 for simulation, 1 for optitrack
self.feedback_source = rospy.get_param('~feedback', 0)
print self.feedback_source
# Load parameters from yaml file
file_name = rospy.get_param('~filename', 'PositionControl.yaml')
file_name = RosPack().get_path(
'mmuav_control') + '/config/' + file_name
initial_params = yaml.load(file(file_name, 'r'))
# (m_uav + m_other)/(C*4*cos(tilt_angle))
self.g = 9.81
self.mot_speed_hover = math.sqrt(
self.g * (initial_params['mass']) /
(initial_params['rotor_c'] * initial_params['rotor_num'] *
math.cos(initial_params['tilt_angle'])))
self.Kff_v = initial_params['Kff_v']
self.Kff_a = initial_params['Kff_a']
# Delta omega(rotor speed) = self.z_ff_scaler*sqrt(a) where a is
# acceleration from trajectory
self.z_ff_scaler = math.sqrt(
(initial_params['mass']) /
(initial_params['rotor_c'] * initial_params['rotor_num']))
self.pos_sp = Point()
self.pos_sp.x = rospy.get_param('~x', 0.0)
self.pos_sp.y = rospy.get_param('~y', 0.0)
self.pos_sp.z = 1.0
self.pos_mv = Point()
self.vel_mv = Vector3()
self.orientation_mv = Quaternion()
self.orientation_mv_euler = Vector3()
self.orientation_sp = Quaternion()
self.velocity_ff = Vector3()
self.acceleration_ff = Vector3()
self.euler_mv = Vector3()
# X controller
self.pid_x = PID()
self.pid_vx = PID()
# Y controller
self.pid_y = PID()
self.pid_vy = PID()
# Z controller
self.pid_z = PID() # pid instance for z control
self.pid_vz = PID() # pid instance for z-velocity control
# YAW
self.yaw_sp = 0 # Yaw setpoint propagates through system
#########################################################
#########################################################
# Add parameters for x controller
self.pid_x.set_kp(initial_params['pid_x']['Kp'])
self.pid_x.set_ki(initial_params['pid_x']['Ki'])
self.pid_x.set_kd(initial_params['pid_x']['Kd'])
self.pid_x.set_lim_high(initial_params['pid_x']['limit']['high'])
self.pid_x.set_lim_low(initial_params['pid_x']['limit']['low'])
# Add parameters for vx controller
self.pid_vx.set_kp(initial_params['pid_vx']['Kp'])
self.pid_vx.set_ki(initial_params['pid_vx']['Ki'])
self.pid_vx.set_kd(initial_params['pid_vx']['Kd'])
self.pid_vx.set_lim_high(initial_params['pid_vx']['limit']['high'])
self.pid_vx.set_lim_low(initial_params['pid_vx']['limit']['low'])
# Add parameters for y controller
self.pid_y.set_kp(initial_params['pid_y']['Kp'])
self.pid_y.set_ki(initial_params['pid_y']['Ki'])
self.pid_y.set_kd(initial_params['pid_y']['Kd'])
self.pid_y.set_lim_high(initial_params['pid_y']['limit']['high'])
self.pid_y.set_lim_low(initial_params['pid_y']['limit']['low'])
# Add parameters for vy controller
self.pid_vy.set_kp(initial_params['pid_vy']['Kp'])
self.pid_vy.set_ki(initial_params['pid_vy']['Ki'])
self.pid_vy.set_kd(initial_params['pid_vy']['Kd'])
self.pid_vy.set_lim_high(initial_params['pid_vy']['limit']['high'])
self.pid_vy.set_lim_low(initial_params['pid_vy']['limit']['low'])
# Add parameters for z controller
self.pid_z.set_kp(initial_params['pid_z']['Kp'])
self.pid_z.set_ki(initial_params['pid_z']['Ki'])
self.pid_z.set_kd(initial_params['pid_z']['Kd'])
self.pid_z.set_lim_high(initial_params['pid_z']['limit']['high'])
self.pid_z.set_lim_low(initial_params['pid_z']['limit']['low'])
# Add parameters for vz controller
self.pid_vz.set_kp(initial_params['pid_vz']['Kp'])
self.pid_vz.set_ki(initial_params['pid_vz']['Ki'])
self.pid_vz.set_kd(initial_params['pid_vz']['Kd'])
self.pid_vz.set_lim_high(initial_params['pid_vz']['limit']['high'])
self.pid_vz.set_lim_low(initial_params['pid_vz']['limit']['low'])
#########################################################
#########################################################
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.t_old = 0
self.pub_pid_x = rospy.Publisher('pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('pid_vx',
PIDController,
queue_size=1)
self.pub_pid_y = rospy.Publisher('pid_y', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('pid_vy',
PIDController,
queue_size=1)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz',
PIDController,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref',
Float64,
queue_size=1)
self.euler_ref_pub = rospy.Publisher('euler_ref',
Vector3,
queue_size=1)
# Set up feedback callbacks
if self.feedback_source == 0:
rospy.Subscriber('pose', PoseStamped, self.pose_cb, queue_size=1)
#rospy.Subscriber('odometry', Odometry, self.vel_cb, queue_size=1)
rospy.Subscriber('velocity_relative',
TwistStamped,
self.vel_cb,
queue_size=1)
elif self.feedback_source == 1:
rospy.Subscriber('optitrack/pose',
PoseStamped,
self.optitrack_pose_cb,
queue_size=1)
rospy.Subscriber('optitrack/velocity',
TwistStamped,
self.optitrack_velocity_cb,
queue_size=1)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb, queue_size=1)
rospy.Subscriber('pose_ref', Pose, self.pose_ref_cb, queue_size=1)
rospy.Subscriber('reset_controllers',
Empty,
self.reset_controllers_cb,
queue_size=1)
rospy.Subscriber('trajectory_point_ref', MultiDOFJointTrajectoryPoint,
self.trajectory_point_ref_cb)
#rospy.Subscriber('imu', Imu, self.ahrs_cb, queue_size=1)
#self.pub_gm_mot = rospy.Publisher('collectiveThrust', GmStatus, queue_size=1)
self.cfg_server = Server(PositionCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag and not rospy.is_shutdown():
print 'Waiting for pose measurements.'
rospy.sleep(0.5)
print "Starting position control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
while not self.start_flag and not rospy.is_shutdown():
print 'Waiting for pose measurements.'
rospy.sleep(0.5)
#rospy.sleep(1.0/float(self.rate))
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant referent value for motor velocity should be stored in variable mot_speed.
# When you implement attitude control set the flag self.attitude_ctl to 1
#self.gm_attitude_ctl = 1 # don't forget to set me to 1 when you implement attitude ctl
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
if dt > 1.05 / float(self.rate) or dt < 0.95 / float(self.rate):
#print dt
pass
self.t_old = t
temp_euler_ref = Vector3()
# x
vx_ref = self.pid_x.compute(self.pos_sp.x, self.pos_mv.x, dt)
vx_output = self.pid_vx.compute(
vx_ref + self.Kff_v * self.velocity_ff.x, self.vel_mv.x,
dt) + self.Kff_a * self.acceleration_ff.x / self.g
# y
vy_ref = self.pid_y.compute(self.pos_sp.y, self.pos_mv.y, dt)
vy_output = self.pid_vy.compute(
vy_ref + self.Kff_v * self.velocity_ff.y, self.vel_mv.y,
dt) + self.Kff_a * self.acceleration_ff.y / self.g
# z
vz_ref = self.pid_z.compute(self.pos_sp.z, self.pos_mv.z, dt)
self.mot_speed = (self.mot_speed_hover + \
(self.pid_vz.compute(vz_ref + self.Kff_v*self.velocity_ff.z,
self.vel_mv.z, dt) + \
self.Kff_a*self.z_ff_scaler*self.acceleration_ff.z)) / \
(cos(self.euler_mv.x)*cos(self.euler_mv.y))
"""(cos(0.0*self.euler_mv.x)*cos(0.0*self.euler_mv.y))"""
#print 1/(cos(self.euler_mv.x)*cos(self.euler_mv.y))
########################################################
########################################################
# Publish to euler ref
temp_euler_ref.x = -cos(self.orientation_mv_euler.z)*vy_output \
+ sin(self.orientation_mv_euler.z)*vx_output
temp_euler_ref.y = sin(self.orientation_mv_euler.z)*vy_output \
+ cos(self.orientation_mv_euler.z)*vx_output
euler_sp = tf.transformations.euler_from_quaternion(
(self.orientation_sp.x, self.orientation_sp.y,
self.orientation_sp.z, self.orientation_sp.w))
temp_euler_ref.z = euler_sp[2]
#euler_ref_decoupled = self.qv_mult((self.orientation_mv.x,
#self.orientation_mv.y, self.orientation_mv.z,
#self.orientation_mv.w), (temp_euler_ref.y,
#temp_euler_ref.x, temp_euler_ref.z))
#temp_euler_ref.x = euler_ref_decoupled[1]
#temp_euler_ref.y = euler_ref_decoupled[0]
#temp_euler_ref.z = euler_ref_decoupled[2]
self.euler_ref_pub.publish(temp_euler_ref)
# gas motors are used to control attitude
# publish referent motor velocity to attitude controller
mot_speed_msg = Float64(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_y.publish(self.pid_y.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def reset_controllers_cb(self, msg):
self.start_flag = False
self.pid_x.reset()
self.pid_vx.reset()
self.pid_y.reset()
self.pid_vy.reset()
self.pid_z.reset()
#self.pid_z.ui_old = 22.0
self.pid_vz.reset()
#rospy.Subscriber('odometry', Odometry, self.vel_cb)
rospy.Subscriber('velocity', TwistStamped, self.vel_cb, queue_size=1)
rospy.Subscriber('pose', PoseStamped, self.pose_cb, queue_size=1)
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
if not self.start_flag:
self.start_flag = True
self.pos_mv = msg.pose.position
self.orientation_mv = msg.pose.orientation
temp_euler = tf.transformations.euler_from_quaternion(
(self.orientation_mv.x, self.orientation_mv.y,
self.orientation_mv.z, self.orientation_mv.w))
self.orientation_mv_euler.x = temp_euler[0]
self.orientation_mv_euler.y = temp_euler[1]
self.orientation_mv_euler.z = temp_euler[2]
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
#if not self.start_flag:
# self.start_flag = True
#mv = self.qv_mult((msg.pose.pose.orientation.x,
# msg.pose.pose.orientation.y, msg.pose.pose.orientation.z,
# msg.pose.pose.orientation.w), (msg.twist.twist.linear.x,
# msg.twist.twist.linear.y, msg.twist.twist.linear.z))
"""temp_euler = tf.transformations.euler_from_quaternion((msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y, msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w))
self.orientation_mv_euler.x = temp_euler[0]
self.orientation_mv_euler.y = temp_euler[1]
self.orientation_mv_euler.z = temp_euler[2]
self.vel_mv.x = cos(self.orientation_mv_euler.z)*msg.twist.twist.linear.x \
- sin(self.orientation_mv_euler.z)*msg.twist.twist.linear.y
self.vel_mv.y = sin(self.orientation_mv_euler.z)*msg.twist.twist.linear.x \
+ cos(self.orientation_mv_euler.z)*msg.twist.twist.linear.y
self.vel_mv.z = msg.twist.twist.linear.z
self.orientation_mv = msg.pose.pose.orientation
"""
self.vel_mv.x = cos(self.orientation_mv_euler.z)*msg.twist.linear.x \
- sin(self.orientation_mv_euler.z)*msg.twist.linear.y
self.vel_mv.y = sin(self.orientation_mv_euler.z)*msg.twist.linear.x \
+ cos(self.orientation_mv_euler.z)*msg.twist.linear.y
self.vel_mv.z = msg.twist.linear.z
#temp_mv = Vector3()
#temp_mv.x = mv[0]
#temp_mv.y = mv[1]
#temp_mv.z = mv[2]
#matrix = tf.transformations.quaternion_matrix((msg.pose.pose.orientation.x,
# msg.pose.pose.orientation.y, msg.pose.pose.orientation.z,
# msg.pose.pose.orientation.w))
#self.vel_mv = msg.twist.twist.linear
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz),
qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy),
qw * qw + qx * qx - qy * qy - qz * qz)
self.orientation_mv_euler.x = self.euler_mv.x
self.orientation_mv_euler.y = self.euler_mv.y
self.orientation_mv_euler.z = self.euler_mv.z
def optitrack_pose_cb(self, msg):
if not self.start_flag:
self.start_flag = True
self.pos_mv = msg.pose.position
self.orientation_mv = msg.pose.orientation
temp_euler = tf.transformations.euler_from_quaternion(
(self.orientation_mv.x, self.orientation_mv.y,
self.orientation_mv.z, self.orientation_mv.w))
self.orientation_mv_euler.x = temp_euler[0]
self.orientation_mv_euler.y = temp_euler[1]
self.orientation_mv_euler.z = temp_euler[2]
def optitrack_velocity_cb(self, msg):
self.vel_mv.x = cos(self.orientation_mv_euler.z)*msg.twist.linear.x \
- sin(self.orientation_mv_euler.z)*msg.twist.linear.y
self.vel_mv.y = sin(self.orientation_mv_euler.z)*msg.twist.linear.x \
+ cos(self.orientation_mv_euler.z)*msg.twist.linear.y
self.vel_mv.z = msg.twist.linear.z
def qv_mult(self, q1, v1):
#v1 = tf.transformations.unit_vector(v1)
q2 = list(v1)
q2.append(0.0)
return tf.transformations.quaternion_multiply(
tf.transformations.quaternion_multiply(q1, q2),
tf.transformations.quaternion_conjugate(q1))[:3]
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vx_sp = msg.x
self.vy_sp = msg.y
self.vz_sp = msg.z
def pose_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.pos_sp = msg.position
self.orientation_sp = msg.orientation
def trajectory_point_ref_cb(self, msg):
'''
Callback for one trajectory point with speed and acceleration
'''
# translation is vector3, we need point
self.pos_sp.x = msg.transforms[0].translation.x
self.pos_sp.y = msg.transforms[0].translation.y
self.pos_sp.z = msg.transforms[0].translation.z
# orientation
self.orientation_sp = msg.transforms[0].rotation
# velocity and acceleration
self.velocity_ff = msg.velocities[0].linear
self.acceleration_ff = msg.accelerations[0].linear
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.x_kp = self.pid_x.get_kp()
config.x_ki = self.pid_x.get_ki()
config.x_kd = self.pid_x.get_kd()
config.vx_kp = self.pid_vx.get_kp()
config.vx_ki = self.pid_vx.get_ki()
config.vx_kd = self.pid_vx.get_kd()
config.y_kp = self.pid_y.get_kp()
config.y_ki = self.pid_y.get_ki()
config.y_kd = self.pid_y.get_kd()
config.vy_kp = self.pid_vy.get_kp()
config.vy_ki = self.pid_vy.get_ki()
config.vy_kd = self.pid_vy.get_kd()
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_x.set_kp(config.x_kp)
self.pid_x.set_ki(config.x_ki)
self.pid_x.set_kd(config.x_kd)
self.pid_vx.set_kp(config.vx_kp)
self.pid_vx.set_ki(config.vx_ki)
self.pid_vx.set_kd(config.vx_kd)
self.pid_y.set_kp(config.y_kp)
self.pid_y.set_ki(config.y_ki)
self.pid_y.set_kd(config.y_kd)
self.pid_vy.set_kp(config.vy_kp)
self.pid_vy.set_ki(config.vy_ki)
self.pid_vy.set_kd(config.vy_kd)
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
# this callback should return config data back to server
return config
class PositionControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
/morus/pose - used to extract z-position of the vehicle
/morus/velocity - used to extract vz of the vehicle
/morus/pos_ref - used to set the reference for z-position
/morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
/morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
/morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
# X controller
self.x_sp = 0.0
self.x_mv = 0.0
self.pid_x = PID()
self.vx_sp = 0.0
self.vx_mv = 0.0
self.pid_vx = PID()
# X controller
self.y_sp = 0.0
self.y_mv = 0.0
self.pid_y = PID()
self.vy_sp = 0.0
self.vy_mv = 0.0
self.pid_vy = PID()
# Z controller
self.z_sp = 2.0 # z-position set point
self.z_ref_filt = 0 # z ref filtered
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.pid_vz = PID() # pid instance for z-velocity control
#########################################################
#########################################################
# Add parameters for x controller
self.pid_x.set_kp(0.65) # 0.05
self.pid_x.set_ki(0.0)
self.pid_x.set_kd(0.03) # 0.07
self.pid_x.set_lim_high(500) # max vertical ascent speed
self.pid_x.set_lim_low(-500) # max vertical descent speed
# Add parameters for vx controller
self.pid_vx.set_kp(0.11) # 0.11
self.pid_vx.set_ki(0.0)
self.pid_vx.set_kd(0)
self.pid_vx.set_lim_high(500) # max velocity of a motor
self.pid_vx.set_lim_low(-500) # min velocity of a motor
# Add parameters for y controller
self.pid_y.set_kp(0.65)
self.pid_y.set_ki(0.0) #0.05
self.pid_y.set_kd(0.03) #0.03
self.pid_y.set_lim_high(500) # max vertical ascent speed
self.pid_y.set_lim_low(-500) # max vertical descent speed
# Add parameters for vy controller
self.pid_vy.set_kp(0.11) # 0.11
self.pid_vy.set_ki(0.0)
self.pid_vy.set_kd(0)
self.pid_vy.set_lim_high(500) # max velocity of a motor
self.pid_vy.set_lim_low(-500) # min velocity of a motor
# Add parameters for z controller
self.pid_z.set_kp(100)
self.pid_z.set_ki(10)
self.pid_z.set_kd(100)
self.pid_z.set_lim_high(500) # max vertical ascent speed
self.pid_z.set_lim_low(-500) # max vertical descent speed
# Add parameters for vz controller
self.pid_vz.set_kp(1) #87.2)
self.pid_vz.set_ki(0.0)
self.pid_vz.set_kd(0) #10.89)
self.pid_vz.set_lim_high(500) # max velocity of a motor
self.pid_vz.set_lim_low(-500) # min velocity of a motor
#########################################################
#########################################################
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.t_old = 0
rospy.Subscriber('pose', PoseStamped, self.pose_cb)
rospy.Subscriber('odometry', Odometry, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
rospy.Subscriber('reset_controllers', Empty, self.reset_controllers_cb)
self.pub_pid_x = rospy.Publisher('pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('pid_vx',
PIDController,
queue_size=1)
self.pub_pid_y = rospy.Publisher('pid_y', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('pid_vy',
PIDController,
queue_size=1)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz',
PIDController,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref',
Float64,
queue_size=1)
self.euler_ref_pub = rospy.Publisher('euler_ref',
Vector3,
queue_size=1)
#self.pub_gm_mot = rospy.Publisher('collectiveThrust', GmStatus, queue_size=1)
self.cfg_server = Server(VpcMmcuavPositionCtlParamsConfig,
self.cfg_callback)
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag and not rospy.is_shutdown():
print 'Waiting for pose measurements.'
rospy.sleep(0.5)
print "Starting height control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
while not self.start_flag and not rospy.is_shutdown():
print 'Waiting for pose measurements.'
rospy.sleep(0.5)
rospy.sleep(1.0 / float(self.rate))
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant referent value for motor velocity should be stored in variable mot_speed.
# When you implement attitude control set the flag self.attitude_ctl to 1
#self.gm_attitude_ctl = 1 # don't forget to set me to 1 when you implement attitude ctl
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
if dt > 1.05 / float(self.rate) or dt < 0.95 / float(self.rate):
#print dt
pass
self.t_old = t
temp_euler_ref = Vector3()
vx_ref = self.pid_x.compute(self.x_sp, self.x_mv, dt)
temp_euler_ref.y = self.pid_vx.compute(vx_ref, self.vx_mv, dt)
vy_ref = self.pid_y.compute(self.y_sp, self.y_mv, dt)
temp_euler_ref.x = self.pid_vy.compute(vy_ref, self.vy_mv, dt)
# (m_uav + m_arms)/(C*4)
self.mot_speed_hover = math.sqrt(9.81 * (2.083 + 0.208 * 4 + 0.6) /
(8.54858e-06 * 4.0))
# prefilter for reference
#a = 0.1
#self.z_ref_filt = (1-a) * self.z_ref_filt + a * self.z_sp
vz_ref = self.pid_z.compute(self.z_sp, self.z_mv, dt)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
#print "mot_speed", self.mot_speed
########################################################
########################################################
# Publish to euler ref
self.euler_ref_pub.publish(temp_euler_ref)
# gas motors are used to control attitude
# publish referent motor velocity to attitude controller
mot_speed_msg = Float64(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_y.publish(self.pid_y.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def reset_controllers_cb(self, msg):
self.start_flag = False
self.pid_x.reset()
self.pid_vx.reset()
self.pid_y.reset()
self.pid_vy.reset()
self.pid_z.reset()
self.pid_z.ui_old = 22.0
self.pid_vz.reset()
rospy.Subscriber('odometry', Odometry, self.vel_cb)
rospy.Subscriber('pose', PoseStamped, self.pose_cb)
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
if not self.start_flag:
self.start_flag = True
self.x_mv = msg.pose.position.x
self.y_mv = -msg.pose.position.y
self.z_mv = msg.pose.position.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
#if not self.start_flag:
# self.start_flag = True
self.vx_mv = msg.twist.twist.linear.x
self.vy_mv = -msg.twist.twist.linear.y
self.vz_mv = msg.twist.twist.linear.z
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vx_sp = msg.x
self.vy_sp = msg.y
self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.x_sp = msg.x
self.y_sp = msg.y
self.z_sp = msg.z
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
#print "CFG callback"
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.x_kp = self.pid_x.get_kp()
config.x_ki = self.pid_x.get_ki()
config.x_kd = self.pid_x.get_kd()
config.vx_kp = self.pid_vx.get_kp()
config.vx_ki = self.pid_vx.get_ki()
config.vx_kd = self.pid_vx.get_kd()
config.y_kp = self.pid_y.get_kp()
config.y_ki = self.pid_y.get_ki()
config.y_kd = self.pid_y.get_kd()
config.vy_kp = self.pid_vy.get_kp()
config.vy_ki = self.pid_vy.get_ki()
config.vy_kd = self.pid_vy.get_kd()
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_x.set_kp(config.x_kp)
self.pid_x.set_ki(config.x_ki)
self.pid_x.set_kd(config.x_kd)
self.pid_vx.set_kp(config.vx_kp)
self.pid_vx.set_ki(config.vx_ki)
self.pid_vx.set_kd(config.vx_kd)
self.pid_y.set_kp(config.y_kp)
self.pid_y.set_ki(config.y_ki)
self.pid_y.set_kd(config.y_kd)
self.pid_vy.set_kp(config.vy_kp)
self.pid_vy.set_ki(config.vy_ki)
self.pid_vy.set_kd(config.vy_kd)
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
# this callback should return config data back to server
return config
class HeightControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
/morus/pose - used to extract z-position of the vehicle
/morus/velocity - used to extract vz of the vehicle
/morus/pos_ref - used to set the reference for z-position
/morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
/morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
/morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.z_sp = 2 # z-position set point
self.z_ref_filt = 0 # z ref filtered
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.pid_vz = PID() # pid instance for z-velocity control
#########################################################
#########################################################
# Add parameters for z controller
self.pid_z.set_kp(10)
self.pid_z.set_ki(0.2)
self.pid_z.set_kd(10)
# Add parameters for vz controller
self.pid_vz.set_kp(1) #87.2)
self.pid_vz.set_ki(0.0)
self.pid_vz.set_kd(0) #10.89)
#########################################################
#########################################################
self.pid_z.set_lim_high(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
self.pid_vz.set_lim_high(350) # max velocity of a motor
self.pid_vz.set_lim_low(-350) # min velocity of a motor
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.gm_attitude_ctl = 0 # flag indicates if attitude control is turned on
self.gm_attitude_ctl = rospy.get_param('~gm_attitude_ctl', 0)
self.t_old = 0
rospy.Subscriber('pose', PoseStamped, self.pose_cb)
rospy.Subscriber('velocity', TwistStamped, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz',
PIDController,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref',
Float32,
queue_size=1)
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=1)
self.pub_gm_mot = rospy.Publisher('collectiveThrust',
GmStatus,
queue_size=1)
self.cfg_server = Server(MavZCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(10)
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for pose measurements.'
rospy.sleep(0.5)
print "Starting height control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant referent value for motor velocity should be stored in variable mot_speed.
# When you implement attitude control set the flag self.attitude_ctl to 1
#self.gm_attitude_ctl = 1 # don't forget to set me to 1 when you implement attitude ctl
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
print("Vrijeme je {0}".format(dt))
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
if dt > 0.105 or dt < 0.095:
print dt
self.t_old = t
self.mot_speed_hover = math.sqrt(293 / 0.000456874 / 4)
# prefilter for reference
a = 0.1
self.z_ref_filt = (1 - a) * self.z_ref_filt + a * self.z_sp
vz_ref = self.pid_z.compute(self.z_ref_filt, self.z_mv, dt)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
########################################################
########################################################
if self.gm_attitude_ctl == 0:
# moving masses are used to control attitude
# Publish motor velocities
mot_speed_msg = Actuators()
mot_speed_msg.angular_velocities = [
self.mot_speed, self.mot_speed, self.mot_speed,
self.mot_speed
]
self.pub_mot.publish(mot_speed_msg)
gm_force_msg = GmStatus()
gm_force_msg.force_M = 4 * self.mot_speed * 0.000456874 * self.mot_speed
gm_force_msg.motor_id = 5
self.pub_gm_mot.publish(gm_force_msg)
else:
# gas motors are used to control attitude
# publish referent motor velocity to attitude controller
mot_speed_msg = Float32(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
if not self.start_flag:
self.start_flag = True
self.z_mv = msg.pose.position.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
#if not self.start_flag:
# self.start_flag = True
self.vz_mv = msg.twist.linear.z
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.z_sp = msg.z
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
'''
Class implements MAV attitude control (roll, pitch, yaw). Two PIDs in cascade are
used for each degree of freedom.
Subscribes to:
/morus/imu - used to extract attitude and attitude rate of the vehicle
/morus/mot_vel_ref - used to receive referent motor velocity from the height controller
/morus/euler_ref - used to set the attitude referent (useful for testing controllers)
Publishes:
/morus/command/motors - referent motor velocities sent to each motor controller
/morus/pid_roll - publishes PID-roll data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_roll_rate - publishes PID-roll_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch - publishes PID-pitch data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch_rate - publishes PID-pitch_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw - publishes PID-yaw data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw_rate - publishes PID-yaw_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controllers param online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_roll_rate = PID() # roll rate (wx) controller
self.pid_pitch = PID() # pitch controller
self.pid_pitch_rate = PID() # pitch rate (wy) controller
self.pid_yaw = PID() # yaw controller
self.pid_yaw_rate = PID() # yaw rate (wz) controller
# #################################################################
# #################################################################
# Add your PID params here
# Matknuti integrator
# P - 7-5, D- 5, P - 5, D - 2.5
self.pid_roll.set_kp(3.0)
self.pid_roll.set_ki(0.0)
self.pid_roll.set_kd(0)
self.pid_roll_rate.set_kp(2.5)
self.pid_roll_rate.set_ki(0.0)
self.pid_roll_rate.set_kd(0)
self.pid_roll_rate.set_lim_high(0.3)
self.pid_roll_rate.set_lim_low(-0.3)
self.pid_pitch.set_kp(3)
self.pid_pitch.set_ki(0.0)
self.pid_pitch.set_kd(0)
self.pid_pitch_rate.set_kp(2.5)
self.pid_pitch_rate.set_ki(0.0)
self.pid_pitch_rate.set_kd(0)
self.pid_pitch_rate.set_lim_high(0.3)
self.pid_pitch_rate.set_lim_low(-0.3)
self.pid_yaw.set_kp(0)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0)
self.pid_yaw_rate.set_kp(0)
self.pid_yaw_rate.set_ki(0)
self.pid_yaw_rate.set_kd(0)
# #################################################################
# #################################################################
#self.rate = 100.0
self.rate = 50
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('mot_vel_ref', Float32, self.mot_vel_ref_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
self.euluer_mv_pub = rospy.Publisher('euler_mv', Vector3, queue_size=1)
# Joint publishers
self.pub_joint0_left = \
rospy.Publisher('joint0_left_controller/command', Float64,
queue_size=1)
self.pub_joint1_left = \
rospy.Publisher('joint1_left_controller/command', Float64,
queue_size=1)
self.pub_joint0_right = \
rospy.Publisher('joint0_right_controller/command', Float64,
queue_size=1)
self.pub_joint1_right = \
rospy.Publisher('joint1_right_controller/command', Float64,
queue_size=1)
self.sub_joint0_left = rospy.Subscriber('joint0_left_controller/state', JointControllerState, self.joint0_left_cb)
self.sub_joint1_left = rospy.Subscriber('joint1_left_controller/state', JointControllerState, self.joint1_left_cb)
self.sub_joint0_right = rospy.Subscriber('joint0_right_controller/state', JointControllerState, self.joint0_right_cb)
self.sub_joint1_right = rospy.Subscriber('joint1_right_controller/state', JointControllerState, self.joint1_right_cb)
self.pub_pid_roll = rospy.Publisher('/pid_roll', PIDController,
queue_size=1)
self.pub_pid_roll_rate = rospy.Publisher('pid_roll_rate',
PIDController, queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch',
PIDController, queue_size=1)
self.pub_pid_pitch_rate = rospy.Publisher('pid_pitch_rate',
PIDController, queue_size=1)
self.pub_pid_yaw = rospy.Publisher('pid_yaw', PIDController,
queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate',
PIDController, queue_size=1)
self.cfg_server = Server(MavAttitudeCtlParamsConfig,
self.cfg_callback)
# Length of a single manipulator arm
self.l = 0.5
# Initial offset from the origin of the manipulator arm to the initial position
self.x_offset = 0.515
self.y_offset = 0
# Inizalize refernce filter
self.euler_x_old = 0
self.euler_y_old = 0
def initial_position(self):
print 'Setting initial position'
rospy.sleep(5)
q1_initial = 1.0298
q2_initial = -2.0596
initial_joint0_left_msg = Float64()
initial_joint0_left_msg.data = q1_initial
initial_joint1_left_msg = Float64()
initial_joint1_left_msg.data = q2_initial
initial_joint0_right_msg = Float64()
initial_joint0_right_msg.data = q1_initial
initial_joint1_right_msg = Float64()
initial_joint1_right_msg.data = q2_initial
self.pub_joint0_left.publish(initial_joint0_left_msg)
self.pub_joint1_left.publish(initial_joint1_left_msg)
self.pub_joint0_right.publish(initial_joint0_right_msg)
self.pub_joint1_right.publish(initial_joint1_right_msg)
rospy.sleep(5)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while rospy.get_time() == 0:
print 'Waiting for clock server to start'
print 'Received first clock message - manipulator control'
while not self.start_flag:
print 'Waiting for the first measurement. (1)'
rospy.sleep(0.5)
print 'Starting attitude control.'
self.t_old = rospy.Time.now()
clock_old = self.clock
# self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
self.ros_rate.sleep()
#rospy.sleep(0.02)
if not self.start_flag:
print 'Waiting for the first IMU measurement.'
rospy.sleep(0.5)
else:
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > 1.0 / self.rate + 0.005:
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < 1.0 / self.rate - 0.005:
self.count += 1
print self.count, ' - ', dt_clk
# dt for some reason is 0 sometimes...
# Causes zero division error in controllers
if dt_clk < 10e-10:
dt_clk = 0.05
# First order reference filter - x
if abs(self.euler_x_old - self.euler_sp.x) > 0.03:
a = 0.99
else:
a = 0.9
ref_x = self.euler_x_old + (1-a) * (self.euler_sp.x - self.euler_x_old)
self.euler_x_old = ref_x
roll_rate_sv = self.pid_roll.compute(ref_x,
self.euler_mv.x, dt_clk)
print "x_sp ", ref_x, ' x_mv: ', self.euler_mv.x
# roll rate pid compute
dy_roll = self.pid_roll_rate.compute(roll_rate_sv,
self.euler_rate_mv.x, dt_clk)
# Publish new euler measured values
vectorMsg = Vector3()
vectorMsg.x = self.euler_mv.x
vectorMsg.y = self.euler_mv.y
self.euluer_mv_pub.publish(vectorMsg)
# First order reference filter - y
if abs(self.euler_y_old - self.euler_sp.y) > 0.03:
a = 0.99
else:
a = 0.9
ref_y = self.euler_y_old + (1-a) * (self.euler_sp.y - self.euler_y_old)
self.euler_y_old = ref_y
print "y_sp ", ref_y, ' y_mv: ', self.euler_mv.y
pitch_rate_sv = self.pid_pitch.compute(ref_y,
self.euler_mv.y, dt_clk)
# pitch rate pid compute
dx_pitch = self.pid_pitch_rate.compute(pitch_rate_sv,
self.euler_rate_mv.y, dt_clk)
# Calculating angles - inverse Jacobian
dqL = self.get_new_dqL(dx_pitch, -dy_roll, 0.15)
dqR = self.get_new_dqR(dx_pitch, -dy_roll, 0.15)
# Calculate new joint values
joint0_right_command_msg = Float64()
joint0_right_command_msg.data = self.q1R + dqR[0]
joint1_right_command_msg = Float64()
joint1_right_command_msg.data = self.q2R + dqR[1]
joint0_left_command_msg = Float64()
joint0_left_command_msg.data = self.q1L + dqL[0]
joint1_left_command_msg = Float64()
joint1_left_command_msg.data = self.q2L + dqL[1]
# Publish new joint values
self.pub_joint0_right.publish(joint0_right_command_msg)
self.pub_joint1_right.publish(joint1_right_command_msg)
self.pub_joint0_left.publish(joint0_left_command_msg)
self.pub_joint1_left.publish(joint1_left_command_msg)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
'''
Calculate new joint increments for right manipulator.
@param dx_pitch
@param dy_roll
@param limit Limit maximum pitch and roll values
@return Tuple containing (dq1R, dq2R)
'''
def get_new_dqR(self, dx_pitch, dy_roll, limit):
# Add initial offset - right manipulator
x_right_target = self.x_offset - dx_pitch
y_right_target = self.y_offset - dy_roll
#x_right_target = 2*self.x_offset - self.l * (math.cos(self.q1L + self.q2L) + math.cos(self.q1L))
#y_right_target = - self.l * (math.sin(self.q1L + self.q2L) + math.sin(self.q1L))
# Get joint offsets of the right manipulator
q1R = self.q1R
q2R = self.q2R
print ''
print 'q1R: ', q1R, ' q2L: ', q2R
# Get current position of the right manipulator
x_right_curr = self.l * (math.cos(q1R + q2R) + math.cos(q1R))
y_right_curr = self.l * (math.sin(q1R + q2R) + math.sin(q1R))
print 'x_right_target: ', x_right_target, ' y_right_target: ', y_right_target
print 'x_right_curr: ', x_right_curr, ' y_right_curr: ', y_right_curr
print 'dx: ', - dx_pitch, ' dy: ', - dy_roll
dx_right_pitch = x_right_target - x_right_curr
dy_right_roll = y_right_target - y_right_curr
# Limit dx / dy
dx_right_pitch = self.saturation(dx_right_pitch, limit)
dy_right_roll = self.saturation(dy_right_roll, limit)
print 'dx_new: ', dx_right_pitch, ' dy_new: ', dy_right_roll
# New angle increments (right)
try:
dq1R = math.cos(q1R + q2R) / (self.l * math.sin(q2R)) * (dx_right_pitch) \
+ math.sin(q1R + q2R) / (self.l * math.sin(q2R)) * (dy_right_roll)
dq2R = - (math.cos(q1R + q2R) + math.cos(q1R)) / (self.l * math.sin(q2R)) * (dx_right_pitch) \
- (math.sin(q1R + q2R) + math.sin(q1R)) / (self.l * math.sin(q2R)) * (dy_right_roll)
except ZeroDivisionError:
print "ZeroDivisionError"
#dq2r - any value
dq2r = 0.01
dq1r = dx_right_pitch - math.cos(q2R) / (math.cos(q2R) + 1) * dq2R
print 'dq1R: ', dq1R, ' dq2R: ', dq2R
print ''
return (dq1R, dq2R)
'''
Calculate new joint increments for the left manipulator.
@param dx_pitch
@param dy_roll
@return Tuple containing (dq1L, dq2L)
'''
def get_new_dqL(self, dx_pitch, dy_roll, limit):
# Add initial offset - left manipulator
x_left_target = self.x_offset + dx_pitch
y_left_target = self.y_offset + dy_roll
# Get joint offsets of the left manipulator
q1L = self.q1L
q2L = self.q2L
print ''
print 'q1L: ', q1L, ' q2L: ', q2L
# Get current (x,y) position of the left manipulator
x_left_curr = self.l * (math.cos(q1L + q2L) + math.cos(q1L))
y_left_curr = self.l * (math.sin(q1L + q2L) + math.sin(q1L))
print 'x_left_target: ', x_left_curr, ' y_left_target: ', y_left_target
print 'x_left_curr: ', x_left_curr, ' y_left_curr: ', y_left_curr
print 'dx: ', dx_pitch, ' dy: ', dy_roll
# Calculate distance to the target position
dx_left_pitch = x_left_target - x_left_curr
dy_left_roll = y_left_target - y_left_curr
# Limit dx / dy
dx_left_pitch = self.saturation(dx_left_pitch, limit)
dy_left_roll = self.saturation(dy_left_roll, limit)
print 'dx_new: ', dx_left_pitch, ' dy_new: ', dy_left_roll
# New angle increments (left)
try:
dq1L = ( math.cos(q1L + q2L) / (self.l * math.sin(q2L)) ) * (dx_left_pitch) + \
( math.sin(q1L + q2L) / (self.l * math.sin(q2L)) ) * (dy_left_roll)
dq2L = -( math.cos(q1L + q2L) + math.cos(q1L) ) / (self.l * math.sin(q2L)) * (dx_left_pitch) \
- ( math.sin(q1L + q2L) + math.sin(q1L) ) / (self.l * math.sin(q2L)) * (dy_left_roll)
except ZeroDivisionError:
print "ZeroDivisionError"
# TODO Handle zero division
#dq2r - any value
dq2r = 0.01
dq1r = dx_right_pitch - math.cos(q2R) / (math.cos(q2R) + 1) * dq2R
print 'dq1L: ', dq1L, ' dq2L: ', dq2L
print ''
return (dq1L, dq2L)
def saturation(self, x, limit):
if x > limit:
return limit
if x < (- limit):
return -limit
return x
def mot_vel_ref_cb(self, msg):
'''
Referent motor velocity callback. (This should be published by height controller).
:param msg: Type Float32
'''
self.w_sp = msg.data
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
if not self.start_flag:
self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw
- qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw
+ qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def clock_cb(self, msg):
self.clock = msg
def joint0_left_cb(self, msg):
self.q1L = msg.process_value
def joint1_left_cb(self, msg):
self.q2L = msg.process_value
def joint0_right_cb(self, msg):
self.q1R = msg.process_value
def joint1_right_cb(self, msg):
self.q2R = msg.process_value
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.roll_r_kp = self.pid_roll_rate.get_kp()
config.roll_r_ki = self.pid_roll_rate.get_ki()
config.roll_r_kd = self.pid_roll_rate.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
config.pitch_r_kp = self.pid_pitch_rate.get_kp()
config.pitch_r_ki = self.pid_pitch_rate.get_ki()
config.pitch_r_kd = self.pid_pitch_rate.get_kd()
config.yaw_kp = self.pid_yaw.get_kp()
config.yaw_ki = self.pid_yaw.get_ki()
config.yaw_kd = self.pid_yaw.get_kd()
config.yaw_r_kp = self.pid_yaw_rate.get_kp()
config.yaw_r_ki = self.pid_yaw_rate.get_ki()
config.yaw_r_kd = self.pid_yaw_rate.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_roll_rate.set_kp(config.roll_r_kp)
self.pid_roll_rate.set_ki(config.roll_r_ki)
self.pid_roll_rate.set_kd(config.roll_r_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
self.pid_pitch_rate.set_kp(config.pitch_r_kp)
self.pid_pitch_rate.set_ki(config.pitch_r_ki)
self.pid_pitch_rate.set_kd(config.pitch_r_kd)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_ki(config.yaw_ki)
self.pid_yaw.set_kd(config.yaw_kd)
self.pid_yaw_rate.set_kp(config.yaw_r_kp)
self.pid_yaw_rate.set_ki(config.yaw_r_ki)
self.pid_yaw_rate.set_kd(config.yaw_r_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
'''
Class implements MAV attitude control (roll, pitch, yaw). Two PIDs in cascade are
used for each degree of freedom.
Subscribes to:
/morus/imu - used to extract attitude and attitude rate of the vehicle
/morus/mot_vel_ref - used to receive referent motor velocity from the height controller
/morus/euler_ref - used to set the attitude referent (useful for testing controllers)
Publishes:
/morus/command/motors - referent motor velocities sent to each motor controller
/morus/pid_roll - publishes PID-roll data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_roll_rate - publishes PID-roll_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch - publishes PID-pitch data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch_rate - publishes PID-pitch_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw - publishes PID-yaw data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw_rate - publishes PID-yaw_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controllers param online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_roll_rate = PID() # roll rate (wx) controller
self.pid_pitch = PID() # pitch controller
self.pid_pitch_rate = PID() # pitch rate (wy) controller
self.pid_yaw = PID() # yaw controller
self.pid_yaw_rate = PID() # yaw rate (wz) controller
##################################################################
##################################################################
# Add your PID params here
self.pid_roll.set_kp(3.0)
self.pid_roll.set_ki(1.0)
self.pid_roll.set_kd(0)
self.pid_roll_rate.set_kp(2.5)
self.pid_roll_rate.set_ki(0.0)
self.pid_roll_rate.set_kd(0)
self.pid_roll_rate.set_lim_high(0.3)
self.pid_roll_rate.set_lim_low(-0.3)
self.pid_pitch.set_kp(3.0)
self.pid_pitch.set_ki(1.0)
self.pid_pitch.set_kd(0)
self.pid_pitch_rate.set_kp(2.5)
self.pid_pitch_rate.set_ki(0.0)
self.pid_pitch_rate.set_kd(0)
self.pid_pitch_rate.set_lim_high(0.3)
self.pid_pitch_rate.set_lim_low(-0.3)
self.pid_yaw.set_kp(1.0)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0)
self.pid_yaw_rate.set_kp(1.0)
self.pid_yaw_rate.set_ki(0)
self.pid_yaw_rate.set_kd(0)
##################################################################
##################################################################
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('mot_vel_ref', Float32, self.mot_vel_ref_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
#self.pub_mass0 = rospy.Publisher('movable_mass_0_position_controller/command', Float64, queue_size=1)
#self.pub_mass1 = rospy.Publisher('movable_mass_1_position_controller/command', Float64, queue_size=1)
#self.pub_mass2 = rospy.Publisher('movable_mass_2_position_controller/command', Float64, queue_size=1)
#self.pub_mass3 = rospy.Publisher('movable_mass_3_position_controller/command', Float64, queue_size=1)
self.attitude_pub = rospy.Publisher('attitude_command', Vector3Stamped, queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll', PIDController, queue_size=1)
self.pub_pid_roll_rate = rospy.Publisher('pid_roll_rate', PIDController, queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch', PIDController, queue_size=1)
self.pub_pid_pitch_rate = rospy.Publisher('pid_pitch_rate', PIDController, queue_size=1)
self.pub_pid_yaw = rospy.Publisher('pid_yaw', PIDController, queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate', PIDController, queue_size=1)
self.cfg_server = Server(MmuavAttitudeCtlParamsConfig, self.cfg_callback)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while (rospy.get_time() == 0) and (not rospy.is_shutdown()):
print 'Waiting for clock server to start'
print 'Received first clock message'
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
#self.ros_rate.sleep()
rospy.sleep(1.0/float(self.rate))
if not self.start_flag:
"Waiting for the first IMU measurement."
rospy.sleep(0.5)
else:
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > (1.0 / self.rate + 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < (1.0 / self.rate - 0.005):
self.count += 1
print self.count, ' - ', dt_clk
# Roll
roll_rate_sv = self.pid_roll.compute(self.euler_sp.x, self.euler_mv.x, dt_clk)
# roll rate pid compute
roll_rate_output = self.pid_roll_rate.compute(roll_rate_sv, self.euler_rate_mv.x, dt_clk)
# Pitch
pitch_rate_sv = self.pid_pitch.compute(self.euler_sp.y, self.euler_mv.y, dt_clk)
# pitch rate pid compute
pitch_rate_output = self.pid_pitch_rate.compute(pitch_rate_sv, self.euler_rate_mv.y, dt_clk)
# Yaw
yaw_rate_sv = self.pid_yaw.compute(self.euler_sp.z, self.euler_mv.z, dt_clk)
# yaw rate pid compute
yaw_rate_output = self.pid_yaw_rate.compute(yaw_rate_sv, self.euler_rate_mv.z, dt_clk)
# Publish mass position
#mass0_command_msg = Float64()
#mass0_command_msg.data = dx_pitch
#mass2_command_msg = Float64()
#mass2_command_msg.data = -dx_pitch
#mass1_command_msg = Float64()
#mass1_command_msg.data = -dy_roll
#mass3_command_msg = Float64()
#mass3_command_msg.data = dy_roll
#self.pub_mass0.publish(mass0_command_msg)
#self.pub_mass1.publish(mass1_command_msg)
#self.pub_mass2.publish(mass2_command_msg)
#self.pub_mass3.publish(mass3_command_msg)
# Publish attitude
attitude_output = Vector3Stamped()
attitude_output.vector.x = roll_rate_output
attitude_output.vector.y = pitch_rate_output
attitude_output.vector.z = yaw_rate_output
attitude_output.header.stamp = rospy.Time.now()
self.attitude_pub.publish(attitude_output)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
def mot_vel_ref_cb(self, msg):
'''
Referent motor velocity callback. (This should be published by height controller).
:param msg: Type Float32
'''
self.w_sp = msg.data
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
if not self.start_flag:
self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.yaw
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def clock_cb(self, msg):
self.clock = msg
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.roll_r_kp = self.pid_roll_rate.get_kp()
config.roll_r_ki = self.pid_roll_rate.get_ki()
config.roll_r_kd = self.pid_roll_rate.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
config.pitch_r_kp = self.pid_pitch_rate.get_kp()
config.pitch_r_ki = self.pid_pitch_rate.get_ki()
config.pitch_r_kd = self.pid_pitch_rate.get_kd()
config.yaw_kp = self.pid_yaw.get_kp()
config.yaw_ki = self.pid_yaw.get_ki()
config.yaw_kd = self.pid_yaw.get_kd()
config.yaw_r_kp = self.pid_yaw_rate.get_kp()
config.yaw_r_ki = self.pid_yaw_rate.get_ki()
config.yaw_r_kd = self.pid_yaw_rate.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_roll_rate.set_kp(config.roll_r_kp)
self.pid_roll_rate.set_ki(config.roll_r_ki)
self.pid_roll_rate.set_kd(config.roll_r_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
self.pid_pitch_rate.set_kp(config.pitch_r_kp)
self.pid_pitch_rate.set_ki(config.pitch_r_ki)
self.pid_pitch_rate.set_kd(config.pitch_r_kd)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_ki(config.yaw_ki)
self.pid_yaw.set_kd(config.yaw_kd)
self.pid_yaw_rate.set_kp(config.yaw_r_kp)
self.pid_yaw_rate.set_ki(config.yaw_r_ki)
self.pid_yaw_rate.set_kd(config.yaw_r_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
'''
Class implements MAV attitude control (roll, pitch, yaw). Two PIDs in cascade are
used for each degree of freedom.
Subscribes to:
/morus/imu - used to extract attitude and attitude rate of the vehicle
/morus/mot_vel_ref - used to receive referent motor velocity from the height controller
/morus/euler_ref - used to set the attitude referent (useful for testing controllers)
Publishes:
/morus/command/motors - referent motor velocities sent to each motor controller
/morus/pid_roll - publishes PID-roll data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_roll_rate - publishes PID-roll_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch - publishes PID-pitch data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch_rate - publishes PID-pitch_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw - publishes PID-yaw data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw_rate - publishes PID-yaw_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controllers param online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.euler_sp_old = Vector3(0, 0, 0)
self.euler_sp_filt = Vector3(0, 0, 0)
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.euler_rate_mv_old = Vector3()
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_roll_rate = PID() # roll rate (wx) controller
self.pid_pitch = PID() # pitch controller
self.pid_pitch_rate = PID() # pitch rate (wy) controller
self.pid_yaw = PID() # yaw controller
self.pid_yaw_rate = PID() # yaw rate (wz) controller
# Adding VPC controllers for roll and pitch
self.pid_vpc_roll = PID()
self.pid_vpc_pitch = PID()
##################################################################
##################################################################
# Add your PID params here
self.pid_roll.set_kp(7.0)
self.pid_roll.set_ki(0.5)
self.pid_roll.set_kd(0.0)
self.pid_roll_rate.set_kp(190.0)
self.pid_roll_rate.set_ki(15.0)
self.pid_roll_rate.set_kd(3.0)
self.pid_roll_rate.set_lim_high(1450.0)
self.pid_roll_rate.set_lim_low(-1450.0)
self.pid_pitch.set_kp(7.0)
self.pid_pitch.set_ki(0.5)
self.pid_pitch.set_kd(0.0)
self.pid_pitch_rate.set_kp(190.0)
self.pid_pitch_rate.set_ki(15.0)
self.pid_pitch_rate.set_kd(3.0)
self.pid_pitch_rate.set_lim_high(1450.0)
self.pid_pitch_rate.set_lim_low(-1450.0)
self.pid_yaw.set_kp(5.0)
self.pid_yaw.set_ki(0.0)
self.pid_yaw.set_kd(0.0)
self.pid_yaw_rate.set_kp(180.0)
self.pid_yaw_rate.set_ki(0.0)
self.pid_yaw_rate.set_kd(0.0)
self.pid_yaw_rate.set_lim_high(1450.0)
self.pid_yaw_rate.set_lim_low(-1450.0)
# VPC pids
self.pid_vpc_roll.set_kp(0)
self.pid_vpc_roll.set_ki(0.0)
self.pid_vpc_roll.set_kd(0)
self.pid_vpc_roll.set_lim_high(0.04)
self.pid_vpc_roll.set_lim_low(-0.04)
self.pid_vpc_pitch.set_kp(0)
self.pid_vpc_pitch.set_ki(0.0)
self.pid_vpc_pitch.set_kd(0)
self.pid_vpc_pitch.set_lim_high(0.04)
self.pid_vpc_pitch.set_lim_low(-0.04)
# Filter parameters
self.rate_mv_filt_K = 1.0
self.rate_mv_filt_T = 0.02
# Reference prefilters
self.roll_reference_prefilter_K = 1.0
self.roll_reference_prefilter_T = 0.0
self.pitch_reference_prefilter_K = 1.0
self.pitch_reference_prefilter_T = 0.0
# Offsets for pid outputs
self.roll_rate_output_trim = 0.0
self.pitch_rate_output_trim = 0.0
##################################################################
##################################################################
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.Ts = 1.0/float(self.rate)
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('mot_vel_ref', Float64, self.mot_vel_ref_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
rospy.Subscriber('reset_controllers', Empty, self.reset_controllers_cb)
self.attitude_pub = rospy.Publisher('attitude_command', Float64MultiArray, queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll', PIDController, queue_size=1)
self.pub_pid_roll_rate = rospy.Publisher('pid_roll_rate', PIDController, queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch', PIDController, queue_size=1)
self.pub_pid_pitch_rate = rospy.Publisher('pid_pitch_rate', PIDController, queue_size=1)
self.pub_pid_yaw = rospy.Publisher('pid_yaw', PIDController, queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate', PIDController, queue_size=1)
self.pub_pid_vpc_roll = rospy.Publisher('pid_vpc_roll', PIDController, queue_size=1)
self.pub_pid_vpc_pitch = rospy.Publisher('pid_vpc_pitch', PIDController, queue_size=1)
self.cfg_server = Server(VpcMmuavAttitudeCtlParamsConfig, self.cfg_callback)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while (rospy.get_time() == 0) and (not rospy.is_shutdown()):
print 'Waiting for clock server to start'
print 'Received first clock message'
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
#self.ros_rate.sleep()
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
rospy.sleep(1.0/float(self.rate))
self.euler_sp_filt.x = simple_filters.filterPT1(self.euler_sp_old.x,
self.euler_sp.x, self.roll_reference_prefilter_T, self.Ts,
self.roll_reference_prefilter_K)
self.euler_sp_filt.y = simple_filters.filterPT1(self.euler_sp_old.y,
self.euler_sp.y, self.pitch_reference_prefilter_T, self.Ts,
self.pitch_reference_prefilter_K)
self.euler_sp_filt.z = self.euler_sp.z
#self.euler_sp.z = simple_filters.filterPT1(self.euler_sp_old.z, self.euler_sp.z, 0.2, self.Ts, 1.0)
self.euler_sp_old = copy.deepcopy(self.euler_sp_filt)
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > (1.0 / self.rate + 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < (1.0 / self.rate - 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < 0.005:
dt_clk = 0.01
# Roll
roll_rate_sv = self.pid_roll.compute(self.euler_sp_filt.x, self.euler_mv.x, dt_clk)
# roll rate pid compute
roll_rate_output = self.pid_roll_rate.compute(roll_rate_sv, self.euler_rate_mv.x, dt_clk) + self.roll_rate_output_trim
# Pitch
pitch_rate_sv = self.pid_pitch.compute(self.euler_sp_filt.y, self.euler_mv.y, dt_clk)
# pitch rate pid compute
pitch_rate_output = self.pid_pitch_rate.compute(pitch_rate_sv, self.euler_rate_mv.y, dt_clk) + self.pitch_rate_output_trim
# Yaw
yaw_rate_sv = self.pid_yaw.compute(self.euler_sp_filt.z, self.euler_mv.z, dt_clk)
# yaw rate pid compute
yaw_rate_output = self.pid_yaw_rate.compute(yaw_rate_sv, self.euler_rate_mv.z, dt_clk)
# VPC stuff
vpc_roll_output = -self.pid_vpc_roll.compute(0.0, roll_rate_output, dt_clk)
# Due to some wiring errors we set output to +, should be -
vpc_pitch_output = -self.pid_vpc_pitch.compute(0.0, pitch_rate_output, dt_clk)
# Publish mass position
#mass0_command_msg = Float64()
#mass0_command_msg.data = dx_pitch
#mass2_command_msg = Float64()
#mass2_command_msg.data = -dx_pitch
#mass1_command_msg = Float64()
#mass1_command_msg.data = -dy_roll
#mass3_command_msg = Float64()
#mass3_command_msg.data = dy_roll
#self.pub_mass0.publish(mass0_command_msg)
#self.pub_mass1.publish(mass1_command_msg)
#self.pub_mass2.publish(mass2_command_msg)
#self.pub_mass3.publish(mass3_command_msg)
# Publish attitude
attitude_output = Float64MultiArray()
attitude_output.data = [roll_rate_output, pitch_rate_output, \
yaw_rate_output, vpc_roll_output, vpc_pitch_output]
self.attitude_pub.publish(attitude_output)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
# Publish VPC pid data
self.pub_pid_vpc_roll.publish(self.pid_vpc_roll.create_msg())
self.pub_pid_vpc_pitch.publish(self.pid_vpc_pitch.create_msg())
def mot_vel_ref_cb(self, msg):
'''
Referent motor velocity callback. (This should be published by height controller).
:param msg: Type Float32
'''
self.w_sp = msg.data
def reset_controllers_cb(self, msg):
self.start_flag = False
self.pid_pitch.reset()
self.pid_pitch_rate.reset()
self.pid_roll.reset()
self.pid_roll_rate.reset()
self.pid_yaw.reset()
self.pid_yaw_rate.reset()
self.pid_vpc_pitch.reset()
self.pid_vpc_roll.reset()
rospy.Subscriber('imu', Imu, self.ahrs_cb)
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
# If we are in first pass initialize filter
if not self.start_flag:
self.start_flag = True
self.euler_rate_mv_old = copy.deepcopy(self.euler_rate_mv)
# Filtering angular velocities
self.euler_rate_mv.x = simple_filters.filterPT1(self.euler_rate_mv_old.x,
self.euler_rate_mv.x, self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
self.euler_rate_mv.y = simple_filters.filterPT1(self.euler_rate_mv_old.y,
self.euler_rate_mv.y, self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
self.euler_rate_mv.z = simple_filters.filterPT1(self.euler_rate_mv_old.z,
self.euler_rate_mv.z, self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
# Set old to current
self.euler_rate_mv_old = copy.deepcopy(self.euler_rate_mv)
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def clock_cb(self, msg):
self.clock = msg
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.roll_r_kp = self.pid_roll_rate.get_kp()
config.roll_r_ki = self.pid_roll_rate.get_ki()
config.roll_r_kd = self.pid_roll_rate.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
config.pitch_r_kp = self.pid_pitch_rate.get_kp()
config.pitch_r_ki = self.pid_pitch_rate.get_ki()
config.pitch_r_kd = self.pid_pitch_rate.get_kd()
config.yaw_kp = self.pid_yaw.get_kp()
config.yaw_ki = self.pid_yaw.get_ki()
config.yaw_kd = self.pid_yaw.get_kd()
config.yaw_r_kp = self.pid_yaw_rate.get_kp()
config.yaw_r_ki = self.pid_yaw_rate.get_ki()
config.yaw_r_kd = self.pid_yaw_rate.get_kd()
# VPC pids
config.vpc_roll_kp = self.pid_vpc_roll.get_kp()
config.vpc_roll_ki = self.pid_vpc_roll.get_ki()
config.vpc_roll_kd = self.pid_vpc_roll.get_kd()
config.vpc_pitch_kp = self.pid_vpc_pitch.get_kp()
config.vpc_pitch_ki = self.pid_vpc_pitch.get_ki()
config.vpc_pitch_kd = self.pid_vpc_pitch.get_kd()
# Rate filter
config.rate_mv_filt_K = self.rate_mv_filt_K
config.rate_mv_filt_T = self.rate_mv_filt_T
# Roll and pitch reference prefilters
config.roll_reference_prefilter_K = self.roll_reference_prefilter_K
config.roll_reference_prefilter_T = self.roll_reference_prefilter_T
config.pitch_reference_prefilter_K = self.pitch_reference_prefilter_K
config.pitch_reference_prefilter_T = self.pitch_reference_prefilter_T
# Rate output offsets
config.roll_rate_output_trim = self.roll_rate_output_trim
config.pitch_rate_output_trim = self.pitch_rate_output_trim
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_roll_rate.set_kp(config.roll_r_kp)
self.pid_roll_rate.set_ki(config.roll_r_ki)
self.pid_roll_rate.set_kd(config.roll_r_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
self.pid_pitch_rate.set_kp(config.pitch_r_kp)
self.pid_pitch_rate.set_ki(config.pitch_r_ki)
self.pid_pitch_rate.set_kd(config.pitch_r_kd)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_ki(config.yaw_ki)
self.pid_yaw.set_kd(config.yaw_kd)
self.pid_yaw_rate.set_kp(config.yaw_r_kp)
self.pid_yaw_rate.set_ki(config.yaw_r_ki)
self.pid_yaw_rate.set_kd(config.yaw_r_kd)
# VPC pids
self.pid_vpc_roll.set_kp(config.vpc_roll_kp)
self.pid_vpc_roll.set_ki(config.vpc_roll_ki)
self.pid_vpc_roll.set_kd(config.vpc_roll_kd)
self.pid_vpc_pitch.set_kp(config.vpc_pitch_kp)
self.pid_vpc_pitch.set_ki(config.vpc_pitch_ki)
self.pid_vpc_pitch.set_kd(config.vpc_pitch_kd)
# Rate filter
self.rate_mv_filt_K = config.rate_mv_filt_K
self.rate_mv_filt_T = config.rate_mv_filt_T
# Roll and pitch reference prefilters
self.roll_reference_prefilter_K = config.roll_reference_prefilter_K
self.roll_reference_prefilter_T = config.roll_reference_prefilter_T
self.pitch_reference_prefilter_K = config.pitch_reference_prefilter_K
self.pitch_reference_prefilter_T = config.pitch_reference_prefilter_T
# Rate output offsets
self.roll_rate_output_trim = config.roll_rate_output_trim
self.pitch_rate_output_trim = config.pitch_rate_output_trim
# this callback should return config data back to server
return config
class HorizontalControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
pose - used to extract z-position of the vehicle
velocity - used to extract vz of the vehicle
pos_ref - used to set the reference for z-position
vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
euler_ref - publishes referent value for euler angles (roll, pitch, yaw)
pid_x - publishes PID-x data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_vx - publishes PID-vx data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_y - publishes PID-x data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_vy - publishes PID-vx data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.yaw_mv = 0 # measured yaw value
self.x_sp = 0 # x-position set point
self.x_mv = 0 # x-position measured value
self.pid_x = PID() # pid instance for x control
self.vx_sp = 0 # vx velocity set_point
self.vx_mv = 0 # vx velocity measured value
self.pid_vx = PID() # pid instance for x-velocity control
self.y_sp = 0 # y-position set point
self.y_mv = 0 # y-position measured value
self.pid_y = PID() # pid instance for y control
self.vy_sp = 0 # vy velocity set_point
self.vy_mv = 0 # vy velocity measured value
self.pid_vy = PID() # pid instance for y-velocity control
# PID controllers for z-axis and yaw
self.z_sp = 1
self.z_mv = 0
self.pid_z = PID()
self.yaw_sp = 0
self.pid_yaw = PID()
#########################################################
#########################################################
# Add parameters for vx controller
self.pid_vx.set_kp(0.468)
self.pid_vx.set_ki(0.09)
self.pid_vx.set_kd(0.0)
# Add parameters for x controller
self.pid_x.set_kp(0.8)
self.pid_x.set_ki(0.0)
self.pid_x.set_kd(0.0)
# Add parameters for vy controller
self.pid_vy.set_kp(0.468)
self.pid_vy.set_ki(0.09)
self.pid_vy.set_kd(0.0)
# Add parameters for y controller
self.pid_y.set_kp(0.8)
self.pid_y.set_ki(0.0)
self.pid_y.set_kd(0.0)
#########################################################
#########################################################
# Add parameters for z controller
self.pid_z.set_kp(0.4)
self.pid_z.set_ki(0.05)
self.pid_z.set_kd(0.1)
# Add parameters for yaw controller
self.pid_yaw.set_kp(1)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0)
self.pid_x.set_lim_up(5) # max vx speed
self.pid_x.set_lim_low(-5) # min vx speed
self.pid_vx.set_lim_up(45.0 / 180 * math.pi) # max pitch angle
self.pid_vx.set_lim_low(-45.0 / 180 * math.pi) # min pitch angle
self.pid_y.set_lim_up(5) # max vy speed
self.pid_y.set_lim_low(-5) # min vy speed
self.pid_vy.set_lim_up(45.0 / 180 * math.pi) # max roll angle
self.pid_vy.set_lim_low(-45.0 / 180 * math.pi) # min roll angle
self.pid_z.set_lim_up(1)
self.pid_z.set_lim_low(-1)
self.pid_yaw.set_lim_up(1)
self.pid_yaw.set_lim_low(-1)
rospy.Subscriber('Optitrack', PoseStamped, self.pose_cb)
rospy.Subscriber('Optitrack_vel', TwistStamped, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
self.pub_pid_x = rospy.Publisher('pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('pid_vx',
PIDController,
queue_size=1)
self.pub_pid_y = rospy.Publisher('pid_y', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('pid_vy',
PIDController,
queue_size=1)
self.euler_ref_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.takeoffPub = rospy.Publisher("takeoff", Empty, queue_size=1)
self.landPub = rospy.Publisher("land", Empty, queue_size=1)
self.resetPub = rospy.Publisher("reset", Empty, queue_size=1)
self.cfg_server = Server(MavXYCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(10)
self.t_start = rospy.Time.now()
# Joy stuff
self.joyMasterFlag = False
rospy.Subscriber("/joy", Joy, self.JoyCallback)
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting horizontal control."
while not rospy.is_shutdown():
self.ros_rate.sleep()
# If joystick control is disabled automatic control is on
if self.joyMasterFlag == False:
########################################################
########################################################
# Implement cascade PID control here.
# Reference for x is stored in self.x_sp.
# Measured x-position is stored in self.x_mv.
# If you want to test only vx - controller, the corresponding reference is stored in self.vx_sp.
# Measured vx-velocity is stored in self.vx_mv
# Reference for y is stored in self.y_sp.
# Measured y-position is stored in self.y_mv.
# If you want to test only vy - controller, the corresponding reference is stored in self.vy_sp.
# Measured vx-velocity is stored in self.vy_mv
# Resultant referent value for roll and pitch (in mobile coordinate system!)
# should be stored in variable roll_sp and pitch_sp
vx_sp = self.pid_x.compute(self.x_sp, self.x_mv)
pitch_sp = self.pid_vx.compute(vx_sp, self.vx_mv)
vy_sp = self.pid_y.compute(self.y_sp, self.y_mv)
roll_sp = self.pid_vy.compute(vy_sp, self.vy_mv)
########################################################
########################################################
# Z and YAW controllers
thrust_sp = self.pid_z.compute(self.z_sp, self.z_mv)
yaw = self.pid_yaw.compute(self.yaw_sp, self.yaw_mv)
euler_sv = Twist()
# Scaling roll and pitch values to 1
euler_sv.linear.y = roll_sp / 0.785398
euler_sv.linear.x = pitch_sp / 0.785398
euler_sv.linear.z = thrust_sp
euler_sv.angular.z = yaw
self.euler_ref_pub.publish(euler_sv)
# Publish PID data - could be usefule for tuning
self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_y.publish(self.pid_y.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
else:
self.pid_x.reset()
self.pid_y.reset()
self.pid_vx.reset()
self.pid_vy.reset()
self.pid_z.reset()
self.pid_yaw.reset()
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
self.x_mv = msg.pose.position.x
self.y_mv = msg.pose.position.y
self.z_mv = msg.pose.position.z
qx = msg.pose.orientation.x
qy = msg.pose.orientation.y
qz = msg.pose.orientation.z
qw = msg.pose.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
#self.roll_mv = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
#self.pitch_mv = math.asin(2 * (qw * qy - qx * qz))
#self.yaw_mv = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
self.yaw_mv = msg.pose.orientation.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
if not self.start_flag:
self.start_flag = True
self.vx_mv = msg.twist.linear.x
self.vy_mv = msg.twist.linear.y
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vx_sp = msg.x
self.vy_sp = msg.y
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.x_sp = msg.x
self.y_sp = msg.y
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.x_kp = self.pid_x.get_kp()
config.x_ki = self.pid_x.get_ki()
config.x_kd = self.pid_x.get_kd()
config.vx_kp = self.pid_vx.get_kp()
config.vx_ki = self.pid_vx.get_ki()
config.vx_kd = self.pid_vx.get_kd()
config.y_kp = self.pid_y.get_kp()
config.y_ki = self.pid_y.get_ki()
config.y_kd = self.pid_y.get_kd()
config.vy_kp = self.pid_vy.get_kp()
config.vy_ki = self.pid_vy.get_ki()
config.vy_kd = self.pid_vy.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_x.set_kp(config.x_kp)
self.pid_x.set_ki(config.x_ki)
self.pid_x.set_kd(config.x_kd)
self.pid_vx.set_kp(config.vx_kp)
self.pid_vx.set_ki(config.vx_ki)
self.pid_vx.set_kd(config.vx_kd)
self.pid_y.set_kp(config.y_kp)
self.pid_y.set_ki(config.y_ki)
self.pid_y.set_kd(config.y_kd)
self.pid_vy.set_kp(config.vy_kp)
self.pid_vy.set_ki(config.vy_ki)
self.pid_vy.set_kd(config.vy_kd)
# this callback should return config data back to server
return config
def JoyCallback(self, data):
self.joyMasterData = data
parrotCmdVel = Twist()
if data.buttons[4] == 1:
self.joyMasterFlag = True
else:
self.joyMasterFlag = False
if self.joyMasterFlag == True:
parrotCmdVel.linear.x = data.axes[3]
parrotCmdVel.linear.y = data.axes[2]
parrotCmdVel.linear.z = data.axes[1]
parrotCmdVel.angular.z = data.axes[0]
self.euler_ref_pub.publish(parrotCmdVel)
if data.buttons[7] == 1:
self.takeoffPub.publish(Empty())
self.x_sp = self.x_mv
self.y_sp = self.y_mv
if data.buttons[6] == 1:
self.landPub.publish(Empty())
if data.buttons[8] == 1:
self.resetPub.publish(Empty())
class HorizontalControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
pose - used to extract x any y position of the vehicle
euler - used to extract measured yaw
velocity - used to extract vx and vy of the vehicle
pos_ref - used to set the reference for x and y -position
vel_ref - used to set the reference for vx and vy-position (useful for testing velocity controller)
Publishes:
euler_ref - publishes referent value for euler angles (roll, pitch, yaw)
pid_x - publishes PID-x data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_vx - publishes PID-vx data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_y - publishes PID-x data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_vy - publishes PID-vx data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.yaw_sp = 0 # referent yaw value
self.yaw_mv = 0 # measured yaw value
self.x_sp = 0 # x-position set point
self.x_mv = 0 # x-position measured value
self.pid_x = PID() # pid instance for x control
self.vx_sp = 0 # vx velocity set_point
self.vx_mv = 0 # vx velocity measured value
self.pid_vx = PID() # pid instance for x-velocity control
self.y_sp = 0 # y-position set point
self.y_mv = 0 # y-position measured value
self.pid_y = PID() # pid instance for y control
self.vy_sp = 0 # vy velocity set_point
self.vy_mv = 0 # vy velocity measured value
self.pid_vy = PID() # pid instance for y-velocity control
#########################################################
#########################################################
# Add parameters for vx controller
self.pid_vx.set_kp(0.982)
self.pid_vx.set_ki(0.20)
self.pid_vx.set_kd(0.0)
# Add parameters for x controller
self.pid_x.set_kp(0.7)
self.pid_x.set_ki(0.0)
self.pid_x.set_kd(0.0)
# Add parameters for vy controller
self.pid_vy.set_kp(0.982)
self.pid_vy.set_ki(0.20)
self.pid_vy.set_kd(0.0)
# Add parameters for y controller
self.pid_y.set_kp(0.7)
self.pid_y.set_ki(0.0)
self.pid_y.set_kd(0.000)
#########################################################
#########################################################
self.pid_x.set_lim_up(15) # max vx speed
self.pid_x.set_lim_low(-15) # min vx speed
self.pid_vx.set_lim_up(45.0 / 180 * math.pi) # max pitch angle
self.pid_vx.set_lim_low(-45.0 / 180 * math.pi) # min pitch angle
self.pid_y.set_lim_up(15) # max vy speed
self.pid_y.set_lim_low(-15) # min vy speed
self.pid_vy.set_lim_up(45.0 / 180 * math.pi) # max roll angle
self.pid_vy.set_lim_low(-45.0 / 180 * math.pi) # min roll angle
self.quaternion = numpy.empty((4, ), dtype=numpy.float64)
self.velocity_b = numpy.empty((3, ), dtype=numpy.float64)
self.velocity_i = numpy.empty((3, ), dtype=numpy.float64)
rospy.Subscriber('odometry', Odometry, self.odometry_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
rospy.Subscriber('yaw_ref', Float32, self.yaw_ref_cb)
self.pub_pid_x = rospy.Publisher('pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('pid_vx',
PIDController,
queue_size=1)
self.pub_pid_y = rospy.Publisher('pid_y', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('pid_vy',
PIDController,
queue_size=1)
self.euler_ref_pub = rospy.Publisher('euler_ref',
Vector3,
queue_size=1)
self.cfg_server = Server(MavXYCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(20)
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting horizontal control."
while not rospy.is_shutdown():
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for x is stored in self.x_sp.
# Measured x-position is stored in self.x_mv.
# If you want to test only vx - controller, the corresponding reference is stored in self.vx_sp.
# Measured vx-velocity is stored in self.vx_mv
# Reference for y is stored in self.y_sp.
# Measured y-position is stored in self.y_mv.
# If you want to test only vy - controller, the corresponding reference is stored in self.vy_sp.
# Measured vx-velocity is stored in self.vy_mv
# Measured yaw angle is stored in self.yaw_mv (in rad)
# Resultant referent value for roll and pitch (in mobile coordinate system!)
# should be stored in variable roll_sp and pitch_sp
#x component
self.vx_sp = self.pid_x.compute(self.x_sp, self.x_mv)
pitch_sp = self.pid_vx.compute(self.vx_sp, self.vx_mv)
#y component
self.vy_sp = self.pid_y.compute(self.y_sp, self.y_mv)
roll_sp = -self.pid_vy.compute(self.vy_sp, self.vy_mv)
print("------------------")
print('PITCH >>', pitch_sp)
########################################################
########################################################
euler_sv = Vector3(roll_sp, pitch_sp, self.yaw_sp)
self.euler_ref_pub.publish(euler_sv)
# Publish PID data - could be usefule for tuning
self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_y.publish(self.pid_y.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
def odometry_cb(self, msg):
'''
Odometry (6DOF - position and orientation and velocities) callback.
:param msg: Type Odometry
'''
if not self.start_flag:
self.start_flag = True
self.x_mv = msg.pose.pose.position.x
self.y_mv = msg.pose.pose.position.y
# transform linear velocity from body frame to inertial frame
self.quaternion[0] = msg.pose.pose.orientation.x
self.quaternion[1] = msg.pose.pose.orientation.y
self.quaternion[2] = msg.pose.pose.orientation.z
self.quaternion[3] = msg.pose.pose.orientation.w
self.velocity_b[0] = msg.twist.twist.linear.x
self.velocity_b[1] = msg.twist.twist.linear.y
self.velocity_b[2] = msg.twist.twist.linear.z
Rib = transformations.quaternion_matrix(self.quaternion)
Rv_b = transformations.translation_matrix(self.velocity_b)
Rv_i = numpy.dot(Rib, Rv_b)
self.velocity_i = transformations.translation_from_matrix(Rv_i)
self.vx_mv = self.velocity_i[0]
self.vy_mv = self.velocity_i[1]
qx = msg.pose.pose.orientation.x
qy = msg.pose.pose.orientation.y
qz = msg.pose.pose.orientation.z
qw = msg.pose.pose.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
#self.roll_mv = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
#self.pitch_mv = math.asin(2 * (qw * qy - qx * qz))
self.yaw_mv = math.atan2(2 * (qw * qz + qx * qy),
qw * qw + qx * qx - qy * qy - qz * qz)
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vx_sp = msg.x
self.vy_sp = msg.y
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.x_sp = msg.x
self.y_sp = msg.y
def yaw_ref_cb(self, msg):
'''
Referent yaw callback. Received value is used as a referent yaw (heading).
:param msg: Type Float32
'''
self.yaw_sp = msg.data
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.x_kp = self.pid_x.get_kp()
config.x_ki = self.pid_x.get_ki()
config.x_kd = self.pid_x.get_kd()
config.vx_kp = self.pid_vx.get_kp()
config.vx_ki = self.pid_vx.get_ki()
config.vx_kd = self.pid_vx.get_kd()
config.y_kp = self.pid_y.get_kp()
config.y_ki = self.pid_y.get_ki()
config.y_kd = self.pid_y.get_kd()
config.vy_kp = self.pid_vy.get_kp()
config.vy_ki = self.pid_vy.get_ki()
config.vy_kd = self.pid_vy.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_x.set_kp(config.x_kp)
self.pid_x.set_ki(config.x_ki)
self.pid_x.set_kd(config.x_kd)
self.pid_vx.set_kp(config.vx_kp)
self.pid_vx.set_ki(config.vx_ki)
self.pid_vx.set_kd(config.vx_kd)
self.pid_y.set_kp(config.y_kp)
self.pid_y.set_ki(config.y_ki)
self.pid_y.set_kd(config.y_kd)
self.pid_vy.set_kp(config.vy_kp)
self.pid_vy.set_ki(config.vy_ki)
self.pid_vy.set_kd(config.vy_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
'''
Class implements MAV attitude control (roll, pitch, yaw). Two PIDs in cascade are
used for each degree of freedom.
Subscribes to:
/morus/imu - used to extract attitude and attitude rate of the vehicle
/morus/mot_vel_ref - used to receive referent motor velocity from the height controller
/morus/euler_ref - used to set the attitude referent (useful for testing controllers)
Publishes:
/morus/command/motors - referent motor velocities sent to each motor controller
/morus/pid_roll - publishes PID-roll data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_roll_rate - publishes PID-roll_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch - publishes PID-pitch data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_pitch_rate - publishes PID-pitch_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw - publishes PID-yaw data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_yaw_rate - publishes PID-yaw_rate data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controllers param online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.euler_sp_old = Vector3(0, 0, 0)
self.euler_sp_filt = Vector3(0, 0, 0)
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.euler_rate_mv_old = Vector3()
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_roll_rate = PID() # roll rate (wx) controller
self.pid_pitch = PID() # pitch controller
self.pid_pitch_rate = PID() # pitch rate (wy) controller
self.pid_yaw = PID() # yaw controller
self.pid_yaw_rate = PID() # yaw rate (wz) controller
self.pid_vpc_roll = PID()
self.pid_vpc_pitch = PID()
##################################################################
##################################################################
# Add your PID params here
self.pid_roll.set_kp(2.0)
self.pid_roll.set_ki(0)
self.pid_roll.set_kd(0)
self.pid_roll_rate.set_kp(0.1)
self.pid_roll_rate.set_ki(0.0)
self.pid_roll_rate.set_kd(0)
self.pid_roll_rate.set_lim_high(0.04)
self.pid_roll_rate.set_lim_low(-0.04)
self.pid_pitch.set_kp(2.0)
self.pid_pitch.set_ki(0)
self.pid_pitch.set_kd(0)
self.pid_pitch_rate.set_kp(0.1)
self.pid_pitch_rate.set_ki(0)
self.pid_pitch_rate.set_kd(0)
self.pid_pitch_rate.set_lim_high(0.04)
self.pid_pitch_rate.set_lim_low(-0.04)
self.pid_yaw.set_kp(10)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(6)
self.pid_yaw_rate.set_kp(30.0)
self.pid_yaw_rate.set_ki(0)
self.pid_yaw_rate.set_kd(0)
# VPC pids, initially 0 so vpc is inactive
self.pid_vpc_roll.set_kp(0)
self.pid_vpc_roll.set_ki(800)
self.pid_vpc_roll.set_kd(0)
self.pid_vpc_roll.set_lim_high(200)
self.pid_vpc_roll.set_lim_low(-200)
self.pid_vpc_pitch.set_kp(0)
self.pid_vpc_pitch.set_ki(800)
self.pid_vpc_pitch.set_kd(0)
self.pid_vpc_pitch.set_lim_high(200)
self.pid_vpc_pitch.set_lim_low(-200)
##################################################################
##################################################################
# Filter parameters
self.rate_mv_filt_K = 1.0
self.rate_mv_filt_T = 0.0
self.q_left = [2.463 - 1.57, -2.4846 + 1.57, -1.117]
self.q_right = [-0.6769896 + 1.57, -2.4846 + 1.57, -1.117]
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz
self.Ts = 1.0 / float(self.rate)
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('mot_vel_ref', Float64, self.mot_vel_ref_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
#self.pub_mass0 = rospy.Publisher('movable_mass_0_position_controller/command', Float64, queue_size=1)
#self.pub_mass1 = rospy.Publisher('movable_mass_1_position_controller/command', Float64, queue_size=1)
#self.pub_mass2 = rospy.Publisher('movable_mass_2_position_controller/command', Float64, queue_size=1)
#self.pub_mass3 = rospy.Publisher('movable_mass_3_position_controller/command', Float64, queue_size=1)
self.attitude_pub = rospy.Publisher('attitude_command',
Float64MultiArray,
queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll',
PIDController,
queue_size=1)
self.pub_pid_roll_rate = rospy.Publisher('pid_roll_rate',
PIDController,
queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch',
PIDController,
queue_size=1)
self.pub_pid_pitch_rate = rospy.Publisher('pid_pitch_rate',
PIDController,
queue_size=1)
self.pub_pid_yaw = rospy.Publisher('pid_yaw',
PIDController,
queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate',
PIDController,
queue_size=1)
self.pub_pid_vpc_roll = rospy.Publisher('pid_vpc_roll',
PIDController,
queue_size=1)
self.pub_pid_vpc_pitch = rospy.Publisher('pid_vpc_pitch',
PIDController,
queue_size=1)
self.cfg_server = Server(MmuavAttitudeCtlParamsConfig,
self.cfg_callback)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while (rospy.get_time() == 0) and (not rospy.is_shutdown()):
print 'Waiting for clock server to start'
print 'Received first clock message'
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
#self.ros_rate.sleep()
rospy.sleep(1.0 / float(self.rate))
# Ramp or filter
#self.euler_sp_filt.x = simple_filters.ramp(self.euler_sp_old.x,
# self.euler_sp.x, self.Ts, 1.0)
#self.euler_sp_filt.y = simple_filters.ramp(self.euler_sp_old.y,
# self.euler_sp.y, self.Ts, 1.0)
#self.euler_sp_old = copy.deepcopy(self.euler_sp_filt)
self.euler_sp_filt.x = self.euler_sp.x
self.euler_sp_filt.y = self.euler_sp.y
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > (1.0 / self.rate + 0.005):
self.count += 1
#print self.count, ' - ', dt_clk
if dt_clk < (1.0 / self.rate - 0.005):
self.count += 1
#print self.count, ' - ', dt_clk
# Roll
roll_rate_sv = self.pid_roll.compute(self.euler_sp_filt.x,
self.euler_mv.x, dt_clk)
# roll rate pid compute
roll_rate_output = self.pid_roll_rate.compute(
roll_rate_sv, self.euler_rate_mv.x, dt_clk)
# Pitch
pitch_rate_sv = self.pid_pitch.compute(self.euler_sp_filt.y,
self.euler_mv.y, dt_clk)
# pitch rate pid compute
pitch_rate_output = self.pid_pitch_rate.compute(
pitch_rate_sv, self.euler_rate_mv.y, dt_clk)
# Yaw
yaw_rate_sv = self.pid_yaw.compute(self.euler_sp.z,
self.euler_mv.z, dt_clk)
# yaw rate pid compute
yaw_rate_output = self.pid_yaw_rate.compute(
yaw_rate_sv, self.euler_rate_mv.z, dt_clk)
# VPC outputs
vpc_roll_output = -self.pid_vpc_roll.compute(
0.0, roll_rate_output, dt_clk)
vpc_pitch_output = -self.pid_vpc_pitch.compute(
0.0, pitch_rate_output, dt_clk)
# Roll and pitch rate outputs are position of the center of the
# mass of arms. Here we compute q.
# Publish attitude
L1 = 0.094
L2 = 0.061
L3 = 0.08
arms_pos = [-roll_rate_output, -pitch_rate_output]
#print arms_pos
t0 = time.time()
q = ArmsKinematics.ik_both_arms(self.q_right, self.q_left,
arms_pos, L1, L2, L3)
#print time.time() - t0
self.q_right = copy.deepcopy(q[0])
self.q_left = copy.deepcopy(q[1])
q_left = self.q_left
q_right = self.q_right
attitude_output = Float64MultiArray()
attitude_output.data = [q_left[0]+1.57, q_left[1]-1.57, q_left[2], \
q_right[0]-1.57, q_right[1]-1.57, q_right[2], yaw_rate_output, \
vpc_roll_output, vpc_pitch_output]
self.attitude_pub.publish(attitude_output)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
# Publish VPC pid data
self.pub_pid_vpc_roll.publish(self.pid_vpc_roll.create_msg())
self.pub_pid_vpc_pitch.publish(self.pid_vpc_pitch.create_msg())
def mot_vel_ref_cb(self, msg):
'''
Referent motor velocity callback. (This should be published by height controller).
:param msg: Type Float32
'''
self.w_sp = msg.data
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz),
qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy),
qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
if not self.start_flag:
self.start_flag = True
self.euler_rate_mv_old = copy.deepcopy(self.euler_rate_mv)
# Filtering angular velocities
self.euler_rate_mv.x = simple_filters.filterPT1(
self.euler_rate_mv_old.x, self.euler_rate_mv.x,
self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
self.euler_rate_mv.y = simple_filters.filterPT1(
self.euler_rate_mv_old.y, self.euler_rate_mv.y,
self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
self.euler_rate_mv.z = simple_filters.filterPT1(
self.euler_rate_mv_old.z, self.euler_rate_mv.z,
self.rate_mv_filt_T, self.Ts, self.rate_mv_filt_K)
# Set old to current
self.euler_rate_mv_old = copy.deepcopy(self.euler_rate_mv)
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def clock_cb(self, msg):
self.clock = msg
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.roll_r_kp = self.pid_roll_rate.get_kp()
config.roll_r_ki = self.pid_roll_rate.get_ki()
config.roll_r_kd = self.pid_roll_rate.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
config.pitch_r_kp = self.pid_pitch_rate.get_kp()
config.pitch_r_ki = self.pid_pitch_rate.get_ki()
config.pitch_r_kd = self.pid_pitch_rate.get_kd()
config.yaw_kp = self.pid_yaw.get_kp()
config.yaw_ki = self.pid_yaw.get_ki()
config.yaw_kd = self.pid_yaw.get_kd()
config.yaw_r_kp = self.pid_yaw_rate.get_kp()
config.yaw_r_ki = self.pid_yaw_rate.get_ki()
config.yaw_r_kd = self.pid_yaw_rate.get_kd()
# VPC pids
config.vpc_roll_kp = self.pid_vpc_roll.get_kp()
config.vpc_roll_ki = self.pid_vpc_roll.get_ki()
config.vpc_roll_kd = self.pid_vpc_roll.get_kd()
config.vpc_pitch_kp = self.pid_vpc_pitch.get_kp()
config.vpc_pitch_ki = self.pid_vpc_pitch.get_ki()
config.vpc_pitch_kd = self.pid_vpc_pitch.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_roll_rate.set_kp(config.roll_r_kp)
self.pid_roll_rate.set_ki(config.roll_r_ki)
self.pid_roll_rate.set_kd(config.roll_r_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
self.pid_pitch_rate.set_kp(config.pitch_r_kp)
self.pid_pitch_rate.set_ki(config.pitch_r_ki)
self.pid_pitch_rate.set_kd(config.pitch_r_kd)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_ki(config.yaw_ki)
self.pid_yaw.set_kd(config.yaw_kd)
self.pid_yaw_rate.set_kp(config.yaw_r_kp)
self.pid_yaw_rate.set_ki(config.yaw_r_ki)
self.pid_yaw_rate.set_kd(config.yaw_r_kd)
# VPC pids
self.pid_vpc_roll.set_kp(config.vpc_roll_kp)
self.pid_vpc_roll.set_ki(config.vpc_roll_ki)
self.pid_vpc_roll.set_kd(config.vpc_roll_kd)
self.pid_vpc_pitch.set_kp(config.vpc_pitch_kp)
self.pid_vpc_pitch.set_ki(config.vpc_pitch_ki)
self.pid_vpc_pitch.set_kd(config.vpc_pitch_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
'''
Class implements MAV attitude control (roll, pitch, yaw). Two PIDs in cascade are
used for each degree of freedom.
Subscribes to:
odometry - used to extract attitude and attitude rate of the vehicle
mot_vel_ref - used to receive reference motor velocity from the height controller
euler_ref - used to set the attitude reference (useful for testing controllers)
Publishes:
command/motors - reference motor velocities sent to each motor controller
pid_roll - publishes PID-roll data - reference value, measured value, P, I, D and total component (useful for tuning params)
pid_roll_rate - publishes PID-roll_rate data - reference value, measured value, P, I, D and total component (useful for tuning params)
pid_pitch - publishes PID-pitch data - reference value, measured value, P, I, D and total component (useful for tuning params)
pid_pitch_rate - publishes PID-pitch_rate data - reference value, measured value, P, I, D and total component (useful for tuning params)
pid_yaw - publishes PID-yaw data - reference value, measured value, P, I, D and total component (useful for tuning params)
pid_yaw_rate - publishes PID-yaw_rate data - reference value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controllers param online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles reference values
self.w_sp = 0 # reference value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.pid_roll = PID() # roll controller
self.pid_roll_rate = PID() # roll rate (wx) controller
self.pid_pitch = PID() # pitch controller
self.pid_pitch_rate = PID() # pitch rate (wy) controller
self.pid_yaw = PID() # yaw controller
self.pid_yaw_rate = PID() # yaw rate (wz) controller
##################################################################
##################################################################
# Add your PID params here
self.pid_roll_rate.set_kp(x)
self.pid_roll_rate.set_ki(x)
self.pid_roll_rate.set_kd(x)
self.pid_roll.set_kp(x)
self.pid_roll.set_ki(x)
self.pid_roll.set_kd(x)
self.pid_pitch_rate.set_kp(x)
self.pid_pitch_rate.set_ki(x)
self.pid_pitch_rate.set_kd(x)
self.pid_pitch.set_kp(x)
self.pid_pitch.set_ki(x)
self.pid_pitch.set_kd(c)
self.pid_yaw_rate.set_kp(x)
self.pid_yaw_rate.set_ki(x)
self.pid_yaw_rate.set_kd(x)
self.pid_yaw.set_kp(x)
self.pid_yaw.set_ki(x)
self.pid_yaw.set_kd(x)
##################################################################
##################################################################
self.ros_rate = rospy.Rate(100) # attitude control at 100 Hz
rospy.Subscriber('odometry', Odometry, self.odometry_cb)
rospy.Subscriber('mot_vel_ref', Float32, self.mot_vel_ref_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed', Actuators, queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll', PIDController, queue_size=1)
self.pub_pid_roll_rate = rospy.Publisher('pid_roll_rate', PIDController, queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch', PIDController, queue_size=1)
self.pub_pid_pitch_rate = rospy.Publisher('pid_pitch_rate', PIDController, queue_size=1)
self.pub_pid_yaw = rospy.Publisher('pid_yaw', PIDController, queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate', PIDController, queue_size=1)
self.cfg_server = Server(MavAttitudeCtlParamsConfig, self.cfg_callback)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while not self.start_flag:
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
while not rospy.is_shutdown():
self.ros_rate.sleep()
####################################################################
####################################################################
# Add your code for cascade control for roll, pitch, yaw.
# reference attitude values are stored in self.euler_sp
# (self.euler_sp.x - roll, self.euler_sp.y - pitch, self.euler_sp.z - yaw)
# Measured attitude values are stored in self.euler_mv (x,y,z - roll, pitch, yaw)
# Measured attitude rate values are store in self.euler_rate_mv (self.euler_rate_mv.x, y, z)
# Your result should be reference velocity value for each motor.
# Store them in variables mot_sp1, mot_sp2, mot_sp3, mot_sp4
####################################################################
####################################################################
# Publish motor velocities
mot_speed_msg = Actuators()
mot_speed_msg.angular_velocities = [mot_sp1,mot_sp2,mot_sp3,mot_sp4]
self.pub_mot.publish(mot_speed_msg)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
def mot_vel_ref_cb(self, msg):
'''
reference motor velocity callback. (This should be published by height controller).
:param msg: Type Float32
'''
self.w_sp = msg.data
def odometry_cb(self, msg):
'''
Odometry callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type nav_msgs/Odometry
'''
if not self.start_flag:
self.start_flag = True
qx = msg.pose.pose.orientation.x
qy = msg.pose.pose.orientation.y
qz = msg.pose.pose.orientation.z
qw = msg.pose.pose.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.twist.twist.angular.x
q = msg.twist.twist.angular.y
r = msg.twist.twist.angular.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.roll_r_kp = self.pid_roll_rate.get_kp()
config.roll_r_ki = self.pid_roll_rate.get_ki()
config.roll_r_kd = self.pid_roll_rate.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
config.pitch_r_kp = self.pid_pitch_rate.get_kp()
config.pitch_r_ki = self.pid_pitch_rate.get_ki()
config.pitch_r_kd = self.pid_pitch_rate.get_kd()
config.yaw_kp = self.pid_yaw.get_kp()
config.yaw_ki = self.pid_yaw.get_ki()
config.yaw_kd = self.pid_yaw.get_kd()
config.yaw_r_kp = self.pid_yaw_rate.get_kp()
config.yaw_r_ki = self.pid_yaw_rate.get_ki()
config.yaw_r_kd = self.pid_yaw_rate.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_roll_rate.set_kp(config.roll_r_kp)
self.pid_roll_rate.set_ki(config.roll_r_ki)
self.pid_roll_rate.set_kd(config.roll_r_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
self.pid_pitch_rate.set_kp(config.pitch_r_kp)
self.pid_pitch_rate.set_ki(config.pitch_r_ki)
self.pid_pitch_rate.set_kd(config.pitch_r_kd)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_ki(config.yaw_ki)
self.pid_yaw.set_kd(config.yaw_kd)
self.pid_yaw_rate.set_kp(config.yaw_r_kp)
self.pid_yaw_rate.set_ki(config.yaw_r_ki)
self.pid_yaw_rate.set_kd(config.yaw_r_kd)
# this callback should return config data back to server
return config
class HeightControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
odometry - used to extract z-position and velocitr of the vehicle
pos_ref - used to set the reference for z-position
vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.z_sp = 0 # z-position set point
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.pid_vz = PID() # pid instance for z-velocity control
#########################################################
#########################################################
# Add parameters for vz controller
self.pid_vz.set_kp(x)
self.pid_vz.set_ki(x)
self.pid_vz.set_kd(x)
# Add parameters for z controller
self.pid_z.set_kp(x)
self.pid_z.set_ki(x)
self.pid_z.set_kd(x)
#########################################################
#########################################################
self.pid_vz.set_lim_up(1400) # max velocity of a motor
self.pid_vz.set_lim_low(0) # min velocity of a motor
self.pid_z.set_lim_up(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.quaternion = numpy.empty((4, ), dtype=numpy.float64)
self.velocity_b = numpy.empty((3, ), dtype=numpy.float64)
self.velocity_i = numpy.empty((3, ), dtype=numpy.float64)
self.attitude_ctl = rospy.get_param(
'~attitude_control'
) # flag indicates if attitude control is turned on
rospy.Subscriber('odometry', Odometry, self.odometry_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz',
PIDController,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref',
Float32,
queue_size=1)
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=1)
self.cfg_server = Server(MavZCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(20)
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting height control."
while not rospy.is_shutdown():
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant reference values for motor velocity should be stored in variable self.mot_speed.
########################################################
########################################################
if self.attitude_ctl == 0:
# Publish motor velocities
mot_speed_msg = Actuators()
mot_speed_msg.angular_velocities = [
self.mot_speed, self.mot_speed, self.mot_speed,
self.mot_speed
]
self.pub_mot.publish(mot_speed_msg)
else:
# publish reference motor velocity to attitude controller
mot_speed_msg = Float32(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
# Publish PID data - could be usefule for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
self.z_mv = msg.pose.position.z
def odometry_cb(self, msg):
'''
Odometry (6DOF - position and orientation and velocities) callback.
:param msg: Type Odometry
'''
if not self.start_flag:
self.start_flag = True
self.z_mv = msg.pose.pose.position.z
# transform linear velocity from body frame to inertial frame
self.quaternion[0] = msg.pose.pose.orientation.x
self.quaternion[1] = msg.pose.pose.orientation.y
self.quaternion[2] = msg.pose.pose.orientation.z
self.quaternion[3] = msg.pose.pose.orientation.w
self.velocity_b[0] = msg.twist.twist.linear.x
self.velocity_b[1] = msg.twist.twist.linear.y
self.velocity_b[2] = msg.twist.twist.linear.z
Rib = transformations.quaternion_matrix(self.quaternion)
Rv_b = transformations.translation_matrix(self.velocity_b)
Rv_i = numpy.dot(Rib, Rv_b)
self.velocity_i = transformations.translation_from_matrix(Rv_i)
self.vz_mv = self.velocity_i[2]
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
if not self.start_flag:
self.start_flag = True
self.vz_mv = msg.twist.linear.z
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.z_sp = msg.z
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
# this callback should return config data back to server
return config
class PositionControl:
'''
Class implements ROS node for cascade PID control of MAV position and yaw angle.
Subscribes to:
/morus/pose - used to extract z-position of the vehicle
/morus/velocity - used to extract vz of the vehicle
/morus/pos_ref - used to set the reference for z-position
/morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
/morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
/morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.x_sp = 0 # x-position set point
self.y_sp = 0 # y-position set point
self.z_sp = 0 # z-position set point
self.x_mv = 0 # x-position measured value
self.y_mv = 0 # y-position measured value
self.z_mv = 0 # z-position measured value
self.pid_x = PID() # pid instance for x control
self.pid_y = PID() # pid instance for y control
self.pid_z = PID() # pid instance for z control
self.vx_sp = 0 # vx velocity set_point
self.vy_sp = 0 # vx velocity set_point
self.vz_sp = 0 # vz velocity set_point
self.vx_mv = 0 # vx velocity measured value
self.vy_mv = 0 # vy velocity measured value
self.vz_mv = 0 # vz velocity measured value
self.vx_mv_old = 0 # vz velocity old measured value
self.vy_mv_old = 0 # vz velocity old measured value
self.vz_mv_old = 0 # vz velocity old measured value
self.pid_vx = PID() # pid instance for x-velocity control
self.pid_vy = PID() # pid instance for y-velocity control
self.pid_vz = PID() # pid instance for z-velocity control
self.pid_yaw = PID() # yaw pid
self.pid_yaw_rate = PID() # yaw rate pid
self.euler_mv = Vector3(0, 0, 0) # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.euler_rate_mv = Vector3(0, 0, 0) # measured angular velocities
self.dwz = 0
self.yaw_ref = 0
#########################################################
#########################################################
# Add parameters for z controller
self.pid_z.set_kp(1.5)
self.pid_z.set_ki(0.1)
self.pid_z.set_kd(0.1)
self.pid_z.set_lim_high(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
# Add parameters for vz controller
self.pid_vz.set_kp(40)
self.pid_vz.set_ki(0.1)
self.pid_vz.set_kd(0.0)
self.pid_vz.set_lim_high(350) # max velocity of a motor
self.pid_vz.set_lim_low(-350) # min velocity of a motor
self.pid_vx.set_kp(0.1)
self.pid_vx.set_ki(0.0)
self.pid_vx.set_kd(0)
self.pid_vx.set_lim_high(0.2)
self.pid_vx.set_lim_low(-0.2)
self.pid_vy.set_kp(0.1)
self.pid_vy.set_ki(0.0)
self.pid_vy.set_kd(0)
self.pid_vy.set_lim_high(0.2)
self.pid_vy.set_lim_low(-0.2)
self.pid_x.set_kp(0.5)
self.pid_x.set_ki(0.0)
self.pid_x.set_kd(0)
self.pid_x.set_lim_high(5)
self.pid_x.set_lim_low(-5)
self.pid_y.set_kp(0.5)
self.pid_y.set_ki(0.0)
self.pid_y.set_kd(0)
self.pid_y.set_lim_high(5)
self.pid_y.set_lim_low(-5)
self.pid_yaw.set_kp(1)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0)
self.pid_yaw_rate.set_kp(100)
self.pid_yaw_rate.set_ki(0)
self.pid_yaw_rate.set_kd(0)
#########################################################
#########################################################
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.t_old = 0
self.x_ref = 0
self.y_ref = 0
self.z_ref = 0
self.x_sp = 0
self.y_sp = 0
self.z_sp = 2
self.yaw_sp = 0
self.pos_ctl = Vector3(1, 1, 1)
self.roll_vpc_command = 0
self.pitch_vpc_command = 0
self.yaw_command = 0
self.filter_const_meas = 0.9
self.filter_const_ref = 5.0
# cascade pid control
self.thrust_const = 0.000456874
self.vehicle_mass = 34 #30+30 +4
self.mot_speed_hover = math.sqrt(
self.vehicle_mass * 9.81 / 4 /
self.thrust_const) #432#432#527 # roughly
rospy.Subscriber('position', PointStamped, self.pos_cb)
rospy.Subscriber('velocity', TwistStamped, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
rospy.Subscriber('yaw_ref', Float32, self.yaw_ref_cb)
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('attitude_rotor_command', Vector3,
self.rotor_command_cb)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz',
PIDController,
queue_size=1)
self.pub_pid_x = rospy.Publisher('pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('pid_vx',
PIDController,
queue_size=1)
self.pub_pid_y = rospy.Publisher('pid_y', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('pid_vy',
PIDController,
queue_size=1)
self.pub_pid_yaw = rospy.Publisher('pid_yaw',
PIDController,
queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate',
PIDController,
queue_size=1)
self.euler_ref_pub = rospy.Publisher('euler_ref',
Vector3,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref',
Float32,
queue_size=1)
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=1)
self.cfg_server = Server(MavPosCtlParamsConfig, self.cfg_callback)
self.rate = 100.0
self.Ts = 1.0 / self.rate
self.ros_rate = rospy.Rate(self.rate)
self.t_start = rospy.Time.now()
self.update_filter_coefficients()
rospy.loginfo("Initialized position controller.")
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting height control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
self.t_old = t
# pt1 filter on reference
self.z_ref = self.filter_ref_a * self.z_ref + (
1.0 - self.filter_ref_a) * self.z_sp
vz_ref = self.pid_z.compute(self.z_ref, self.z_mv, self.Ts)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, self.Ts)
self.x_ref = self.filter_ref_a * self.x_ref + (
1.0 - self.filter_ref_a) * self.x_sp
vx_ref = self.pid_x.compute(self.x_ref, self.x_mv, self.Ts)
pitch_r = self.pid_vx.compute(vx_ref, self.vx_mv, self.Ts)
self.y_ref = self.filter_ref_a * self.y_ref + (
1.0 - self.filter_ref_a) * self.y_sp
vy_ref = self.pid_y.compute(self.y_ref, self.y_mv, self.Ts)
roll_r = -self.pid_vy.compute(vy_ref, self.vy_mv, self.Ts)
roll_ref = math.cos(self.euler_mv.z) * roll_r + math.sin(
self.euler_mv.z) * pitch_r
pitch_ref = -math.sin(self.euler_mv.z) * roll_r + math.cos(
self.euler_mv.z) * pitch_r
# yaw control
yaw_rate_sv = self.pid_yaw.compute(self.yaw_sp, self.euler_mv.z,
self.Ts)
# yaw rate pid compute
self.yaw_command = self.pid_yaw_rate.compute(
yaw_rate_sv, self.euler_rate_mv.z, self.Ts)
mot_speed_msg = Actuators()
mot_speed1 = self.mot_speed + self.yaw_command
mot_speed2 = self.mot_speed - self.yaw_command
mot_speed3 = self.mot_speed + self.yaw_command
mot_speed4 = self.mot_speed - self.yaw_command
mot_speed_msg.angular_velocities = [
mot_speed1, mot_speed2, mot_speed3, mot_speed4
]
self.pub_mot.publish(mot_speed_msg)
vec3_msg = Vector3(roll_ref, pitch_ref, 0)
self.euler_ref_pub.publish(vec3_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_y.publish(self.pid_y.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
def pos_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
self.x_mv = msg.point.x
self.y_mv = msg.point.y
self.z_mv = msg.point.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
if not self.start_flag:
self.start_flag = True
self.vx_mv = self.filter_const_meas * self.vx_mv_old + (
1 - self.filter_const_meas) * msg.twist.linear.x
self.vx_mv_old = self.vx_mv
self.vy_mv = self.filter_const_meas * self.vy_mv_old + (
1 - self.filter_const_meas) * msg.twist.linear.y
self.vy_mv_old = self.vy_mv
self.vz_mv = self.filter_const_meas * self.vz_mv_old + (
1 - self.filter_const_meas) * msg.twist.linear.z
self.vz_mv_old = self.vz_mv
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vx_sp = msg.x
self.vy_sp = msg.y
self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.x_sp = msg.x
self.y_sp = msg.y
self.z_sp = msg.z
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
#if not self.start_flag:
# self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz),
qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy),
qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def yaw_ref_cb(self, msg):
'''
Yaw ref callback.
:param msg: Type Float32
'''
self.yaw_sp = msg.data
def rotor_command_cb(self, msg):
'''
Vpc output callback, msg.x roll vpc out, msg.y pitch vpc out, msg.z yaw out
:param msg: Type Vector3
'''
self.yaw_command = msg.z
def update_filter_coefficients(self):
self.filter_ref_a = self.filter_const_ref / (self.filter_const_ref +
self.Ts)
self.filter_ref_b = self.Ts / (self.filter_const_ref + self.Ts)
self.filter_meas_a = self.filter_const_meas / (self.filter_const_meas +
self.Ts)
self.filter_meas_b = self.Ts / (self.filter_const_meas + self.Ts)
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for yaw and yaw rate controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
config.x_kp = self.pid_x.get_kp()
config.x_ki = self.pid_x.get_ki()
config.x_kd = self.pid_x.get_kd()
config.vx_kp = self.pid_vx.get_kp()
config.vx_ki = self.pid_vx.get_ki()
config.vx_kd = self.pid_vx.get_kd()
config.y_kp = self.pid_y.get_kp()
config.y_ki = self.pid_y.get_ki()
config.y_kd = self.pid_y.get_kd()
config.vy_kp = self.pid_vy.get_kp()
config.vy_ki = self.pid_vy.get_ki()
config.vy_kd = self.pid_vy.get_kd()
config.filter_ref = self.filter_const_ref
config.filter_meas = self.filter_const_meas
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
self.pid_x.set_kp(config.x_kp)
self.pid_x.set_ki(config.x_ki)
self.pid_x.set_kd(config.x_kd)
self.pid_vx.set_kp(config.vx_kp)
self.pid_vx.set_ki(config.vx_ki)
self.pid_vx.set_kd(config.vx_kd)
self.pid_y.set_kp(config.y_kp)
self.pid_y.set_ki(config.y_ki)
self.pid_y.set_kd(config.y_kd)
self.pid_vy.set_kp(config.vy_kp)
self.pid_vy.set_ki(config.vy_ki)
self.pid_vy.set_kd(config.vy_kd)
self.filter_const_ref = config.filter_ref
self.filter_const_meas = config.filter_meas
self.update_filter_coefficients()
# this callback should return config data back to server
return config
class HeightControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
/morus/pose - used to extract z-position of the vehicle
/morus/velocity - used to extract vz of the vehicle
/morus/pos_ref - used to set the reference for z-position
/morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
/morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
/morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.x_sp = 0 # x-position set point
self.y_sp = 0 # z-position set point
self.z_sp = 0 # z-position set point
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.vz_mv_old = 0 # vz velocity old measured value
self.pid_vz = PID() # pid instance for z-velocity control
#########################################################
#########################################################
# Add parameters for z controller
#self.pid_z.set_kp(2)
#self.pid_z.set_ki(0.25)
#self.pid_z.set_kd(0.5)
# Add parameters for vz controller
#self.pid_vz.set_kp(87.2)
#self.pid_vz.set_ki(0)
#self.pid_vz.set_kd(10.89)
self.pid_z.set_kp(1.025)
self.pid_z.set_ki(0.018)
self.pid_z.set_kd(0.0)
# Add parameters for vz controller
self.pid_vz.set_kp(209.79)
self.pid_vz.set_ki(0.0)
self.pid_vz.set_kd(0.1)
#########################################################
#########################################################
self.pid_z.set_lim_high(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
self.pid_vz.set_lim_high(1475) # max velocity of a motor
self.pid_vz.set_lim_low(-1475) # min velocity of a motor
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.gm_attitude_ctl = 0 # flag indicates if attitude control is turned on
self.gm_attitude_ctl = rospy.get_param('~gm_attitude_ctl')
self.t_old = 0
rospy.Subscriber('/arducopter/sensors/pose1',
PoseWithCovarianceStamped, self.pose_cb)
rospy.Subscriber('/arducopter/velocity', TwistStamped, self.vel_cb)
rospy.Subscriber('/arducopter/vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('/arducopter/pos_ref', Vector3, self.pos_ref_cb)
self.pub_pid_z = rospy.Publisher('/arducopter/pid_z',
PIDController,
queue_size=1)
self.pub_pid_vz = rospy.Publisher('/arducopter/pid_vz',
PIDController,
queue_size=1)
self.mot_ref_pub = rospy.Publisher('/arducopter/mot_vel_ref',
Float32,
queue_size=1)
self.pub_mot = rospy.Publisher('/arducopter/command/motors',
MotorSpeed,
queue_size=1)
self.cfg_server = Server(MavZCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(10)
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting height control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant referent value for motor velocity should be stored in variable mot_speed.
# When you implement attitude control set the flag self.attitude_ctl to 1
#self.gm_attitude_ctl = 1 # don't forget to set me to 1 when you implement attitude ctl
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
#if dt > 0.105 or dt < 0.095:
# print dt
self.t_old = t
self.mot_speed_hover = 923.7384 #432#527 # roughly
vz_ref = self.pid_z.compute(self.z_sp, self.z_mv, dt)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
########################################################
########################################################
if self.gm_attitude_ctl == 0:
# Publish motor velocities
mot_speed_msg = MotorSpeed()
dx_speed = self.x_sp * self.mot_speed
dy_speed = self.y_sp * self.mot_speed
mot_speed1 = self.mot_speed - dx_speed
mot_speed3 = self.mot_speed + dx_speed
mot_speed2 = self.mot_speed - dy_speed
mot_speed4 = self.mot_speed + dy_speed
mot_speed_msg.motor_speed = [
mot_speed1, mot_speed2, mot_speed3, mot_speed4
]
self.pub_mot.publish(mot_speed_msg)
else:
# publish referent motor velocity to attitude controller
#mot_speed_msg = Float32(self.mot_speed)
#self.mot_ref_pub.publish(mot_speed_msg)
mot_speed_msg = MotorSpeed()
if self.z_sp <= -1.0:
self.mot_speed = 0
mot_speed_msg.motor_speed = [
self.mot_speed, self.mot_speed, self.mot_speed,
self.mot_speed
]
self.pub_mot.publish(mot_speed_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
self.z_mv = msg.pose.pose.position.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
if not self.start_flag:
self.start_flag = True
a = 0.95
self.vz_mv = (1 - a) * msg.twist.linear.z + a * self.vz_mv_old
self.vz_mv_old = self.vz_mv
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.x_sp = msg.x
self.y_sp = msg.y
self.z_sp = msg.z
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_pitch = PID() # pitch controller
##################################################################
##################################################################
# Add your PID params here
self.pid_roll.set_kp(0.02) # 0.001
self.pid_roll.set_ki(0.08)
self.pid_roll.set_kd(0.0)
self.pid_roll.set_dead_zone(0.0)
self.pid_pitch.set_kp(0.02)
self.pid_pitch.set_ki(0.08)
self.pid_pitch.set_kd(0.0)
self.pid_pitch.set_dead_zone(0.0)
self.joint0 = [0, -45, -45,
0, -45, -45,
0, -45, -45,
0, -45, -45]
self.joint_ref = copy.deepcopy(self.joint0)
self.joint_msg = JointState();
self.joint_pos = [0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
self.ml = 1.494
self.mb = 2.564
self.M = self.mb + 4 * self.ml
self.Sx = 0.02
self.Sz = 0.02
self.D = 0.424
self.nu = 1/2.564
self.Jacobian = numpy.matrix([[0.0,0.0,0.0,0.0], [0.0,0.0,0.0,0.0]])
self.inv_Jacobian = numpy.matrix([[0.0,0.0],[0.0,0.0], [0.0,0.0],[0.0,0.0]])
self.u_meas = numpy.matrix([[0.0],[0.0],[0.0],[0.0]])
self.dp_ref = numpy.matrix([[0.0], [0.0]])
self.du_ref = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
print self.u_meas
##################################################################
##################################################################
self.rate = 20.0
self.ros_rate = rospy.Rate(self.rate) # attitude control at 20 Hz
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
rospy.Subscriber('aquashoko_joint_positions', JointState, self.joint_cb)
self.pub_joint_references = rospy.Publisher('aquashoko_chatter', JointState, queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll', PIDController, queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch', PIDController, queue_size=1)
self.cfg_server = Server(AttitudeControlParamsConfig, self.cfg_callback)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while rospy.get_time() == 0:
print 'Waiting for clock server to start'
print 'Received first clock message'
while not self.start_flag:
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
self.ros_rate.sleep()
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk < 0.0001:
#this should never happen, but if it does, set default value of sample time
dt_clk = 1.0 / self.rate
self.u_meas[0,0] = math.radians((self.joint_pos[0] - self.joint_pos[6]) / 2.0)
self.u_meas[1,0] = math.radians((self.joint_pos[3] - self.joint_pos[9]) / 2.0)
self.u_meas[2,0] = math.radians((self.joint_pos[1] - self.joint_pos[7]) / 2.0)
self.u_meas[3,0] = math.radians((self.joint_pos[4] - self.joint_pos[10]) / 2.0)
self.compute_jacobian()
self.inv_Jacobian = numpy.linalg.pinv(self.Jacobian)
#self.inv_Jacobian = numpy.matrix([[1.0*12.0,1.0*25.0],[1.0*-25.0, 1.0*12.0], [-25.0,0.0],[0.0,-25.0]])
delta_y_ref = self.pid_roll.compute(self.euler_sp.x, self.euler_mv.x, dt_clk)
delta_x_ref = -self.pid_pitch.compute(self.euler_sp.y, self.euler_mv.y, dt_clk)
self.dp_ref[0,0] = delta_x_ref
self.dp_ref[1,0] = delta_y_ref
self.du_ref = self.inv_Jacobian * self.dp_ref
self.joint_ref[1] = math.degrees(self.du_ref[2,0]) + self.joint0[1]
self.joint_ref[7] = math.degrees(-self.du_ref[2,0]) + self.joint0[7]
self.joint_ref[4] = math.degrees(self.du_ref[3,0]) + self.joint0[4]
self.joint_ref[10] = math.degrees(-self.du_ref[3,0]) + self.joint0[10]
self.joint_ref[0] = math.degrees(self.du_ref[0,0]) + self.joint0[0]
self.joint_ref[6] = math.degrees(-self.du_ref[0,0]) + self.joint0[6]
self.joint_ref[3] = math.degrees(self.du_ref[1,0]) + self.joint0[3]
self.joint_ref[9] = math.degrees(-self.du_ref[1,0]) + self.joint0[9]
#self.joint_ref[2] = self.joint_ref[1]
#self.joint_ref[5] = self.joint_ref[4]
#self.joint_ref[8] = self.joint_ref[7]
#self.joint_ref[11] = self.joint_ref[10]
self.joint_msg.header.stamp = rospy.Time.now()
self.joint_msg.position = copy.deepcopy(self.joint_ref)
self.pub_joint_references.publish(self.joint_msg)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.loop_count = self.loop_count + 1
def compute_jacobian(self):
c1 = math.cos(self.u_meas[0,0])
s1 = math.sin(self.u_meas[0,0])
c2 = math.cos(self.u_meas[1,0])
s2 = math.sin(self.u_meas[1,0])
c3 = math.cos(self.u_meas[2,0])
s3 = math.sin(self.u_meas[2,0])
c4 = math.cos(self.u_meas[3,0])
s4 = math.sin(self.u_meas[3,0])
self.Jacobian[0,0] = self.compute_j00_j11(s1, s3, c1)
self.Jacobian[1,1] = self.compute_j00_j11(s2, s4, c2)
self.Jacobian[0,1] = -1.0 * self.compute_j01_j10_j02_j13(c2, c4)
self.Jacobian[1,0] = self.compute_j01_j10_j02_j13(c1, c3)
self.Jacobian[0,2] = -1.0 * self.compute_j01_j10_j02_j13(c1, c3)
self.Jacobian[1,3] = -1.0 * self.compute_j01_j10_j02_j13(c2, c4)
self.Jacobian[0,3] = self.compute_j03_j12(s2, s4)
self.Jacobian[1,2] = -1.0 * self.compute_j03_j12(s1, s3)
def compute_j00_j11(self, s1,s2, c1):
res = self.ml * (math.sqrt(2) * s1 * s2 * (8 * self.ml * self.Sx + self.mb * (self.D*(1-self.nu) + 2 * self.Sx)) / self.M + 4 * c1 * self.Sz) / \
(2 * (self.nu * self.mb + 4 * self.ml))
return res
def compute_j01_j10_j02_j13(self, c1,c2):
res = (c1 * c2 * self.ml * (8 * self.ml * self.Sx + self.mb * (self.D*(1-self.nu) + 2 * self.Sx))) / \
(math.sqrt(2) * self.M * (self.nu * self.mb + 4 * self.ml))
return res
def compute_j03_j12(self, s1,s2):
res = (s1 * s2 * self.ml * (8 * self.ml * self.Sx + self.mb * (self.D*(1-self.nu) + 2 * self.Sx))) / \
(math.sqrt(2) * self.M * (self.nu * self.mb + 4 * self.ml))
return res
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
if not self.start_flag:
self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def clock_cb(self, msg):
self.clock = msg
def joint_cb(self, msg):
self.joint_pos = copy.deepcopy(msg.position)
#print self.joint_pos
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
# this callback should return config data back to server
return config
class HeightControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
/morus/pose - used to extract z-position of the vehicle
/morus/velocity - used to extract vz of the vehicle
/morus/pos_ref - used to set the reference for z-position
/morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
/morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
/morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
/morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.x_sp = 0 # x-position set point
self.y_sp = 0 # y-position set point
self.z_sp = 0 # z-position set point
self.x_mv = 0 # x-position measured value
self.y_mv = 0 # y-position measured value
self.z_mv = 0 # z-position measured value
self.pid_x = PID() # pid instance for x control
self.pid_y = PID() # pid instance for y control
self.pid_z = PID() # pid instance for z control
self.vx_sp = 0 # vx velocity set_point
self.vy_sp = 0 # vx velocity set_point
self.vz_sp = 0 # vz velocity set_point
self.vx_mv = 0 # vx velocity measured value
self.vy_mv = 0 # vy velocity measured value
self.vz_mv = 0 # vz velocity measured value
self.vx_mv_old = 0 # vz velocity old measured value
self.vy_mv_old = 0 # vz velocity old measured value
self.vz_mv_old = 0 # vz velocity old measured value
self.pid_vx = PID() # pid instance for x-velocity control
self.pid_vy = PID() # pid instance for y-velocity control
self.pid_vz = PID() # pid instance for z-velocity control
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.euler_rate_mv = Vector3() # measured angular velocities
self.yaw_sp = 0
self.yaw_mv = 0
self.pid_yaw = PID()
self.pid_yaw_rate = PID()
self.dwz = 0
self.w0 = 923.7384
self.dx_speed = 0
self.dy_speed = 0
#########################################################
#########################################################
# Add parameters for z controller
self.pid_z.set_kp(1.025)
self.pid_z.set_ki(0.018)
self.pid_z.set_kd(0.0)
# Add parameters for vz controller
self.pid_vz.set_kp(209.79)
self.pid_vz.set_ki(0.0)
self.pid_vz.set_kd(0.1)
self.pid_vx.set_kp(24)
self.pid_vx.set_ki(20)
self.pid_vx.set_kd(12)
self.pid_vx.set_lim_high(0.05 * self.w0)
self.pid_vx.set_lim_low(-0.05 * self.w0)
self.pid_vy.set_kp(24)
self.pid_vy.set_ki(20)
self.pid_vy.set_kd(12)
self.pid_vy.set_lim_high(0.05 * self.w0)
self.pid_vy.set_lim_low(-0.05 * self.w0)
self.pid_x.set_kp(0.0)
self.pid_x.set_ki(0.0)
self.pid_x.set_kd(0.0)
self.pid_x.set_lim_high(0.75)
self.pid_x.set_lim_low(-0.75)
self.pid_yaw.set_kp(1.86)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0)
self.pid_yaw_rate.set_kp(6.5)
self.pid_yaw_rate.set_ki(0)
self.pid_yaw_rate.set_kd(0)
#########################################################
#########################################################
self.pid_z.set_lim_high(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
self.pid_vz.set_lim_high(1475) # max velocity of a motor
self.pid_vz.set_lim_low(-1475) # min velocity of a motor
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.gm_attitude_ctl = 0 # flag indicates if attitude control is turned on
self.gm_attitude_ctl = rospy.get_param('~gm_attitude_ctl')
self.t_old = 0
rospy.Subscriber('/arducopter/sensors/pose1', PoseWithCovarianceStamped, self.pose_cb)
rospy.Subscriber('/arducopter/velocity', TwistStamped, self.vel_cb)
rospy.Subscriber('/arducopter/vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('/arducopter/pos_ref', Vector3, self.pos_ref_cb)
rospy.Subscriber('/arducopter/euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/arducopter/imu', Imu, self.ahrs_cb)
self.pub_pid_z = rospy.Publisher('/arducopter/pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('/arducopter/pid_vz', PIDController, queue_size=1)
self.pub_pid_x = rospy.Publisher('/arducopter/pid_x', PIDController, queue_size=1)
self.pub_pid_vx = rospy.Publisher('/arducopter/pid_vx', PIDController, queue_size=1)
self.pub_pid_vy = rospy.Publisher('/arducopter/pid_vy', PIDController, queue_size=1)
self.pub_pid_yaw = rospy.Publisher('/arducopter/pid_yaw', PIDController, queue_size=1)
self.pub_pid_wz = rospy.Publisher('/arducopter/pid_yaw_rate', PIDController, queue_size=1)
self.mot_ref_pub = rospy.Publisher('/arducopter/mot_vel_ref', Float32, queue_size=1)
self.pub_mot = rospy.Publisher('/arducopter/command/motors', MotorSpeed, queue_size=1)
self.cfg_server = Server(MavCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(20)
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
while not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
print "Starting height control."
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
self.ros_rate.sleep() # 5 ms sleep
########################################################
########################################################
# Implement cascade PID control here.
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
#if dt > 0.105 or dt < 0.095:
# print dt
self.t_old = t
self.mot_speed_hover = 923.7384#300.755#432#527 # roughly
#height control
vz_ref = self.pid_z.compute(self.z_sp, self.z_mv, dt)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
# vx control
#vx_ref = self.pid_x.compute(self.x_sp, self.x_mv, dt)
self.dx_speed = self.pid_vx.compute(self.vx_sp, self.vx_mv, dt)
self.dy_speed = self.pid_vy.compute(self.vy_sp, self.vy_mv, dt)
yaw_r_ref = self.pid_yaw.compute(self.euler_sp.z, self.euler_mv.z, dt)
self.dwz = self.pid_yaw_rate.compute(yaw_r_ref, self.euler_rate_mv.z, dt)
########################################################
########################################################
if self.gm_attitude_ctl == 0:
# Publish motor velocities
mot_speed_msg = MotorSpeed()
#dx_speed = self.x_sp * self.mot_speed
#dy_speed = self.y_sp * self.mot_speed
mot_speed1 = self.mot_speed - self.dx_speed + self.dwz
mot_speed3 = self.mot_speed + self.dx_speed + self.dwz
mot_speed2 = self.mot_speed - self.dy_speed - self.dwz
mot_speed4 = self.mot_speed + self.dy_speed - self.dwz
if(self.z_mv < 0.14 and self.z_sp == 0):
mot_speed_msg.motor_speed = [0,0,0,0]
self.pub_mot.publish(mot_speed_msg)
else:
mot_speed_msg.motor_speed = [mot_speed1, mot_speed2, mot_speed3, mot_speed4]
self.pub_mot.publish(mot_speed_msg)
else:
# publish referent motor velocity to attitude controller
mot_speed_msg = Float32(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
self.ros_rate.sleep() # 5 ms sleep
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
#self.pub_pid_x.publish(self.pid_x.create_msg())
self.pub_pid_vx.publish(self.pid_vx.create_msg())
self.pub_pid_vy.publish(self.pid_vy.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_wz.publish(self.pid_yaw_rate.create_msg())
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
self.x_mv = msg.pose.pose.position.x
self.y_mv = msg.pose.pose.position.y
self.z_mv = msg.pose.pose.position.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
if not self.start_flag:
self.start_flag = True
a = 0.99
self.vx_mv = (1-a)*msg.twist.linear.x + a * self.vx_mv_old
self.vx_mv_old = self.vx_mv
self.vy_mv = (1-a)*msg.twist.linear.y + a * self.vy_mv_old
self.vy_mv_old = self.vy_mv
self.vz_mv = (1-a)*msg.twist.linear.z + a * self.vz_mv_old
self.vz_mv_old = self.vz_mv
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vx_sp = msg.x
self.vy_sp = msg.y
#self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.x_sp = msg.x
self.y_sp = msg.y
self.z_sp = msg.z
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
if not self.start_flag:
self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
config.vx_kp = self.pid_vx.get_kp()
config.vx_ki = self.pid_vx.get_ki()
config.vx_kd = self.pid_vx.get_kd()
config.vy_kp = self.pid_vy.get_kp()
config.vy_ki = self.pid_vy.get_ki()
config.vy_kd = self.pid_vy.get_kd()
config.yaw_kp = self.pid_yaw.get_kp()
config.yaw_ki = self.pid_yaw.get_ki()
config.yaw_kd = self.pid_yaw.get_kd()
config.yaw_r_kp = self.pid_yaw_rate.get_kp()
config.yaw_r_ki = self.pid_yaw_rate.get_ki()
config.yaw_r_kd = self.pid_yaw_rate.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
self.pid_vx.set_kp(config.vx_kp)
self.pid_vx.set_ki(config.vx_ki)
self.pid_vx.set_kd(config.vx_kd)
self.pid_vy.set_kp(config.vy_kp)
self.pid_vy.set_ki(config.vy_ki)
self.pid_vy.set_kd(config.vy_kd)
self.pid_yaw.set_kp(config.yaw_kp)
self.pid_yaw.set_ki(config.yaw_ki)
self.pid_yaw.set_kd(config.yaw_kd)
self.pid_yaw_rate.set_kp(config.yaw_r_kp)
self.pid_yaw_rate.set_ki(config.yaw_r_ki)
self.pid_yaw_rate.set_kd(config.yaw_r_kd)
# this callback should return config data back to server
return config
class HeightControl:
'''
Class implements ROS node for cascade (z, vz) PID control for MAV height.
Subscribes to:
pose - used to extract z-position of the vehicle
velocity - used to extract vz of the vehicle
pos_ref - used to set the reference for z-position
vel_ref - used to set the reference for vz-position (useful for testing velocity controller)
Publishes:
mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s)
pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params)
pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params)
Dynamic reconfigure is used to set controller params online.
'''
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # indicates if we received the first measurement
self.config_start = False # flag indicates if the config callback is called for the first time
self.z_sp = 0 # z-position set point
self.z_ref_filt = 0 # z ref filtered
self.z_mv = 0 # z-position measured value
self.pid_z = PID() # pid instance for z control
self.vz_sp = 0 # vz velocity set_point
self.vz_mv = 0 # vz velocity measured value
self.pid_vz = PID() # pid instance for z-velocity control
self.pid_yaw_rate = PID()
self.euler_mv = Vector3(0, 0, 0) # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.euler_rate_mv = Vector3(0, 0, 0) # measured angular velocities
self.dwz = 0
#########################################################
#########################################################
# Add parameters for z controller
self.pid_z.set_kp(4.0) # 0.5
self.pid_z.set_ki(0.02) # 0.01
self.pid_z.set_kd(0.75)
# Add parameters for vz controller
self.pid_vz.set_kp(25.0)# 20, 87.2)
self.pid_vz.set_ki(0.01) # 0.1
self.pid_vz.set_kd(20.0)# 10, 10.89)
# Yaw rate params
self.pid_yaw_rate.set_kp(75)
self.pid_yaw_rate.set_ki(5)
self.pid_yaw_rate.set_kd(15)
#########################################################
#########################################################
self.pid_z.set_lim_high(5) # max vertical ascent speed
self.pid_z.set_lim_low(-5) # max vertical descent speed
self.pid_vz.set_lim_high(350) # max velocity of a motor
self.pid_vz.set_lim_low(-350) # min velocity of a motor
self.mot_speed = 0 # referent motors velocity, computed by PID cascade
self.gm_attitude_ctl = 0 # flag indicates if attitude control is turned on
self.gm_attitude_ctl = rospy.get_param('~gm_attitude_ctl')
self.t_old = 0
rospy.Subscriber('pose_with_covariance', PoseWithCovarianceStamped, self.pose_cb)
rospy.Subscriber('velocity', TwistStamped, self.vel_cb)
rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb)
rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('imu', Imu, self.ahrs_cb)
self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1)
self.pub_pid_vz = rospy.Publisher('pid_vz', PIDController, queue_size=1)
self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate', PIDController, queue_size=1)
self.mot_ref_pub = rospy.Publisher('mot_vel_ref', Float32, queue_size=1)
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed', Actuators, queue_size=1)
self.cfg_server = Server(MavZCtlParamsConfig, self.cfg_callback)
self.ros_rate = rospy.Rate(10)
self.t_start = rospy.Time.now()
def run(self):
'''
Runs ROS node - computes PID algorithms for z and vz control.
'''
self.t_old = rospy.Time.now()
#self.t_old = datetime.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
########################################################
########################################################
# Implement cascade PID control here.
# Reference for z is stored in self.z_sp.
# Measured z-position is stored in self.z_mv.
# If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp.
# Measured vz-velocity is stored in self.vz_mv
# Resultant referent value for motor velocity should be stored in variable mot_speed.
# When you implement attitude control set the flag self.attitude_ctl to 1
#self.gm_attitude_ctl = 1 # don't forget to set me to 1 when you implement attitude ctl
if not self.start_flag:
print 'Waiting for velocity measurements.'
rospy.sleep(0.5)
else:
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
#t = datetime.now()
#dt = (t - self.t_old).total_seconds()
if dt > 0.105 or dt < 0.095:
print dt
self.t_old = t
#Corrections for HIL
self.mot_speed_hover = 427
#self.mot_speed_hover = 280
# prefilter for reference
a = 0.1
self.z_ref_filt = (1-a) * self.z_ref_filt + a * self.z_sp
vz_ref = self.pid_z.compute(self.z_ref_filt, self.z_mv, dt)
self.mot_speed = self.mot_speed_hover + \
self.pid_vz.compute(vz_ref, self.vz_mv, dt)
self.dwz = self.pid_yaw_rate.compute(self.euler_sp.z, self.euler_rate_mv.z, dt)
########################################################
########################################################
if self.gm_attitude_ctl == 0:
# moving masses are used to control attitude
# Publish motor velocities
mot_speed_msg = Actuators(); mot_speed_msg.header.stamp = t
mot_speed1 = self.mot_speed + self.dwz
mot_speed2 = self.mot_speed - self.dwz
mot_speed3 = self.mot_speed + self.dwz
mot_speed4 = self.mot_speed - self.dwz
mot_speed_msg.angular_velocities = [mot_speed1, mot_speed2, mot_speed3, mot_speed4]
self.pub_mot.publish(mot_speed_msg)
else:
# gas motors are used to control attitude
# publish referent motor velocity to attitude controller
mot_speed_msg = Float32(self.mot_speed)
self.mot_ref_pub.publish(mot_speed_msg)
# Publish PID data - could be useful for tuning
self.pub_pid_z.publish(self.pid_z.create_msg())
self.pub_pid_vz.publish(self.pid_vz.create_msg())
def pose_cb(self, msg):
'''
Pose (6DOF - position and orientation) callback.
:param msg: Type PoseWithCovarianceStamped
'''
self.z_mv = msg.pose.pose.position.z
def vel_cb(self, msg):
'''
Velocity callback (linear velocity - x,y,z)
:param msg: Type Vector3Stamped
'''
if not self.start_flag:
self.start_flag = True
self.vz_mv = msg.twist.linear.z
def vel_ref_cb(self, msg):
'''
Referent velocity callback. Use received velocity values when during initial tuning
velocity controller (i.e. when position controller still not implemented).
:param msg: Type Vector3
'''
self.vz_sp = msg.z
def pos_ref_cb(self, msg):
'''
Referent position callback. Received value (z-component) is used as a referent height.
:param msg: Type Vector3
'''
self.z_sp = msg.z
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
if not self.start_flag:
self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz), qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def cfg_callback(self, config, level):
"""
Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.z_kp = self.pid_z.get_kp()
config.z_ki = self.pid_z.get_ki()
config.z_kd = self.pid_z.get_kd()
config.vz_kp = self.pid_vz.get_kp()
config.vz_ki = self.pid_vz.get_ki()
config.vz_kd = self.pid_vz.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_z.set_kp(config.z_kp)
self.pid_z.set_ki(config.z_ki)
self.pid_z.set_kd(config.z_kd)
self.pid_vz.set_kp(config.vz_kp)
self.pid_vz.set_ki(config.vz_ki)
self.pid_vz.set_kd(config.vz_kd)
# this callback should return config data back to server
return config
class AttitudeControl:
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_pitch = PID() # pitch controller
##################################################################
##################################################################
# Add your PID params here
self.pid_roll.set_kp(5.0)
self.pid_roll.set_ki(6.0)
self.pid_roll.set_kd(0.0)
self.pid_pitch.set_kp(5.0)
self.pid_pitch.set_ki(6.0)
self.pid_pitch.set_kd(0.0)
self.joint0 = [0, -45, -45, 0, -45, -45, 0, -45, -45, 0, -45, -45]
self.joint_ref = copy.deepcopy(self.joint0)
self.joint_msg = JointState()
##################################################################
##################################################################
self.rate = 20.0
self.ros_rate = rospy.Rate(self.rate) # attitude control at 20 Hz
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
self.pub_joint_references = rospy.Publisher('aquashoko_chatter',
JointState,
queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll',
PIDController,
queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch',
PIDController,
queue_size=1)
self.cfg_server = Server(AttitudeControlParamsConfig,
self.cfg_callback)
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while rospy.get_time() == 0:
print 'Waiting for clock server to start'
print 'Received first clock message'
while not self.start_flag:
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
self.ros_rate.sleep()
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk < 0.0001:
#this should never happen, but if it does, set default value of sample time
dt_clk = 1.0 / self.rate
roll_cmd = self.pid_roll.compute(self.euler_sp.x, self.euler_mv.x,
dt_clk)
pitch_cmd = self.pid_pitch.compute(self.euler_sp.y,
self.euler_mv.y, dt_clk)
# Publish joint references
self.joint_ref[1] = math.degrees(pitch_cmd) + self.joint0[1]
self.joint_ref[7] = -math.degrees(pitch_cmd) + self.joint0[7]
self.joint_ref[4] = -math.degrees(roll_cmd) + self.joint0[4]
self.joint_ref[10] = math.degrees(roll_cmd) + self.joint0[10]
self.joint_msg.header.stamp = rospy.Time.now()
self.joint_msg.position = copy.deepcopy(self.joint_ref)
self.pub_joint_references.publish(self.joint_msg)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
def ahrs_cb(self, msg):
'''
AHRS callback. Used to extract roll, pitch, yaw and their rates.
We used the following order of rotation - 1)yaw, 2) pitch, 3) roll
:param msg: Type sensor_msgs/Imu
'''
if not self.start_flag:
self.start_flag = True
qx = msg.orientation.x
qy = msg.orientation.y
qz = msg.orientation.z
qw = msg.orientation.w
# conversion quaternion to euler (yaw - pitch - roll)
self.euler_mv.x = math.atan2(2 * (qw * qx + qy * qz),
qw * qw - qx * qx - qy * qy + qz * qz)
self.euler_mv.y = math.asin(2 * (qw * qy - qx * qz))
self.euler_mv.z = math.atan2(2 * (qw * qz + qx * qy),
qw * qw + qx * qx - qy * qy - qz * qz)
# gyro measurements (p,q,r)
p = msg.angular_velocity.x
q = msg.angular_velocity.y
r = msg.angular_velocity.z
sx = math.sin(self.euler_mv.x) # sin(roll)
cx = math.cos(self.euler_mv.x) # cos(roll)
cy = math.cos(self.euler_mv.y) # cos(pitch)
ty = math.tan(self.euler_mv.y) # cos(pitch)
# conversion gyro measurements to roll_rate, pitch_rate, yaw_rate
self.euler_rate_mv.x = p + sx * ty * q + cx * ty * r
self.euler_rate_mv.y = cx * q - sx * r
self.euler_rate_mv.z = sx / cy * q + cx / cy * r
def euler_ref_cb(self, msg):
'''
Euler ref values callback.
:param msg: Type Vector3 (x-roll, y-pitch, z-yaw)
'''
self.euler_sp = msg
def clock_cb(self, msg):
self.clock = msg
def cfg_callback(self, config, level):
""" Callback for dynamically reconfigurable parameters (P,I,D gains for each controller)
"""
if not self.config_start:
# callback is called for the first time. Use this to set the new params to the config server
config.roll_kp = self.pid_roll.get_kp()
config.roll_ki = self.pid_roll.get_ki()
config.roll_kd = self.pid_roll.get_kd()
config.pitch_kp = self.pid_pitch.get_kp()
config.pitch_ki = self.pid_pitch.get_ki()
config.pitch_kd = self.pid_pitch.get_kd()
self.config_start = True
else:
# The following code just sets up the P,I,D gains for all controllers
self.pid_roll.set_kp(config.roll_kp)
self.pid_roll.set_ki(config.roll_ki)
self.pid_roll.set_kd(config.roll_kd)
self.pid_pitch.set_kp(config.pitch_kp)
self.pid_pitch.set_ki(config.pitch_ki)
self.pid_pitch.set_kd(config.pitch_kd)
# this callback should return config data back to server
return config
