class Controller(object):
def __init__(self, p, t='pid'):
self.t = t
if t is 'pid':
t_w, t_k_p, t_k_i, t_k_d, x_w, x_k_p, x_k_i, x_k_d = p
self.t_w = t_w
self.x_w = x_w
self.t_pid = PID(t_k_p, t_k_i, t_k_d)
self.x_pid = PID(x_k_p, x_k_i, x_k_d)
elif t is 'net':
w = []
end = 0
for a, b in zip(network_topology[:-1], network_topology[1:]):
w.append(np.reshape(p[end:end + a * b], (a, b)))
end = end + a * b
self.c_net = Net(w)
def compute(self, s):
t = self.t
if t is 'pid':
t_w, x_w = self.t_w, self.x_w
x, _, t, _ = s
return t_w * self.t_pid.compute(t,tau) + \
x_w * self.x_pid.compute(x,tau)
elif t is 'net':
return self.c_net.compute(s)
class AttitudeControl:
"""
Class implements attitude control (roll, pitch, yaw).
"""
def __init__(self):
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles reference values
self.x_mv = 0 # x-position measured value
self.x_sp = 0 # x-position set point
self.y_mv = 0 # y-position measured value
self.y_sp = 0 # y-position set point
self.pid_roll = PID(1, 0, 1.45, 1, -1) # roll controller
self.pid_pitch = PID(1, 0, 1.45, 1, -1) # pitch controller
self.pid_yaw = PID(2, 0, 0, 5, -5) # yaw controller
self.vel_msg = Twist()
def runPose(self, data):
self.vel_msg.linear.x = self.pid_roll.compute(self.x_sp,
data.position.x)
self.vel_msg.linear.y = self.pid_pitch.compute(self.x_sp,
data.position.y)
self.vel_msg.angular.z = self.pid_yaw.compute(self.euler_sp.z,
data.orientation.z)
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(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 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 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 OdomDancer(PathDancerBase):
def __init__(self):
super(OdomDancer,self).__init__()
self.cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.cmd_msg = Twist()
self.initialized = False
self.initial_orientation = None
self.position = (0,0)
self.orientation = 0.0
self.odom_sub = rospy.Subscriber('odometry/filtered/local', Odometry, self.odom_cb)
self.vpid = PID(1.0,0.01,0.0)
self.wpid = PID(1.0,0.01,0.0)
def odom_cb(self, msg):
p = msg.pose.pose.position
q = msg.pose.pose.orientation
x = p.x
y = p.y
t = np.arctan2(q.z,q.w) * 2
if not self.initialized:
self.x0 = x
self.y0 = y
self.t0 = t
self.initialized = True
self.position = (x - self.x0, y - self.y0)
self.orientation = t - self.t0
def run(self, points):
### SETUP ALIAS NAMES ###
l = self.cmd_msg.linear
a = self.cmd_msg.angular
### SETUP CONFIGURATION ###
r = rospy.Rate(20) # 20 hz
dt = 1 / 20. #
position_tolerance = 0.1
orientation_tolerance = np.deg2rad(5)
while not self.initialized:
r.sleep()
### RUN CONTROL LOOP ###
for pt in points:
print pt
### ADJUST HEADING ###
self.wpid.reset()
while True:
dx,dy = (pt[0] - self.position[0], pt[1] - self.position[1])
theta = np.arctan2(dy,dx) - self.orientation # angular difference
theta = np.arctan2(np.sin(theta), np.cos(theta)) # lazy normalize
delta = np.sqrt(dx**2+dy**2)
if theta < orientation_tolerance:
# reached goal
break
w = cap(-0.5, self.wpid.compute(theta, dt), 0.5)
a.z = w
l.x = 0
self.cmd_pub.publish(self.cmd_msg)
r.sleep()
### REACH DESTINATION ###
self.wpid.reset()
self.vpid.reset()
while True:
dx,dy = (pt[0] - self.position[0], pt[1] - self.position[1])
theta = np.arctan2(dy,dx) - self.orientation # angular difference
theta = np.arctan2(np.sin(theta), np.cos(theta)) # lazy normalize
delta = np.sqrt(dx**2+dy**2)
if delta < position_tolerance:
# reached goal
break
w = cap(-0.5, self.wpid.compute(theta, dt), 0.5)
v = cap(-0.5, self.vpid.compute(delta, dt), 0.5)
a.z = w
l.x = v
self.cmd_pub.publish(self.cmd_msg)
r.sleep()
class SpheroControl:
def __init__(self, velocity_pub):
#rospy.init_node('SpheroControl2')
self.velocity_pub = velocity_pub
#self.sub = rospy.Subscriber('zoom_speed', Twist, self.run)
self.white_width = 70
self.black_width = 11
self.distance_covered = 0.0
#self.PAS = position_and_speed()
self.pid_x = PID(0.55, 0, 0, 1, -1)
self.pid_y = PID(0.55, 0, 0, 1, -1)
self.old_time = rospy.get_time()
#self.run(100)
#rospy.Rate(20)
#rospy.spin()
self.firstPass = True
def run_y(
self,
destination,
current_position,
first_Time,
enemy_green_pos,
enemy_red_pos,
):
velocity = Twist()
# d = down - positive x axis
# r = right - positive y axis
# l = left - negative y axis
# u = up - negative x axis
"""
if direction != self.old_direction:
self.old_direction = direction
"""
#self.is_path_clear(current_position, enemy_green_pos, enemy_red_pos, destination[2])
velocity.linear.y = -120 * self.pid_y.compute(
(destination[1]) / 650, current_position[1] / 650)
velocity.linear.x = 120 * self.pid_x.compute(
(destination[0]) / 650, current_position[0] / 650)
if not self.is_path_clear(current_position, enemy_green_pos,
enemy_red_pos, destination[2]):
velocity.linear.x = 0
velocity.linear.y = 0
if destination[2] == 'u' or destination[2] == 'd':
#velocity.linear.x = 0
error = destination[1] - current_position[1]
else:
#velocity.linear.y = 0
error = destination[0] - current_position[0]
'''
if first_Time == True:
if destination[2] == 'u':
velocity.linear.y=50
elif destination[2] == 'd':
velocity.linear.y=-50
elif destination[2] == 'r':
velocity.linear.x=50
elif destination[2] == 'l':
velocity.linear.x=-50
'''
#print("sphero command y: {}".format(velocity.linear.y))
#print("sphero command x: {}".format(velocity.linear.x))
#print("Distance covered: {}".format(self.distance_covered))
#print()
"""
if direction == 'forward':
velocity.linear.x = self.pid.compute(path_length, distance_covered)
elif direction == 'right':
velocity.linear.y = -self.pid.compute(path_length, distance_covered)
elif direction == 'left':
velocity.linear.y = self.pid.compute(path_length, distance_covered)
elif direction == 'backward':
velocity.linear.x = -self.pid.compute(path_length, distance_covered)
"""
#print("current position y: " + str(current_position[0]))
self.velocity_pub.publish(velocity)
return error
def is_path_clear(self, current_position, enemy_green_pos, enemy_red_pos,
direction):
clear = True
if (direction == "u" or direction == "d"):
if ((enemy_green_pos[0] < current_position[0] + 35
and enemy_green_pos[0] > current_position[0] - 35)
or (enemy_red_pos[0] < current_position[0] + 35
and enemy_red_pos[0] > current_position[0] - 35)):
if direction == "u":
if ((enemy_green_pos[1] < current_position[1] - 70
and enemy_green_pos[1] > current_position[1])
or (enemy_red_pos[1] < current_position[1] - 70
and enemy_red_pos[1] > current_position[1])):
clear = False
if direction == "d":
if ((enemy_green_pos[1] < current_position[1] + 70
and enemy_green_pos[1] > current_position[1])
or (enemy_red_pos[1] < current_position[1] + 70
and enemy_red_pos[1] > current_position[1])):
clear = False
if (direction == "l" or direction == "r"):
if ((enemy_green_pos[1] < current_position[1] + 35
and enemy_green_pos[1] > current_position[1] - 35)
or (enemy_red_pos[1] < current_position[1] + 35
and enemy_red_pos[1] > current_position[1] - 35)):
if direction == "l":
if ((enemy_green_pos[0] < current_position[0] - 70
and enemy_green_pos[0] > current_position[0])
or (enemy_red_pos[0] < current_position[0] - 70
and enemy_red_pos[0] > current_position[0])):
clear = False
if direction == "r":
if ((enemy_green_pos[0] < current_position[0] + 70
and enemy_green_pos[0] > current_position[0])
or (enemy_red_pos[0] < current_position[0] + 70
and enemy_red_pos[0] > current_position[0])):
clear = False
print(clear)
return clear
"""
if direction == 'forward':
velocity.linear.x = self.pid.compute(path_length, distance_covered)
elif direction == 'right':
velocity.linear.y = -self.pid.compute(path_length, distance_covered)
elif direction == 'left':
velocity.linear.y = self.pid.compute(path_length, distance_covered)
elif direction == 'backward':
velocity.linear.x = -self.pid.compute(path_length, distance_covered)
"""
print("current position y: " + str(current_position[0]))
self.velocity_pub.publish(velocity)
return error
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:
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(50)
self.pid_vz.set_ki(40)
self.pid_vz.set_kd(0)
# Add parameters for z controller
self.pid_z.set_kp(0.8)
self.pid_z.set_ki(0)
self.pid_z.set_kd(0)
self.t_old = datetime.now()
#########################################################
#########################################################
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.
hover_0 = 650.95
Ts = datetime.now() - self.t_old
self.t_old = datetime.now()
#print( Ts.total_seconds())
print(self.z_sp)
print(self.z_mv)
self.vz_sp = self.pid_z.compute(self.z_sp, self.z_mv)
self.mot_speed = self.pid_vz.compute(self.vz_sp,
self.vz_mv) + hover_0
#self.mot_speed = hover_0
print("mot_speed ", 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 AngleTiltCtl:
def __init__(self):
self.first_measurement = False
self.t = 0
self.t_old = 0
# Initialize subscribers
# Pose subscriber
self.pose_sub = rospy.Subscriber('/morus/pose', PoseStamped,
self.pose_cb)
# Odometry subscriber
self.odometry = rospy.Subscriber('/morus/odometry', Odometry,
self.odometry_cb)
# IMU subscriber
self.imu = rospy.Subscriber('/morus/Imu', Imu, self.imu_cb)
# Pose reference subscriber
self.pose_sp = rospy.Subscriber('/morus/pose_ref', Vector3,
self.pose_sp_cb)
self.angle_sp = rospy.Subscriber('/morus/angle_ref', Vector3,
self.angle_sp_cb)
# Initialize publishers, motor speed and tilt for roll and pitch
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=1)
self.pub_roll_tilt0 = rospy.Publisher(
'/morus/angle_tilt_0_controller/command', Float64, queue_size=1)
self.pub_roll_tilt1 = rospy.Publisher(
'/morus/angle_tilt_2_controller/command', Float64, queue_size=1)
self.pub_pitch_tilt0 = rospy.Publisher(
'/morus/angle_tilt_1_controller/command', Float64, queue_size=1)
self.pub_pitch_tilt1 = rospy.Publisher(
'/morus_angle_tilti_3_controller/command', Float64, queue_size=1)
# Publishing PIDs in order to use dynamic reconfigure
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_pitch_rate_PID = rospy.Publisher('PID_pitch_rate',
PIDController,
queue_size=1)
self.pub_pitch_PID = rospy.Publisher('PID_pitch',
PIDController,
queue_size=1)
self.pub_roll_rate_PID = rospy.Publisher('PID_roll_rate',
PIDController,
queue_size=1)
self.pub_roll_PID = rospy.Publisher('PID_roll',
PIDController,
queue_size=1)
self.pub_yaw_PID = rospy.Publisher('PID_yaw_rate_PID',
PIDController,
queue_size=1)
self.pub_angles = rospy.Publisher('rpy_angles', Vector3, queue_size=1)
self.pub_angles_sp = rospy.Publisher('rpy_angles_sp',
Vector3,
queue_size=1)
self.ros_rate = rospy.Rate(50)
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.euler_mv = Vector3(0., 0., 0) # measured euler angles
self.euler_sp = Vector3(0., 0., 0.) # euler angles referent values
self.pose_sp = Vector3(0., 0., 0.0)
self.pose_mv = Vector3(0., 0., 0.)
self.euler_rate_mv = Vector3(0, 0, 0) # measured angular velocities
self.roll_sp_filt, self.pitch_sp_filt, self.yaw_sp_filt = 0, 0, 0
self.dwz = 0
########################################
# Position control PIDs
self.pid_x = PID(4, 1, 2, 0.05, -0.05)
self.pid_y = PID(4, 1, 2, 0.05, -0.05)
# Define PID for height control
self.z_ref_filt = 0
self.z_mv = 0
self.pid_z = PID(4.1, 0.01, 1.5, 4, -4)
self.pid_vz = PID(85, 0.1, 15, 200, -200)
########################################
########################################
# initialize pitch_rate PID controller
self.pitch_rate_PID = PID(2, 0.05, 1, 50, -50)
self.pitch_PID = PID(75, 0, 1, 150, -150)
# initialize roll rate PID controller
self.roll_rate_PID = PID(2, 0.05, 1, 0.5, -0.5)
self.roll_PID = PID(75, 0, 5, +150, -150)
# initialize yaw rate PID controller
self.yaw_rate_PID = PID(35, 2, 35, 50, -50)
self.yaw_PID = PID(2, 0, 2, 30, -30)
def pose_cb(self, msg):
"""
Callback functon for assigning values from pose IMU
:param msg: /morus/pose PoseStamped msg type used to extract pose and orientation of UAV
"""
# entered subscribed callback func, set first_measurement flag as True
self.first_measurement = True
self.pose_mv.x = msg.pose.position.x
self.pose_mv.y = msg.pose.position.y
self.pose_mv.z = msg.pose.position.z
self.qx = msg.pose.orientation.x
self.qy = msg.pose.orientation.y
self.qz = msg.pose.orientation.z
self.qw = msg.pose.orientation.w
def odometry_cb(self, msg):
"""
Callback function for assigning values from odometry measurements
:param msg: nav_msgs/Odometry,
an estimate of position and velocity in free space
"""
self.lin_x = msg.twist.twist.linear.x
self.lin_y = msg.twist.twist.linear.y
self.lin_z = msg.twist.twist.linear.z
# TO DO: transform speeds global speed
self.euler_rate_mv.x = msg.twist.twist.angular.x
self.euler_rate_mv.y = msg.twist.twist.angular.y
self.euler_rate_mv.z = msg.twist.twist.angular.z
def pose_sp_cb(self, msg):
self.pose_sp.x = msg.x
self.pose_sp.y = msg.y
self.pose_sp.z = msg.z
def angle_sp_cb(self, msg):
self.euler_sp.x = msg.x
self.euler_sp.y = msg.y
self.euler_sp.z = msg.z
def imu_cb(self, msg):
"""
Callback function used to extract measured values from IMU, lin_acc and ang_vel
:param msg:
:return:
"""
self.lin_acc_x = msg.linear_acceleration.x
## TO DO: add rest if needed
def quat_to_eul_conv(self, qx, qy, qz, qw):
"""
Convert quaternions to euler angles (roll, pitch, yaw)
"""
# roll (x-axis rotation)
sinr = 2. * (qw * qx + qy * qz)
cosr = 1. - 2. * (qx * qx + qy * qy)
self.roll = math.atan2(sinr, cosr)
# pitch (y-axis rotation)
sinp = 2. * (qw * qy - qz * qx)
sinp = 1. if sinp > 1. else sinp
sinp = -1. if sinp < -1. else sinp
self.pitch = math.asin(sinp)
# yaw (z-axis rotation)
siny = 2. * (qw * qz + qx * qy)
cosy = 1 - 2. * (qy * qy + qz * qz)
self.yaw = math.atan2(siny, cosy)
self.euler_mv.x = self.roll
self.euler_mv.y = self.pitch
self.euler_mv.z = self.yaw
def run(self):
"""
Runs quadcopter control algorithm
"""
while not self.first_measurement:
print("Waiting for first measurement")
rospy.sleep(0.5)
print("Started angle control")
self.t_old = rospy.Time.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
# discretization time
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
if dt > 0.105 or dt < 0.095:
print(dt)
self.t_old = t
self.quat_to_eul_conv(self.qx, self.qy, self.qz, self.qw)
self.hover_speed = math.sqrt(293 / 0.000456874 / 4)
# HEIGHT CONTROL:
a = 0.2
self.z_ref_filt = (1 - a) * self.z_ref_filt + a * self.pose_sp.z
vz_ref = self.pid_z.compute(self.z_ref_filt, self.pose_mv.z, dt)
domega_z = self.pid_vz.compute(vz_ref, self.vz_mv, dt)
## ATTITUDE CONTROL:
# roll cascade (first PID -> roll ref, second PID -> roll_rate ref)
#self.euler_sp.x = -self.pid_y.compute(self.pose_sp.y, self.pose_mv.y, dt)
b = 0.5
self.roll_sp_filt = (1 -
b) * self.euler_sp.x + b * self.roll_sp_filt
roll_rate_ref = self.roll_rate_PID.compute(self.roll_sp_filt,
self.euler_mv.x, dt)
dwx = self.roll_PID.compute(roll_rate_ref, self.euler_rate_mv.x,
dt)
# pitch cascade(first PID -> pitch ref, second PID -> pitch_rate ref)
#self.euler_sp.y = self.pid_x.compute(self.pose_sp.x, self.pose_mv.x, dt)
b = 0.5
self.pitch_sp_filt = (1 -
b) * self.euler_sp.y + b * self.pitch_sp_filt
pitch_rate_ref = self.pitch_rate_PID.compute(
self.pitch_sp_filt, self.euler_mv.y, dt)
dwy = self.pitch_PID.compute(pitch_rate_ref, self.euler_rate_mv.y,
dt)
# yaw cascade (first PID -> yaw ref, second PID -> yaw_rate ref)
a = 0.3
self.yaw_sp_filt = (1 - a) * self.euler_sp.z + a * self.yaw_sp_filt
yaw_rate_ref = self.yaw_rate_PID.compute(self.yaw_sp_filt,
self.euler_mv.z, dt)
dwz = self.yaw_PID.compute(yaw_rate_ref, self.euler_rate_mv.z, dt)
if VERBOSE:
print("Pitch speed: {}\n Roll speed: {}\n, Yaw speed: {}\n".
format(dwy, dwx, dwz))
print(
"Yaw measured_value:{}\n Yaw_reference_value:{}\n".format(
self.euler_mv.z, self.euler_sp.z))
print("Roll measured_value:{}\n, Roll_reference_value:{}\n".
format(self.euler_mv.x, self.euler_sp.x))
print("Pitch measured_value:{}\n, Pitch_reference_value:{}\n".
format(self.euler_mv.y, self.euler_sp.y))
print("x_m:{}\nx:{}\ny_m:{}\ny:{}\nz_m:{}\nz:{}".format(
self.pose_mv.x, self.pose_sp.x, self.pose_mv.y,
self.pose_sp.y, self.pose_mv.z, self.pose_sp.z))
samples_0 = numpy.random.normal(self.hoover_speed, 5, size=50000)
samples_1 = numpy.random.normal(self.hoover_speed, 5, size=50000)
samples_2 = numpy.random.normal(self.hoover_speed, 5, size=50000)
samples_3 = numpy.random.normal(self.hoover_speed, 5, size=50000)
#motor_speed_1 = self.hover_speed + domega_z + dwz - dwy
#motor_speed_2 = self.hover_speed + domega_z - dwz #+ dwx
#motor_speed_3 = self.hover_speed + domega_z + dwz + dwy
#motor_speed_4 = self.hover_speed + domega_z - dwz #- dwx
print(motor_speed_1, motor_speed_2, motor_speed_3, motor_speed_4)
#print(self.z_sp, self.z_mv)
if abs(roll_rate_ref - self.euler_rate_mv.x) < 10e-3:
dwx = 0
self.pub_roll_tilt0.publish(dwx)
self.pub_roll_tilt1.publish(-dwx)
# publish motor speeds
motor_speed_msg = Actuators()
motor_speed_msg.angular_velocities = [
motor_speed_1, motor_speed_2, motor_speed_3, motor_speed_4
]
# 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_pitch_PID.publish(self.pitch_PID.create_msg())
self.pub_pitch_rate_PID.publish(self.pitch_rate_PID.create_msg())
self.pub_roll_PID.publish(self.roll_PID.create_msg())
self.pub_roll_rate_PID.publish(self.roll_rate_PID.create_msg())
self.pub_yaw_PID.publish(self.yaw_PID.create_msg())
# publish_angles
self.pub_angles.publish(self.euler_mv)
self.pub_angles_sp.publish(self.euler_sp)
self.pub_mot.publish(motor_speed_msg)
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 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 DroneControl:
def __init__(self):
# flags
self.stop = False
self.drone_prepared = False
self.ref_set = False
self.goto = False
# max speed
self.max_thrust = 0.0
self.min_thrust = 179.0
self.max_side = 179.0
self.min_speed = 0.0
self.mid_speed = 92.0
self.u = Quaternion(self.mid_speed, self.mid_speed, self.min_thrust, self.mid_speed)
self.pid_x = PID(1, 0, 0, self.max_side - self.mid_speed, self.min_speed - self.mid_speed)
self.pid_y = PID(1, 0, 0, self.max_side - self.mid_speed, self.min_speed - self.mid_speed)
self.pid_z = PID(1, 0, 0, self.min_thrust, self.max_thrust)
self.pid_yaw = PID(1, 0, 0, self.max_side - self.mid_speed, self.min_speed - self.mid_speed)
self.pose_ref = Quaternion(0., 0., 0., -np.pi/2)
self.pose_meas = Quaternion(-1., -1., -1., -1.)
# SUBS AND PUBS
# position reference
self.pose_subscriber = rospy.Subscriber(
"drone/pose_ref",
Quaternion,
self.setpoint_cb)
# measured position
self.measurement_subscriber = rospy.Subscriber(
"drone_position",
Quaternion,
self.measurement_cb)
# key listener
self.key_subscriber = rospy.Subscriber(
"keyboard",
Vector3,
self.keyboard_cb)
self.drone_input = rospy.Publisher(
'control/drone_input',
Quaternion)
self.control_value = rospy.Publisher(
'control_value',
Quaternion,
queue_size=10)
self.error_pub = rospy.Publisher(
'pose_error',
Float64,
queue_size=10)
# Controller rate
self.controller_rate = 10
self.rate = rospy.Rate(self.controller_rate)
self.controller_info = rospy.get_param("~verbose", False)
def setpoint_cb(self, data):
self.pose_ref = data
self.ref_set = True
def measurement_cb(self, data):
self.pose_meas = data
if data.z != -1.0 and not self.ref_set:
self.ref_set = True
self.pose_ref = data
def keyboard_cb(self, data):
# pressing down => 1. drone ready; 2. drone go to ref
# pressing up => stop drone
if data.x == 1.0:
if self.drone_prepared:
self.goto = True
else:
self.drone_prepared = True
elif data.x == -1.0:
self.stop = True
self.goto = False
def controller_dron_comm(self):
self.drone_input.publish(Quaternion(self.mid_speed, self.mid_speed, self.max_thrust, self.mid_speed))
rospy.sleep(1)
self.drone_input.publish(Quaternion(self.mid_speed, self.mid_speed, self.min_thrust, self.mid_speed))
rospy.sleep(1)
def run(self):
""" First measurement for first reference. """
while not self.ref_set:
print("DroneControl.run() - Waiting for first reference.")
rospy.sleep(1)
self.controller_dron_comm()
""" Prepared drone """
while not self.drone_prepared:
print("DroneControl.run() - Waiting for drone prepared.")
rospy.sleep(1)
""" Run ROS node - computes PID algorithms for control """
print("DroneControl.run() - Starting position control")
self.t_old = rospy.Time.now()
while not rospy.is_shutdown():
self.rate.sleep()
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
if dt < 0.99 / self.controller_rate:
continue
elif self.pose_meas.z == -1.0:
continue
if self.goto:
self.pose_ref.z += 20
self.goto = False
self.t_old = t
# HEIGHT CONTROL
self.u.z = -self.pid_x.compute(self.pose_ref.z, self.pose_meas.z, dt) + self.u.z
# PITCH CONTROL OUTER LOOP
# x - position control
self.u.x = self.pid_x.compute(self.pose_ref.x, self.pose_meas.x, dt) + self.mid_speed
# ROLL CONTROL OUTER LOOP
# y position control
self.u.y = self.pid_y.compute(self.pose_ref.y, self.pose_meas.y, dt) + self.mid_speed
# PITCH AND ROLL YAW ADJUSTMENT
roll_sp_2 = math.cos(self.pose_meas.w) * self.u.x + \
math.sin(self.pose_meas.w) * self.u.y
self.u.y = math.cos(self.pose_meas.w) * self.u.y - \
math.sin(self.pose_meas.w) * self.u.x
self.u.x = roll_sp_2
# YAW CONTROL
self.u.w = self.pid_yaw.compute(-np.pi/2, self.pose_meas.w, dt)
# Calculate position error
error = math.sqrt((self.pose_ref.x - self.pose_meas.x) ** 2 +
(self.pose_ref.y - self.pose_meas.y) ** 2 +
(self.pose_ref.z - self.pose_meas.z) ** 2)
self.error_pub.publish(error)
# Print out controller information
if self.controller_info:
print(dt)
print("Comparison x:{}\nx_m:{}\ny:{}\ny_m:{}\nz:{}\nz_m{}\nyaw:{}\nyaw_m:{}".format(
self.pose_ref.x,
self.pose_meas.x,
self.pose_ref.y,
self.pose_meas.y,
self.pose_ref.z,
self.pose_meas.z,
self.pose_ref.w,
self.pose_meas.w))
print("Current quadcopter height is: {}".format(self.pose_meas.z))
print("Pitch PID output is:{}\n"
"Roll PID output is:{}\n"
"Yaw PID output is:{}\n"
"Error: {}\n".format(self.u.x, self.u.y, self.u.w, error))
if self.stop:
self.drone_input.publish(Quaternion(self.mid_speed, self.mid_speed, self.min_thrust, self.mid_speed))
else:
self.drone_input.publish(self.u)
from pid import PID
import matplotlib.pyplot as plt
import math
test_pid = PID(0, 0, 100, 0.05, 0.8, 0, 10)
inp = [math.sin(math.radians(x)) for x in range(0, 359)]
out = [test_pid.compute(x)[0] for x in inp]
err = [test_pid.compute(x)[1] for x in inp]
print("Max Output: %.5f | Max Err: %.5f\n" % (max(out), max(err)))
plt.ion()
fig = plt.figure()
plt.plot(range(0, 359), inp, 'b-', range(0, 359), out, 'r-', range(0, 359),
[0 for x in range(0, 359)], 'k--', range(0, 359), err, 'm--')
fig.canvas.draw()
plt.show(block=False)
while True:
fig.canvas.draw()
plt.pause(0.05)
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
self.t_old = datetime.now()
##################################################################
##################################################################
# Add your PID params here
self.pid_roll_rate.set_kp(80)
self.pid_roll_rate.set_ki(41)
self.pid_roll_rate.set_kd(0)
self.pid_roll.set_kp(5.0)
self.pid_roll.set_ki(0.0)
self.pid_roll.set_kd(0.0)
self.pid_pitch_rate.set_kp(80)
self.pid_pitch_rate.set_ki(41)
self.pid_pitch_rate.set_kd(0)
self.pid_pitch.set_kp(5.0)
self.pid_pitch.set_ki(0.0)
self.pid_pitch.set_kd(0.0)
self.pid_yaw_rate.set_kp(80)
self.pid_yaw_rate.set_ki(40)
self.pid_yaw_rate.set_kd(0.0)
self.pid_yaw.set_kp(0.4)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0.0)
##################################################################
##################################################################
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
u=self.pid_roll.compute(self.euler_sp.x,self.euler_mv.x)
mot_speedr=self.pid_roll_rate.compute(u,self.euler_rate_mv.x)
mot_sp1_r=-mot_speedr
mot_sp2_r=mot_speedr
mot_sp3_r=mot_speedr
mot_sp4_r=-mot_speedr
u=self.pid_pitch.compute(self.euler_sp.y,self.euler_mv.y)
mot_speedp=self.pid_pitch_rate.compute(u,self.euler_rate_mv.y)
mot_sp1_p=-mot_speedp
mot_sp2_p=-mot_speedp
mot_sp3_p=mot_speedp
mot_sp4_p=mot_speedp
u=self.pid_yaw.compute(self.euler_sp.z,self.euler_mv.z)
mot_speedy=self.pid_yaw_rate.compute(u,self.euler_rate_mv.z)
print("-----------")
print("YAW >>> " , mot_speedy )
mot_sp1_y=-mot_speedy
mot_sp2_y=mot_speedy
mot_sp3_y=-mot_speedy
mot_sp4_y=mot_speedy
mot_sp1=self.w_sp + mot_sp1_r + mot_sp1_p + mot_sp1_y
mot_sp2=self.w_sp + mot_sp2_r + mot_sp2_p + mot_sp2_y
mot_sp3=self.w_sp + mot_sp3_r + mot_sp3_p + mot_sp3_y
mot_sp4=self.w_sp + mot_sp4_r + mot_sp4_p + mot_sp4_y
Ts = datetime.now() - self.t_old
self.t_old = datetime.now()
print( Ts.total_seconds())
####################################################################
####################################################################
# 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
from pid import PID
import matplotlib.pyplot as plt
import math
test_pid = PID(0, 0, 100, 0.05, 0.8, 0, 10)
inp = [math.sin(math.radians(x)) for x in range(0, 359)]
out = [test_pid.compute(x)[0] for x in inp]
err = [test_pid.compute(x)[1] for x in inp]
print("Max Output: %.5f | Max Err: %.5f\n" % (max(out), max(err)))
plt.ion()
fig = plt.figure()
plt.plot(range(0, 359), inp, 'b-',
range(0, 359), out, 'r-',
range(0, 359), [0 for x in range(0, 359)], 'k--',
range(0, 359), err, 'm--')
fig.canvas.draw()
plt.show(block=False)
while True:
fig.canvas.draw()
plt.pause(0.05)
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:
/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 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 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 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:
/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 RealFlight:
def __init__(self):
self.pose_sub = rospy.Subscriber("bebop/optitrack/pose", PoseStamped,
self.pose_cb)
self.vel_sub = rospy.Subscriber("/bebop/optitrack/velocity",
TwistStamped, self.vel_cb)
self.pose_subscriber = rospy.Subscriber("/bebop/pos_ref", Vector3,
self.setpoint_cb)
self.angle_subscriber = rospy.Subscriber("/bebop/angle_ref", Vector3,
self.angle_cb)
self.pose_pub = rospy.Publisher("/bebop/pose_set",
Vector3,
queue_size=10)
self.rms_pub = rospy.Publisher("/bebop/rms", Float64, queue_size=10)
self.vel_publisher = rospy.Publisher('/bebop/cmd_vel',
Twist,
queue_size=10)
self.twist_msg = Twist()
self.sleep_sec = 2
self.first_measurement1 = False
self.first_measurement2 = False
self.controller_rate = 50
self.rate = rospy.Rate(self.controller_rate)
self.euler_mv = Vector3(0., 0., 0.)
self.euler_sp = Vector3(0., 0., 0.)
self.euler_rate_mv = Vector3(0., 0., 0.)
self.pose_sp = Vector3(0., 0., 1.)
self.qx, self.qy, self.qz, self.qw = 0., 0., 0., 0.
self.p, self.q, self.r, = 0., 0., 0.
self.x_mv, self.y_mv, self.z_mv = 0., 0., 0.
# Pre-filter constants
self.filt_const_x = 0.5
self.filt_const_y = 0.5
self.filt_const_z = 0.1
self.x_filt_sp = 0.
self.y_filt_sp = 0.
self.z_filt_sp = 0.
self.pid_z = PID(4, 0.05, 0.1, MAX_VZ, -MAX_VZ)
self.pid_x = PID(0.25, 0.0, 0.1, MAX_TILT, -MAX_TILT)
self.pid_y = PID(0.25, 0.0, 0.1, MAX_TILT, -MAX_TILT)
self.yaw_PID = PID(1, 0, 0.0, MAX_ROTV, -MAX_ROTV)
self.RMS = 0
self.counter = 0
def pose_cb(self, data):
"""PoseStamped msg"""
self.first_measurement1 = True
self.x_mv = data.pose.position.x
self.y_mv = data.pose.position.y
self.z_mv = data.pose.position.z
self.qx = data.pose.orientation.x
self.qy = data.pose.orientation.y
self.qz = data.pose.orientation.z
self.qw = data.pose.orientation.w
def vel_cb(self, data):
"""TwistStamped msg"""
self.first_measurement2 = True
self.vx_mv = data.twist.linear.x
self.vy_mv = data.twist.linear.y
self.vz_mv = data.twist.linear.z
self.p = data.twist.angular.x
self.q = data.twist.angular.y
self.r = data.twist.angular.z
def convert_to_euler(self, qx, qy, qz, qw):
"""Calculate roll, pitch and yaw angles/rates with quaternions"""
# 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 = self.p
q = self.q
r = self.r
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 run(self):
""" Run ROS node - computes PID algorithms for z and vz control """
#while not self.first_measurement1 and not self.first_measurement2:
#print("LaunchBebop.run() - Waiting for first measurement.")
#rospy.sleep(self.sleep_sec)
print("LaunchBebop.run() - Starting position control")
self.t_old = rospy.Time.now()
while not rospy.is_shutdown():
self.rate.sleep()
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
self.t_old = t
if dt < 1.0 / self.controller_rate:
continue
self.convert_to_euler(self.qx, self.qy, self.qz, self.qw)
# HEIGHT CONTROL
self.z_filt_sp = prefilter(self.pose_sp.z, self.z_filt_sp,
self.filt_const_z)
vz_sp = self.pid_z.compute(self.z_filt_sp, self.z_mv, dt)
# PITCH CONTROL OUTER LOOP
# x - position control
self.x_filt_sp = prefilter(self.pose_sp.x, self.x_filt_sp,
self.filt_const_x)
pitch_sp = self.pid_x.compute(self.pose_sp.x, self.x_mv, dt)
# ROLL CONTROL OUTER LOOP
# y position control
self.y_filt_sp = prefilter(self.pose_sp.y, self.y_filt_sp,
self.filt_const_y)
roll_sp = -self.pid_y.compute(self.pose_sp.y, self.y_mv, dt)
# PITCH AND ROLL YAW ADJUSTMENT
roll_sp_2 = math.cos(self.euler_mv.z) * roll_sp + \
math.sin(self.euler_mv.z) * pitch_sp
pitch_sp = math.cos(self.euler_mv.z) * pitch_sp - \
math.sin(self.euler_mv.z) * roll_sp
roll_sp = roll_sp_2
# YAW CONTROL
error_yrc = self.euler_sp.z - self.euler_mv.z
if math.fabs(error_yrc) > math.pi:
self.euler_sp.z = (self.euler_mv.z/math.fabs(self.euler_mv.z))*\
(2*math.pi - math.fabs(self.euler_sp.z))
rot_v_sp = self.yaw_PID.compute(self.euler_sp.z, self.euler_mv.z,
dt)
self.twist_msg.linear.x = pitch_sp / MAX_TILT
self.twist_msg.linear.y = -roll_sp / MAX_TILT
self.twist_msg.linear.z = vz_sp / MAX_VZ
self.twist_msg.angular.z = rot_v_sp / MAX_ROTV
self.RMS += math.sqrt((self.pose_sp.x - self.x_mv)**2 + (self.pose_sp.y - self.y_mv)**2 \
+ (self.pose_sp.z - self.z_mv)**2)
self.counter += 1
if VERBOSE:
print(
"x_sp: {}\n x_mv: {}\n y_sp: {}\n y_mv: {}\n z_sp: {}\n z_mv: {}\n"
.format(self.pose_sp.x, self.x_mv, self.pose_sp.y,
self.y_mv, self.pose_sp.z, self.z_mv))
print("Yaw measured: {}\n ".format(self.euler_mv.z))
print("Pitch setpoint: {}".format(pitch_sp))
print("Roll setpoint: {}".format(roll_sp))
print("Yaw setpoint: {}".format(rot_v_sp))
print("lin_vel command: {}".format(self.twist_msg.linear))
print("ang_vel command: {}".format(self.twist_msg.angular))
print("RMS : {}".format(self.RMS / self.counter))
self.vel_publisher.publish(self.twist_msg)
self.pose_pub.publish(self.pose_sp)
self.rms_pub.publish(Float64(self.RMS))
def angle_cb(self, data):
self.euler_sp = Vector3(data.x, data.y, data.z)
def setpoint_cb(self, data):
self.pose_sp.x = data.x
self.pose_sp.y = data.y
self.pose_sp.z = data.z
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
import numpy as np
import matplotlib.pyplot as plt
import sys
sys.path.append('..')
from cube_system import CubeSystem
from pid import PID
cs = CubeSystem()
cs.current_theta_b = np.deg2rad(-6)
controller = PID(-80, -200, -8, 0, 0.02)
time_data = []
theta_b_data = []
current_data = []
for time, theta, _, _, _ in cs.simulate(max_time=2, h=0.001, sample_time=0.02):
cs.update_BLDC_current(np.clip(controller.compute(theta, time), -10, 10))
time_data.append(time)
theta_b_data.append(np.rad2deg(theta))
current_data.append(cs.I_val)
plt.style.use('ggplot')
plt.subplot(1, 2, 1)
plt.plot(time_data, theta_b_data)
plt.xlabel("Time [s]")
plt.ylabel("theta [degrees]")
plt.subplot(1, 2, 2)
plt.plot(time_data, current_data)
plt.xlabel("Time [s]")
plt.ylabel("BLDC Current (A)")
plt.show()
class AngleTiltCtl:
def __init__(self):
self.first_measurement = False
self.t = 0
self.t_old = 0
# Initialize subscribers
# Pose subscriber
self.pose_sub = rospy.Subscriber('/morus/pose', PoseStamped,
self.pose_cb)
# Odometry subscriber
self.odometry = rospy.Subscriber('/morus/odometry', Odometry,
self.odometry_cb)
# IMU subscriber
self.imu = rospy.Subscriber('/morus/Imu', Imu, self.imu_cb)
# Pose reference subscriber
self.pose_sp = rospy.Subscriber('/morus/pose_ref', Vector3,
self.pose_sp_cb)
self.angle_sp = rospy.Subscriber('/morus/angle_ref', Vector3,
self.angle_sp_cb)
self.tilt_x_sp = rospy.Subscriber('/morus/tilt_x_ref', Float64,
self.tilt_sp_x_cb)
self.tilt_y_sp = rospy.Subscriber('/morus/tilt_y_ref', Float64,
self.tilt_sp_y_cb)
self.ref_tilt = rospy.Subscriber('/morus/tilt_ref', Float64,
self.ref_tilt_cb)
self.vel_ref_sub = rospy.Subscriber('/morus/lin_vel_ref', Vector3,
self.vel_ref_cb)
# Initialize publishers, motor speed and tilt for roll and pitch
self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed',
Actuators,
queue_size=1)
self.pub_roll_tilt0 = rospy.Publisher(
'/morus/angle_tilt_0_controller/command', Float64, queue_size=1)
self.pub_roll_tilt1 = rospy.Publisher(
'/morus/angle_tilt_2_controller/command', Float64, queue_size=1)
self.pub_pitch_tilt0 = rospy.Publisher(
'/morus/angle_tilt_1_controller/command', Float64, queue_size=1)
self.pub_pitch_tilt1 = rospy.Publisher(
'/morus/angle_tilt_3_controller/command', Float64, queue_size=1)
self.pub_tilt_ref = rospy.Publisher("/morus/tilt_ref",
Float64,
queue_size=1)
self.pub_tilt_x_ref = rospy.Publisher("/morus/tilt_x_ref_",
Float64,
queue_size=1)
self.pub_tilt_y_ref = rospy.Publisher("/morus/tilt_y_ref_",
Float64,
queue_size=1)
self.pub_vel_ref = rospy.Publisher("/morus/pub_lin_vel_ref",
Vector3,
queue_size=1)
self.pub_pose_ref = rospy.Publisher("/morus/pub_pose_ref",
Vector3,
queue_size=1)
# Publishing PIDs in order to use dynamic reconfigure
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_pitch_rate_PID = rospy.Publisher('PID_pitch_rate',
PIDController,
queue_size=1)
self.pub_pitch_PID = rospy.Publisher('PID_pitch',
PIDController,
queue_size=1)
self.pub_roll_rate_PID = rospy.Publisher('PID_roll_rate',
PIDController,
queue_size=1)
self.pub_roll_PID = rospy.Publisher('PID_roll',
PIDController,
queue_size=1)
self.pub_yaw_PID = rospy.Publisher('PID_yaw_rate_PID',
PIDController,
queue_size=1)
self.pub_angles = rospy.Publisher('/morus/euler_mv',
Vector3,
queue_size=1)
self.pub_angles_sp = rospy.Publisher('/morus/euler_sp',
Vector3,
queue_size=1)
self.ros_rate = rospy.Rate(50)
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.euler_mv = Vector3(0., 0., 0) # measured euler angles
self.euler_sp = Vector3(0., 0., 0.) # euler angles referent values
self.pose_sp = Vector3(0., 0., 0.0)
self.pose_mv = Vector3(0., 0., 0.)
self.pose_mv_tf = Vector3(0., 0., 0.)
self.vel_mv = Vector3(0., 0., 0.)
self.vel_ref = Vector3(0., 0., 0.)
self.euler_rate_mv = Vector3(0, 0, 0) # measured angular velocities
self.roll_sp_filt, self.pitch_sp_filt, self.yaw_sp_filt = 0, 0, 0
self.dwz = 0
self.tilt_x = 0
self.tilt_y = 0
self.lim_tilt = 0.15
c = 1000
# Position control PIDs -> connect directly to rotors tilt
self.pid_x = PID(1.25, 0, 0., self.lim_tilt, -self.lim_tilt)
self.pid_vx = PID(1.85, 0, 1.2, 500, -500)
self.pid_y = PID(1.25, 0, 0., self.lim_tilt, -self.lim_tilt)
self.pid_vy = PID(1.85, 0, 1.2, 500, -500)
# Define PID for height control
self.z_ref_filt = 0
self.z_mv = 0
self.pid_z = PID(3, 0.01, 1.5, 4, -4)
self.pid_vz = PID(85, 0.1, 15, 200, -200)
# initialize pitch_rate PID controller
self.pitch_rate_PID = PID(4, 0.05, 1, 50, -50)
self.pitch_PID = PID(75, 0, 1, 50, -50)
# initialize roll rate PID controller
self.roll_rate_PID = PID(4, 0.05, 1, 50, -50)
self.roll_PID = PID(75, 0, 1, 50, -50)
# initialize yaw rate PID controller
self.yaw_rate_PID = PID(15, 0.1, 25, 5, -5)
self.yaw_PID = PID(95, 0.5, 65, +150, -150)
def ref_tilt_cb(self, msg):
self.ref_tilt = msg.data
def vel_ref_cb(self, msg):
self.vel_ref.x = msg.x
self.vel_ref.y = msg.y
self.vel_ref.z = msg.z
def pose_cb(self, msg):
"""
Callback functon for assigning values from pose IMU
:param msg: /morus/pose PoseStamped msg type used to extract pose and orientation of UAV
"""
# entered subscribed callback func, set first_measurement flag as True
self.first_measurement = True
self.pose_mv.x = msg.pose.position.x
self.pose_mv.y = msg.pose.position.y
self.pose_mv.z = msg.pose.position.z
self.qx = msg.pose.orientation.x
self.qy = msg.pose.orientation.y
self.qz = msg.pose.orientation.z
self.qw = msg.pose.orientation.w
def tilt_sp_x_cb(self, msg):
self.tilt_x = msg.data
def tilt_sp_y_cb(self, msg):
self.tilt_y = msg.data
def odometry_cb(self, msg):
"""
Callback function for assigning values from odometry measurements
:param msg: nav_msgs/Odometry,
an estimate of position and velocity in free space
"""
self.vel_mv.x = msg.twist.twist.linear.x
self.vel_mv.y = msg.twist.twist.linear.y
self.vel_mv.z = msg.twist.twist.linear.z
self.euler_rate_mv.x = msg.twist.twist.angular.x
self.euler_rate_mv.y = msg.twist.twist.angular.y
self.euler_rate_mv.z = msg.twist.twist.angular.z
def pose_sp_cb(self, msg):
self.pose_sp.x = msg.x
self.pose_sp.y = msg.y
self.pose_sp.z = msg.z
def angle_sp_cb(self, msg):
self.euler_sp.x = msg.x
self.euler_sp.y = msg.y
self.euler_sp.z = msg.z
def imu_cb(self, msg):
"""
Callback function used to extract measured values from IMU, lin_acc and ang_vel
:param msg:
:return:
"""
self.lin_acc_x = msg.linear_acceleration.x
## TO DO: add rest if needed
def quat_to_eul_conv(self, qx, qy, qz, qw):
"""
Convert quaternions to euler angles (roll, pitch, yaw)
"""
# roll (x-axis rotation)
sinr = 2. * (qw * qx + qy * qz)
cosr = 1. - 2. * (qx * qx + qy * qy)
self.roll = math.atan2(sinr, cosr)
# pitch (y-axis rotation)
sinp = 2. * (qw * qy - qz * qx)
sinp = 1. if sinp > 1. else sinp
sinp = -1. if sinp < -1. else sinp
self.pitch = math.asin(sinp)
# yaw (z-axis rotation)
siny = 2. * (qw * qz + qx * qy)
cosy = 1 - 2. * (qy * qy + qz * qz)
self.yaw = math.atan2(siny, cosy)
self.euler_mv.x = self.roll
self.euler_mv.y = self.pitch
self.euler_mv.z = self.yaw
def run(self):
"""
Runs quadcopter control algorithm
"""
while not self.first_measurement:
self.tilt_x = 0
self.tilt_y = 0
self.ref_tilt = 0
self.pub_tilt_x_ref.publish(self.tilt_x)
self.pub_tilt_y_ref.publish(self.tilt_y)
self.pub_pose_ref.publish(self.pose_sp)
print("Waiting for first measurement")
rospy.sleep(0.5)
print("Started angle control")
self.t_old = rospy.Time.now()
while not rospy.is_shutdown():
self.ros_rate.sleep()
# discretization time
t = rospy.Time.now()
dt = (t - self.t_old).to_sec()
if dt > 0.105 or dt < 0.095:
print(dt)
if dt < 0.01:
dt = 0.01
self.t_old = t
self.quat_to_eul_conv(self.qx, self.qy, self.qz, self.qw)
self.hover_speed = math.sqrt(293 / 0.000456874 / 4)
# HEIGHT CONTROL:
a = 0.2
self.z_ref_filt = (1 - a) * self.z_ref_filt + a * self.pose_sp.z
vz_ref = self.pid_z.compute(self.z_ref_filt, self.pose_mv.z, dt)
domega_z = self.pid_vz.compute(vz_ref, self.vz_mv, dt)
# ATTITUDE CONTROL:
# roll cascade (first PID -> y ref, second PID -> tilt_roll_rate ref)
#b = 0.2
#self.roll_sp_filt = (1 - b) * self.euler_sp.x + b * self.roll_sp_filt
roll_rate_ref = self.roll_rate_PID.compute(self.euler_sp.x,
self.euler_mv.x, dt)
dwx = self.roll_PID.compute(roll_rate_ref, self.euler_rate_mv.x,
dt)
# pitch cascade(first PID -> pitch ref, second PID -> pitch_rate ref
b = 0.3
#self.pitch_sp_filt = (1 - b) * self.euler_sp.y + b * self.pitch_sp_filt
pitch_rate_ref = self.pitch_rate_PID.compute(
self.euler_sp.y, self.euler_mv.y, dt)
dwy = self.pitch_PID.compute(pitch_rate_ref, self.euler_rate_mv.y,
dt)
#dwy = 0
# yaw cascade (first PID -> yaw ref, second PID -> yaw_rate ref)
a = 0.8
self.yaw_sp_filt = (1 - a) * self.euler_sp.z + a * self.yaw_sp_filt
yaw_rate_ref = self.yaw_rate_PID.compute(self.yaw_sp_filt,
self.euler_mv.z, dt)
dwz = self.yaw_PID.compute(yaw_rate_ref, self.euler_rate_mv.z, dt)
# POSE CONTROL WITH ROTORS TILT
# Global speed -> local speed
vel_mv_x_corr = self.vel_mv.x * np.cos(
self.yaw) - self.vel_mv.y * np.sin(self.yaw)
vel_mv_y_corr = self.vel_mv.x * np.sin(
self.yaw) + self.vel_mv.y * np.cos(self.yaw)
vel_mv_x_corr = self.vel_mv.x
vel_mv_y_corr = self.vel_mv.y
pose_sp_x = prefilter(self.pose_mv.x, 0.5, self.pose_sp.x)
pose_sp_y = prefilter(self.pose_mv.y, 0.5, self.pose_sp.y)
vel_sp_x = self.pid_vx.compute(pose_sp_x, self.pose_mv.x, dt)
vel_sp_y = self.pid_vy.compute(pose_sp_y, self.pose_mv.y, dt)
# --> added vel_ref to configure inner control loop
tilt_x = self.pid_x.compute(vel_sp_x, self.vel_mv.x, dt)
tilt_y = self.pid_y.compute(vel_sp_y, self.vel_mv.y, dt)
# small addition in order to be still while on ground
if self.pose_mv.z < 1.0:
tilt_tf_x = 0
tilt_tf_y = 0
if abs(self.euler_sp.z - self.euler_mv.z) > 0.1:
tilt_tf_x = np.cos(self.euler_mv.z) * tilt_x + np.sin(
self.euler_mv.z) * tilt_y
tilt_tf_y = -np.sin(self.euler_mv.z) * tilt_x + np.cos(
self.euler_mv.z) * tilt_y
else:
tilt_tf_x = np.cos(self.euler_sp.z) * tilt_x + np.sin(
self.euler_sp.z) * tilt_y
tilt_tf_y = -np.sin(self.euler_sp.z) * tilt_x + np.cos(
self.euler_sp.z) * tilt_y
if VERBOSE:
print("Pitch speed: {}\n Roll speed: {}\n, Yaw speed: {}\n".
format(dwy, dwx, dwz))
print(
"Yaw measured_value:{}\n Yaw_reference_value:{}\n".format(
self.euler_mv.z, self.euler_sp.z))
print("Roll measured_value:{}\n, Roll_reference_value:{}\n".
format(self.euler_mv.x, self.euler_sp.x))
print("Pitch measured_value:{}\n, Pitch_reference_value:{}\n".
format(self.euler_mv.y, self.euler_sp.y))
print("x_m:{}\nx:{}\ny_m:{}\ny:{}\nz_m:{}\nz:{}".format(
self.pose_mv.x, self.pose_sp.x, self.pose_mv.y,
self.pose_sp.y, self.pose_mv.z, self.pose_sp.z))
motor_speed_1 = self.hover_speed + domega_z + dwz - dwy
motor_speed_2 = self.hover_speed + domega_z - dwz + dwx
motor_speed_3 = self.hover_speed + domega_z + dwz + dwy
motor_speed_4 = self.hover_speed + domega_z - dwz - dwx
print(
"rotor_front:{}\nrotor_left:{}\nrotor_back:{}\nrotor_right:{}\n"
.format(motor_speed_1, motor_speed_2, motor_speed_3,
motor_speed_4))
# CONTROL TILT
self.pub_roll_tilt0.publish(-tilt_tf_y)
self.pub_roll_tilt1.publish(+tilt_tf_y)
self.pub_pitch_tilt0.publish(tilt_tf_x)
self.pub_pitch_tilt1.publish(-tilt_tf_x)
# PLOT TILT
#self.pub_roll_tilt0.publish(self.tilt_y)
#self.pub_roll_tilt1.publish(-self.tilt_y)
#self.pub_pitch_tilt0.publish(self.tilt_x)
#self.pub_pitch_tilt1.publish(-self.tilt_x)
motor_speed_msg = Actuators()
motor_speed_msg.angular_velocities = [
motor_speed_1, motor_speed_2, motor_speed_3, motor_speed_4
]
# publish reference_data
self.pub_tilt_x_ref.publish(self.tilt_x)
self.pub_tilt_y_ref.publish(self.tilt_y)
# publish_angles
self.pub_angles.publish(self.euler_mv)
self.pub_angles_sp.publish(self.euler_sp)
self.pub_vel_ref.publish(Vector3(vel_sp_x, vel_sp_y, vz_ref))
self.pub_pose_ref.publish(self.pose_sp)
self.pub_mot.publish(motor_speed_msg)
def startSim(max_iter, state_d, pid_ks):
ii = 0
arr = [0]*15
if len(pid_ks) == 3:
arr[ii] = pid_ks[0]
arr[ii + 1] = pid_ks[1]
arr[ii + 2] = pid_ks[2]
pid_ks = arr
k = np.reshape(np.array(pid_ks), (5, 3))
saver = Saver()
collecter1 = []
collecter2 = []
collecter3 = []
collecter4 = []
state = {
"vx": 0,
"vy": 0,
"vz": 0,
"x": 0,
"y": 0,
"z": 0,
"wx": 0,
"wy": 0,
"wz": 0,
"gamma": 0, # крен
"psi": 0, # рыскание
"nu": 0 # тангаж
}
motors = [0, 0, 0, 0]
# print(k)
i = 0
# -- PIDS --
pid_dy = PID(k[0][0], k[0][1], k[0][2])
pid_dx = PID(k[1][0], k[1][1], k[1][2])
pid_dz = PID(k[2][0], k[2][1], k[2][2])
pid_gamma = PID(k[3][0], k[3][1], k[3][2])
pid_nu = PID(k[4][0], k[4][1], k[4][2])
pid_psi = PID(1, 1, 1)
pid_gamma.setLimits(-1, 1)
pid_nu.setLimits(-1, 1)
while i < max_iter:
state = world_physics(getNewState(state, motors))
saver.put(state)
P = sv.getFullP(motors)
pid_dx.setLimits((-1)*P, P)
Ux = (-1)*pid_dx.compute(state_d["x"] - state["x"])
pid_dz.setLimits((-1)*P, P)
Uz = pid_dz.compute(state_d["z"] - state["z"])
if P != 0:
state_d["gamma"] = np.clip(asin((Uz*cos(state_d["psi"]) + Ux*sin(state_d["psi"])) / P), -1, 1)
state_d["nu"] = np.clip(asin((Uz*sin(state_d["psi"]) + Ux*cos(state_d["psi"])) / P), -1, 1)
U1 = round(pid_dy.compute(state_d["y"] - state["y"]), 2)
U2 = round(pid_gamma.compute(state_d["gamma"] - state["gamma"]), 2)*U1*0.5
U3 = round(pid_nu.compute(state_d["nu"] - state["nu"]), 2)*U1*0.5
U4 = round(pid_psi.compute(state_d["psi"] - state["psi"]), 2)*U1*0.5
motors = [round(U1 - U2 + U4, 2), round(U1 + U3 - U4, 2), round(U1 + U2 + U4, 2), round(U1 - U3 - U4, 2)]
collecter1.append(U1)
collecter2.append(U2)
collecter3.append(U3)
collecter4.append(U4)
lgg._print("N____________________________", i)
lgg._print(motors)
lgg._print(state)
lgg._print(state_d)
lgg._print(U1, U2, U3, U4)
lgg._print(Ux, Uz)
# input()
i += 1
return [saver, collecter1, collecter2, collecter3, collecter4]
class DroneControl:
def __init__(self):
self.change = [0, 0, 0]
# key pressed speed increment
self.dx = 3
self.dt = 0
self.t_old = rospy.Time.now()
# max speed
self.max_thrust = 179
self.max_side = 179
self.min_speed = 0
self.mid_speed = 92
self.u_speed = Quaternion(0., 0., 0., 0.)
self.pid_x = PID(1, 0, 0, self.max_side - self.mid_speed,
self.mid_speed - self.min_speed)
self.pid_y = PID(1, 0, 0, self.max_side - self.mid_speed,
self.mid_speed - self.min_speed)
self.pid_z = PID(1, 0, 0, self.max_thrust - self.mid_speed,
self.mid_speed - self.min_speed)
self.pid_yaw = PID(1, 0, 0, self.max_side - self.mid_speed,
self.mid_speed - self.min_speed)
self.ref_set = False
self.speed = 0
self.pose_ref = Quaternion(0., 0., 0., 0.)
self.pose_meas = Vector3(0., 0., 0.)
# initialize subscribers
self.pose_subscriber = rospy.Subscriber("drone/pose_ref", Quaternion,
self.setpoint_cb)
self.measurement_subscriber = rospy.Subscriber("drone_position",
Vector3,
self.measurement_cb)
self.drone_input = rospy.Publisher('control/drone_input', Quaternion)
# initialize publishers
self.control_value = rospy.Publisher('control_value',
Quaternion,
queue_size=10)
self.toggle_speed = rospy.Publisher('toggle_speed', UInt16)
self.u = Quaternion(0., 0., 0., 0.)
# Controller rate
self.controller_rate = 50
self.rate = rospy.Rate(self.controller_rate)
def setpoint_cb(self, data):
self.pose_ref = data
self.speed = data.x
self.ref_set = True
def measurement_cb(self, data):
self.pose_meas = data
def compute_PID(self):
self.u.x = self.pid_x.compute(self.pose_ref.x, self.pose_meas.x,
dt) + self.mid_speed
self.u.y = self.pid_y.compute(self.pose_ref.y, self.pose_meas.y,
dt) + self.mid_speed
self.u.z = self.pid_z.compute(self.pose_ref.z, self.pose_meas.z,
dt) + self.mid_speed
#self.u.z = self.pid_yaw.compute(self.pose_ref.w, self.pose_meas.w, dt) + self.mid_speed
def controller_dron_comm(self):
self.drone_input.publish(
Quaternion(self.mid_speed, self.mid_speed, self.mid_speed,
self.mid_speed))
rospy.sleep(1)
self.drone_input.publish(
Quaternion(self.mid_speed, self.mid_speed, self.max_thrust,
self.mid_speed))
rospy.sleep(1)
def run(self):
""" Run ROS node - computes PID algorithms for z and vz control """
while not self.ref_set:
print("DroneControl.run() - Waiting for first reference.")
rospy.sleep(1)
print("DroneControl.run() - Starting position control")
self.controller_dron_comm()
while not rospy.is_shutdown():
self.rate.sleep()
t = rospy.Time.now()
old = [self.u.x, self.u.y, self.u.z]
for i in range(0, 2):
if not self.change[i]:
old[i] = self.mid_speed
else:
old[i] = old[i] + self.change[i]
if old[i] > self.max_side:
old[i] = self.max_side
elif old[i] < self.min_speed:
old[i] = self.min_speed
old[2] = old[2] + self.change[2]
if old[2] < self.min_speed:
old[2] = self.min_speed
elif old[2] > self.max_thrust:
old[2] = self.max_thrust
self.dt = (t - self.t_old).to_sec()
self.t_old = t
self.u_speed.x = old[0]
self.u_speed.y = old[1]
self.u_speed.z = old[2]
self.u_speed.w = old[3]
self.drone_input.publish(self.u_speed)
self.toggle_speed.publish(self.speed)
class Quad():
def __init__(self, imu, motors, altimeter):
self.imu = imu
self.altimeter = altimeter
self.motors = motors
self.pid_alt = PID(1,1,1)
self.pid_roll = PID(1,0,1)
self.pid_pitch = PID(1,0,1)
self.pid_yaw = PID(1,0,1)
self.power_precedent = 0.
self.setConsigne()
self.running = True
threading.Thread(target = self.run).start()
def getPower(self, option = 'test'):
if option == 'test':
alt = 1
power = 0.5
else:
alt = self.altimeter.getAltitude()
power = self.pid_alt.compute(alt, self.alt_c)
# print 'power',power,alt
return power
def getAttitudeRegulation(self, option = 'maintainConsign'):
roll, pitch, yaw = self.imu.getEuler()
vmin = -0.5
vmax = 0.5
if option == 'maintainConsign':
regul_roll = self.pid_roll.compute(roll, self.roll_c)
if regul_roll > vmax : regul_roll = vmax
if regul_roll < vmin : regul_roll = vmin
regul_pitch = self.pid_pitch.compute(pitch, self.pitch_c)
if regul_pitch > vmax : regul_pitch = vmax
if regul_pitch < vmin : regul_pitch = vmin
regul_yaw = self.pid_yaw.compute(yaw, self.yaw_c)
if regul_yaw > vmax : regul_yaw = vmax
if regul_yaw < vmin : regul_yaw = vmin
dmotors = (regul_roll*matRoll + 1) * (regul_pitch*matPitch + 1) * (regul_yaw*matYaw + 1)
# print 'attitude',regul_roll,regul_pitch,regul_yaw,dmotors
return dmotors
else: raise(notimplementederror)
def setDistributedPower(self):
power = self.getPower()
equilibration = self.getAttitudeRegulation()
self.distributedPower = equilibration * power
# print 'distributedPower', equilibration,power,self.distributedPower
self.motors.setMotorsSpeed(self.distributedPower)
def setConsigne(self, alt_c = 1, roll_c = 0., yaw_c = 0., pitch_c = 0.):
self.alt_c = alt_c
self.roll_c = roll_c
self.yaw_c = yaw_c
self.pitch_c = pitch_c
def run(self):
self.setConsigne()
while self.running:
self.setDistributedPower()
time.sleep(0.1)
def getDistributedPower(self):
return self.distributedPower
class PositionControl(object):
'''
Constructor for position control. Initializes parameters for PID controller.
'''
def __init__(self):
self.clock = Clock()
self.start_flag1 = False
self.start_flag2 = False
# Initialize controllers for position and velocity
self.pid_x = PID()
self.pid_vx = PID()
self.pid_y = PID()
self.pid_vy = PID()
#Initalize controller parameters.
self.pid_x.set_kp(0.25)
self.pid_x.set_kd(0.0)
self.pid_x.set_ki(0)
self.pid_vx.set_kp(0.25)
self.pid_vx.set_kd(0)
self.pid_vx.set_ki(0)
self.pid_vx.set_lim_high(0.05)
self.pid_vx.set_lim_low(-0.05)
self.pid_y.set_kp(0.25)
self.pid_y.set_kd(0.0)
self.pid_y.set_ki(0)
self.pid_vy.set_kp(0.25)
self.pid_vy.set_kd(0)
self.pid_vy.set_ki(0)
self.pid_vy.set_lim_high(0.05)
self.pid_vy.set_lim_low(-0.05)
# Initialize controller frequency
self.rate = 50
self.ros_rate = rospy.Rate(self.rate)
# Initialize subscribers
rospy.Subscriber('/clock', Clock, self.clock_cb)
rospy.Subscriber('/morus/odometry', Odometry, self.pose_cb)
rospy.Subscriber('/morus/pos_ref', Vector3, self.pos_ref_cb)
self.euler_pub = rospy.Publisher('/morus/euler_ref',
Vector3,
queue_size=1)
def run(self):
while not self.start_flag1 and not self.start_flag2:
print 'Waiting for the first measurement / reference'
rospy.sleep(0.5)
print 'Starting position control.'
# Starting reference
self.x_ref = 0
self.y_ref = 0
clock_old = self.clock
while not rospy.is_shutdown():
self.ros_rate.sleep()
# Calculate dt
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk < 10e-10:
dt_clk = 0.05
# Calculate new roll value
vx_sp = self.pid_x.compute(self.x_ref, self.x_mv, dt_clk)
roll = self.pid_vx.compute(vx_sp, self.vx_mv, dt_clk)
# Calculate new pitch value
vy_sp = self.pid_y.compute(self.y_ref, self.y_mv, dt_clk)
pitch = self.pid_vy.compute(vy_sp, self.vy_mv, dt_clk)
print ''
print dt_clk
print 'x_ref: ', self.x_ref, ' y_ref: ', self.y_ref
print 'vx_sp: ', vx_sp, ' vy_sp: ', vy_sp
print 'vx_mv: ', self.vx_mv, ' vy_mv: ', self.vy_mv
print 'roll: ', roll, ' pitch: ', pitch
newMsg = Vector3()
newMsg.x = -pitch
newMsg.y = roll
newMsg.z = 0
self.euler_pub.publish(newMsg)
def clock_cb(self, msg):
self.clock = msg
def pose_cb(self, msg):
self.start_flag1 = True
self.x_mv = msg.pose.pose.position.x
self.y_mv = msg.pose.pose.position.y
self.vx_mv = msg.twist.twist.linear.x
self.vy_mv = msg.twist.twist.linear.y
def pos_ref_cb(self, msg):
self.start_flag2 = True
self.x_ref = msg.x
self.y_ref = msg.y
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
