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