forked from cesar.alejandro/oscillation_ctrl
174 lines
4.1 KiB
Python
Executable File
174 lines
4.1 KiB
Python
Executable File
#!/usr/bin/env python2.7
|
|
|
|
### Cesar Rodriguez Sept 21
|
|
### Used to control thrust of drone
|
|
|
|
import rospy, tf
|
|
import numpy as np
|
|
|
|
from geometry_msgs.msg import PoseStamped, TwistStamped
|
|
from mavros_msgs.msg import AttitudeTarget
|
|
|
|
class PID:
|
|
def __init__(self):
|
|
# rate(s)
|
|
rate = 35.0
|
|
self.dt = 1/rate
|
|
# Variables needed for testing start
|
|
self.tstart = rospy.get_time() # Keep track of the start time
|
|
while self.tstart == 0.0: # Need to make sure get_rostime works
|
|
self.tstart = rospy.get_time()
|
|
|
|
# tuning gains
|
|
#self.Kp = 0.335
|
|
#self.Kd = 0.1
|
|
self.Kp = 2.7
|
|
self.Kd = 0.5
|
|
|
|
# drone var
|
|
self.drone_m = 1.437
|
|
self.max_a = 14.2
|
|
self.max_t = self.drone_m*self.max_a
|
|
|
|
# message variables
|
|
self.pose = PoseStamped()
|
|
self.pose_buff = PoseStamped()
|
|
self.pose_buff.pose.position.z = 2.5
|
|
|
|
self.attitude = AttitudeTarget()
|
|
self.attitude.type_mask = 1|2|4 # ignore body rate command
|
|
self.attitude.orientation.x = 0.0
|
|
self.attitude.orientation.y = 0.0
|
|
self.attitude.orientation.z = 0.0
|
|
self.attitude.orientation.w = 1.0
|
|
|
|
self.des_alt = 2.5
|
|
self.dr_vel = TwistStamped()
|
|
#self.cur_att = PoseStamped()
|
|
self.R = np.empty([3,3])
|
|
|
|
# clients
|
|
|
|
|
|
# subscribers
|
|
rospy.Subscriber('/mavros/local_position/pose',PoseStamped, self.pose_cb)
|
|
rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.droneVel_cb)
|
|
|
|
# publishers
|
|
self.pub_attitude = rospy.Publisher('/command/thrust', AttitudeTarget, queue_size=10)
|
|
|
|
|
|
# publishing rate
|
|
self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
|
|
|
|
def pose_cb(self,msg):
|
|
self.cur_alt = msg.pose.position.z
|
|
|
|
self.pose = msg
|
|
|
|
# Callback for drone velocity
|
|
def droneVel_cb(self,msg):
|
|
try:
|
|
self.dr_vel = msg
|
|
|
|
except ValueError or TypeError:
|
|
pass
|
|
|
|
|
|
def quaternion_rotation_matrix(self):
|
|
"""
|
|
Covert a quaternion into a full three-dimensional rotation matrix.
|
|
|
|
Input
|
|
:param Q: A 4 element array representing the quaternion (q0,q1,q2,q3)
|
|
|
|
Output
|
|
:return: A 3x3 element matrix representing the full 3D rotation matrix.
|
|
This rotation matrix converts a point in the local reference
|
|
frame to a point in the global reference frame.
|
|
"""
|
|
# Extract the values from Q
|
|
q0 = self.pose.pose.orientation.w
|
|
q1 = self.pose.pose.orientation.x
|
|
q2 = self.pose.pose.orientation.y
|
|
q3 = self.pose.pose.orientation.z
|
|
|
|
# First row of the rotation matrix
|
|
r00 = 2 * (q0 * q0 + q1 * q1) - 1
|
|
r01 = 2 * (q1 * q2 - q0 * q3)
|
|
r02 = 2 * (q1 * q3 + q0 * q2)
|
|
|
|
# Second row of the rotation matrix
|
|
r10 = 2 * (q1 * q2 + q0 * q3)
|
|
r11 = 2 * (q0 * q0 + q2 * q2) - 1
|
|
r12 = 2 * (q2 * q3 - q0 * q1)
|
|
|
|
# Third row of the rotation matrix
|
|
r20 = 2 * (q1 * q3 - q0 * q2)
|
|
r21 = 2 * (q2 * q3 + q0 * q1)
|
|
r22 = 2 * (q0 * q0 + q3 * q3) - 1
|
|
|
|
# 3x3 rotation matrix
|
|
rot_matrix = np.array([[r00, r01, r02],
|
|
[r10, r11, r12],
|
|
[r20, r21, r22]])
|
|
|
|
return rot_matrix
|
|
|
|
def operation(self):
|
|
self.error = self.cur_alt - self.des_alt # error in z dir.
|
|
self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix
|
|
|
|
# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
|
|
# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
|
|
thrust = np.dot(9.81*self.drone_m -self.Kp*self.error -self.Kd*self.dr_vel.twist.linear.z,self.R.T[2])/self.max_t
|
|
self.attitude.thrust = thrust[2] # save thurst in z-dir
|
|
|
|
# Value needs to be between 0 - 1.0
|
|
if self.attitude.thrust >= 1.0:
|
|
self.attitude.thrust = 1.0
|
|
elif self.attitude.thrust <= 0.0:
|
|
self.attitude.thrust = 0.0
|
|
|
|
|
|
def publisher(self,pub_time):
|
|
self.operation() # determine thrust
|
|
self.attitude.header.stamp = rospy.Time.now()
|
|
self.pub_attitude.publish(self.attitude)
|
|
|
|
def user_feedback(self):
|
|
rospy.loginfo('Des: %.2f',self.des_alt)
|
|
rospy.loginfo('Error: %.2f',self.error)
|
|
rospy.loginfo('Throttle: %f\n',self.attitude.thrust)
|
|
|
|
if __name__=="__main__":
|
|
|
|
# Initiate ROS node
|
|
rospy.init_node('thrust_PID')
|
|
try:
|
|
PID() # create class object
|
|
|
|
rospy.spin() # loop until shutdown signal
|
|
|
|
except rospy.ROSInterruptException:
|
|
pass
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|