From ee7ec838698764408d9329b5af255f41622a5e92 Mon Sep 17 00:00:00 2001 From: "cesar.alejnadro" Date: Wed, 13 Apr 2022 14:41:13 -0300 Subject: [PATCH] Controller now uses MAVROS for localization --- launch/klausen_dampen.launch | 15 ++--- src/LinkState.py | 82 +++++++++--------------- src/klausen_control.py | 118 +++++++++++++++++------------------ src/path_follow.cpp | 2 +- src/ref_signalGen.py | 110 +++++++++++++++----------------- 5 files changed, 149 insertions(+), 178 deletions(-) diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index d62e5bc..a7271a5 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -25,13 +25,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="LinkState.py" name="linkStates_node" - launch-prefix="xterm -fa 'Monospace' -fs 18 -e" + launch-prefix="xterm -e" /> - + + + diff --git a/src/LinkState.py b/src/LinkState.py index 4c4adf3..3a49cbf 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -19,7 +19,6 @@ class Main: # rate(s) rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz - self.dt = 1.0/rate rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru @@ -41,21 +40,18 @@ class Main: # Will be set to true when test should start self.bool = False - # Vehicle is spawned with yaw offset for convenience, need to deal with that - self.yaw_offset = 0.0 - # variables for message gen - self.status = tethered_status() - self.drone_id = 'spiri_with_tether::spiri::base' - self.pload_id = 'spiri_with_tether::mass::payload' + self.status = tethered_status() + self.drone_id = 'spiri_with_tether::spiri::base' + self.pload_id = 'spiri_with_tether::mass::payload' self.loadAngles = LoadAngles() # initialize variables - self.tetherL = 0.0 # Tether length - self.has_run = 0 # Boolean to keep track of first run instance - self.phibuf = 0.0 # Need buffers to determine their rates + self.tetherL = 0.0 # Tether length + self.has_run = 0 # Boolean to keep track of first run instance + self.phibuf = 0.0 # Need buffers to determine their rates self.thetabuf = 0.0 # - self.pload = True # Check if payload exists + self.pload = True # Check if payload exists # Max dot values to prevent 'blowup' self.phidot_max = 3.0 @@ -74,6 +70,7 @@ class Main: self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state) self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow) + self.gui_timer = rospy.Timer(rospy.Duration(1/10.0), self.user_feedback) def cutoff(self,value,ceiling): """ @@ -102,9 +99,9 @@ class Main: eul[2] = Yaw """ eul = euler_from_quaternion([orientation.x, - orientation.y, - orientation.z, - orientation.w]) + orientation.y, + orientation.z, + orientation.w]) return eul # Get link states (drone and pload) and determine angle between them @@ -123,7 +120,7 @@ class Main: drone_P = get_P(self.drone_id,reference) # Get orientation of drone in euler angles to determine yaw offset - drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) + self.drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) # Check if payload is part of simulation if not drone_P.success: @@ -136,7 +133,7 @@ class Main: if not self.has_run == 1: if self.pload == True: # Determine yaw offset - self.yaw_offset = drone_Eul[2] + self.yaw_offset = self.drone_Eul[2] # Get tether length based off initial displacement @@ -155,25 +152,9 @@ class Main: # Need to detemine their location to get angle of deflection # Drone - drone_Px = drone_P.link_state.pose.position.x - drone_Py = drone_P.link_state.pose.position.y - drone_Pz = drone_P.link_state.pose.position.z - - # Get drone orientation - #drone_q = [drone_P.link_state.pose.orientation.x,drone_P.link_state.pose.orientation.y, - # drone_P.link_state.pose.orientation.z,drone_P.link_state.pose.orientation.w] - - # offset orientation by yaw offset - #q_offset = quaternion_from_euler(0,0,-self.yaw_offset) - #drone_q = quaternion_multiply(drone_q,q_offset) - - #drone_P.link_state.pose.orientation.x = drone_q[0] - #drone_P.link_state.pose.orientation.y = drone_q[1] - #drone_P.link_state.pose.orientation.z = drone_q[2] - #drone_P.link_state.pose.orientation.w = drone_q[3] - - # Get euler angles again for feedback to user - #drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) + self.drone_Px = drone_P.link_state.pose.position.x + self.drone_Py = drone_P.link_state.pose.position.y + self.drone_Pz = drone_P.link_state.pose.position.z if self.pload == True: # If there is payload, determine the variables # Pload @@ -181,7 +162,7 @@ class Main: pload_Py = pload_P.link_state.pose.position.y # Determine theta (pitch) - x_sep = pload_Px - drone_Px + x_sep = pload_Px - self.drone_Px if math.fabs(x_sep) >= self.tetherL or x_sep == 0: self.loadAngles.theta = 0 @@ -194,12 +175,12 @@ class Main: self.thetabuf = self.loadAngles.theta # Determine phi (roll) - y_sep = pload_Py - drone_Py + y_sep = pload_Py - self.drone_Py if math.fabs(y_sep) >= self.tetherL or y_sep == 0: self.loadAngles.phi = 0 else: - self.loadAngles.phi = math.asin(y_sep/self.tetherL) + self.loadAngles.phi = -math.asin(y_sep/self.tetherL) # Determine phidot self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt @@ -209,16 +190,6 @@ class Main: else: # Otherwise, vars = 0 x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0 - # Print and save results - print "\n" - rospy.loginfo("") - print"Roll: "+str(round(drone_Eul[0]*180/3.14,2)),"\nPitch: "+str(round(drone_Eul[1]*180/3.14,2)),"\nYaw: "+str(round(drone_Eul[2]*180/3.14,2)) - print "drone pos.x: " + str(round(drone_Px,2)) - print "drone pos.y: " + str(round(drone_Py,2)) - print "drone pos.z: " + str(round(drone_Pz,2)) - print "phi: " + str(round(self.loadAngles.phi*180/3.14,3)) - print "theta: " + str(round(self.loadAngles.theta*180/3.14,3)) - # Populate message self.status.header.stamp = rospy.Time.now() self.status.drone_pos = drone_P.link_state.pose @@ -232,6 +203,17 @@ class Main: except rospy.ServiceException as e: rospy.loginfo("Get Link State call failed: {0}".format(e)) + def user_feedback(self,gui_timer): + # Print and save results + print "\n" + rospy.loginfo("") + print"Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)),"\nPitch: "+str(round(self.drone_Eul[1]*180/3.14,2)),"\nYaw: "+str(round(self.drone_Eul[2]*180/3.14,2)) + print "drone pos.x: " + str(round(self.drone_Px,2)) + print "drone pos.y: " + str(round(self.drone_Py,2)) + print "drone pos.z: " + str(round(self.drone_Pz,2)) + print "phi: " + str(round(self.loadAngles.phi*180/3.14,3)) + print "theta: " + str(round(self.loadAngles.theta*180/3.14,3)) + def path_follow(self,path_timer): now = rospy.get_time() if now - self.tstart < self.wait: @@ -240,7 +222,6 @@ class Main: self.bool = True self.pub_wd.publish(self.bool) - if __name__=="__main__": # Initiate ROS node @@ -250,5 +231,4 @@ if __name__=="__main__": rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: - pass - + pass \ No newline at end of file diff --git a/src/klausen_control.py b/src/klausen_control.py index 563d8f9..769d695 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -12,44 +12,44 @@ import numpy as np import time import math -from tf.transformations import * -from scipy.integrate import odeint -from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles -from controller_msgs.msg import FlatTarget -from geometry_msgs.msg import Point, Pose -from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion -from gazebo_msgs.srv import GetLinkState -from sensor_msgs.msg import Imu -from pymavlink import mavutil +from tf.transformations import * +from scipy.integrate import odeint +from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles +from controller_msgs.msg import FlatTarget +from geometry_msgs.msg import Point, Pose +from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion +from gazebo_msgs.srv import GetLinkState +from sensor_msgs.msg import Imu +from pymavlink import mavutil class Main: def __init__(self): # rate(s) - rate = 200 # rate for the publisher method, specified in Hz -- 20 Hz + rate = 200 # rate for the publisher method, specified in Hz -- 20 Hz # initialize variables # time variables - self.dt = 1.0/rate - self.tmax = self.dt - self.n = self.tmax/self.dt + 1 - self.t = np.linspace(0, self.tmax, self.n) # Time array - self.tstart = rospy.get_time() # Keep track of the start time + self.dt = 1.0/rate + self.tmax = self.dt + self.n = self.tmax/self.dt + 1 + self.t = np.linspace(0, self.tmax, self.n) # Time array + 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() # Msgs types - self.vel_data = TwistStamped() # This is needed to get drone vel from gps - self.imu_data = Imu() # Needed for to get drone acc from IMU - self.path_pos = np.zeros([3,1]) - self.path_vel = np.zeros([3,1]) - self.path_acc = np.zeros([3,1]) - self.dr_pos = Pose() - self.quaternion = PoseStamped() + self.vel_data = TwistStamped() # This is needed to get drone vel from gps + self.imu_data = Imu() # Needed for to get drone acc from IMU + self.path_pos = np.zeros([3,1]) + self.path_vel = np.zeros([3,1]) + self.path_acc = np.zeros([3,1]) + self.dr_pos = Pose() + self.quaternion = PoseStamped() # used to send quaternion attitude commands self.load_angles = LoadAngles() - self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q + self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q # Drone var self.has_run = 0 # Bool to keep track of first run instance @@ -78,12 +78,12 @@ class Main: self.K2 = np.identity(5) # Values which require updates. *_p = prior - self.z1_p = np.zeros([3,1]) - self.a45_0 = np.zeros(2) - self.alpha = np.zeros([5,1]) + self.z1_p = np.zeros([3,1]) + self.a45_0 = np.zeros(2) + self.alpha = np.zeros([5,1]) self.alphadot = np.zeros([5,1]) - self.a45 = self.alpha[3:5] - self.a45dot = np.array([[0],[0]]) + self.a45 = self.alpha[3:5] + self.a45dot = np.array([[0],[0]]) # Error terms self.z1 = np.zeros([3,1]) # dr_pos - ref_sig_pos @@ -91,10 +91,10 @@ class Main: # Constants self.drone_m = 1.437 - self.pl_m = 0.5 - self.tot_m = self.drone_m + self.pl_m - self.tune = 1 # Tuning parameter - self.dist = np.array([0,0,0,0.01,0.01]) # Wind disturbance + self.pl_m = 0.5 + self.tot_m = self.drone_m + self.pl_m + self.tune = 1 # Tuning parameter + self.dist = np.array([0,0,0,0.01,0.01]) # Wind disturbance # --------------------------------------------------------------------------------# # SUBSCRIBERS @@ -102,6 +102,8 @@ class Main: # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb) + #rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb) + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) @@ -133,9 +135,10 @@ class Main: # Callback drone pose - def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub + def dronePos_cb(self,msg): try: self.dr_pos = msg.pose + #self.dr_pos = msg.drone_pos except ValueError: pass @@ -143,7 +146,7 @@ class Main: # Callback for drone velocity ####### IS THIS ON? ########## def droneVel_cb(self,msg): try: - self.dr_vel = np.array([[msg.twist.linear.x],[msg.twist.linear.y],[msg.twist.linear.z]]) + self.dr_vel = np.array([[msg.twist.linear.x],[msg.twist.linear.y],[msg.twist.linear.z]]) self.dr_angVel = np.array([[msg.twist.angular.x],[msg.twist.angular.y],[msg.twist.angular.z]]) except ValueError or TypeError: @@ -162,7 +165,7 @@ class Main: try: self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]]) self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]]) - self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO + self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO except ValueError: pass @@ -190,15 +193,15 @@ class Main: def control(self): # Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi] - p = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z]]) - g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.load_angles.theta],[self.load_angles.phi]]) + p = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z]]) + g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.load_angles.theta],[self.load_angles.phi]]) # State some variables for shorthand - L = self.tetherL + L = self.tetherL c_theta = math.cos(self.load_angles.theta) c_phi = math.cos(self.load_angles.phi) s_theta = math.sin(self.load_angles.theta) - s_phi = math.sin(self.load_angles.phi) + s_phi = math.sin(self.load_angles.phi) # Check for tether if L <= 0.01: @@ -211,24 +214,16 @@ class Main: # Control matrices - this may be better in _init_ M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta], - - [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta], - - [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta], - - [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.01*s_theta**2, 0], - - [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]] + [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta], + [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta], + [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.01*s_theta**2, 0], + [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]] C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta], - - [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta + self.load_angles.thetadot*c_theta*s_phi)], - - [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)], - - [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)], - - [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]] + [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta + self.load_angles.thetadot*c_theta*s_phi)], + [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)], + [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)], + [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]] G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]] @@ -271,7 +266,7 @@ class Main: self.a45dot = np.array([[self.a45dot[0]],[self.a45dot[1]]]) else: # if no tether, alpha_4:5 = 0 - self.a45 = np.array([[0],[0]]) + self.a45 = np.array([[0],[0]]) self.a45dot = np.array([[0],[0]]) # Determine a_1:3 @@ -290,20 +285,21 @@ class Main: Ki = self.tune*self.K1 # Desired body-oriented forces + # shouldnt it be tot_m*path_acc? Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,self.dt*(self.z1 - self.z1_p)) - + # Update self.z1_p for "integration" self.z1_p = self.z1 # Covert Fd into drone frame - dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] + dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] dr_orientation_inv = quaternion_inverse(dr_orientation) - + Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0],dr_orientation_inv)) # Real part of Fd needs = 0 # Convert forces to attiude *EulerAng[2] = yaw = 0 - self.EulerAng[1] = math.atan(-Fd_tf[0]/(self.drone_m*9.81)) # Pitch -- maybe - self.EulerAng[0] = math.atan(Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll -- maybe + self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch + self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll # Convert Euler angles to quaternions q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) diff --git a/src/path_follow.cpp b/src/path_follow.cpp index 5f04a7a..49078d2 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -229,7 +229,7 @@ int main(int argc, char **argv) attitude.thrust = thrust.thrust; // Determine which messages to send - if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){ + if(ros::Time::now() - tkoff_req > ros::Duration(30.0) && takeoff){ attitude.orientation = q_des; attitude.header.stamp = ros::Time::now(); att_targ.publish(attitude); diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index d04467b..6235967 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -32,8 +32,6 @@ class Main: self.tstart = rospy.get_time() self.dt = 1.0/rate -# self.dt = 0.5 - # self.tmax = 100 self.tmax = self.dt self.n = self.tmax/self.dt + 1 self.t = np.linspace(0, self.tmax, self.n) # Time array @@ -68,12 +66,13 @@ class Main: # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) + #rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/state', State, self.state_cb) # --------------------------------------------------------------------------------# -# PUBLISHERS +# PUBLISHERS # --------------------------------------------------------------------------------# # Publish desired path to compute attitudes self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10) @@ -84,7 +83,7 @@ class Main: self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) # --------------------------------------------------------------------------------# -# FEEDBACK AND INPUT SHAPING +# FEEDBACK AND INPUT SHAPING # --------------------------------------------------------------------------------# # Smooth path variables @@ -92,36 +91,29 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 3.13 # 3.13 works well? ######################################################################### + self.w_tune = 3.5 # 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: if self.tetherL == 0.0: self.wn = self.w_tune else: - self.wn = math.sqrt(9.81/self.tetherL) + self.wn = math.sqrt(9.81/self.tetherL) - self.wd = self.wn*math.sqrt(1 - self.epsilon**2) - self.k4 = 4*self.epsilon*self.w_tune - self.k3 = ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4 - self.k2 = (4*self.epsilon*self.w_tune**3)/(self.k4*self.k3) - self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4) + self.wd = self.wn*math.sqrt(1 - self.epsilon**2) + self.k4 = 4*self.epsilon*self.w_tune + self.k3 = ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4 + self.k2 = (4*self.epsilon*self.w_tune**3)/(self.k4*self.k3) + self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4) # For saturation: - self.jmax = [3, 3, 1] - self.amax = [1.5, 1.5, 1] - self.vmax = [3, 3, 1] - self.max = [0, 3, 1.5, 3] #[0, 3, 1.5, 3] + self.jmax = [3, 3, 1] + self.amax = [1.5, 1.5, 1] + self.vmax = [3, 3, 1] + self.max = [0, 3, 1.5, 3] #[0, 3, 1.5, 3] # create the array: [vmax; amax; jmax] self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T - - # Desired position array - #if rospy.has_param('sim/waypoints'): - # self.xd = rospy.get_param('sim/waypoints') # waypoints - #elif rospy.get_time() - self.tstart >= 3.0: - # self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints - - #self.xd = Point() + self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.empty_point = Point() # Needed to query waypoint_server @@ -137,11 +129,11 @@ class Main: self.td = 2*self.Gd*math.pi/self.wd # Constants for shape filter/ Input shaping - self.t1 = 0 - self.t2 = math.pi/self.wd - self.K = math.exp(-self.epsilon*math.pi/math.sqrt(1 - self.epsilon**2)) - self.A1 = 1/(1 + self.K) - self.A2 = self.A1*self.K + self.t1 = 0 + self.t2 = math.pi/self.wd + self.K = math.exp(-self.epsilon*math.pi/math.sqrt(1 - self.epsilon**2)) + self.A1 = 1/(1 + self.K) + self.A2 = self.A1*self.K # Need to determine how large of any array needs to be stored to use delayed functions self.SF_delay_x = np.zeros([4,int(round(self.t2/self.dt))]) # Shapefilter delay; 4 - p,v,a,j @@ -160,11 +152,11 @@ class Main: self.FB_idx = 0 # Constants for sigmoid - self.at = 3 # acceleration theshold - self.pc = 0.7 # From Klausen 2017 - self.sk = len(self.SF_delay_x[0]) # from Klausen 2017 - self.ak = np.zeros(len(self.SF_delay_x[0])) - self.s_gain = 0.0 # Gain found from sigmoid + self.at = 3 # acceleration theshold + self.pc = 0.7 # From Klausen 2017 + self.sk = len(self.SF_delay_x[0]) # from Klausen 2017 + self.ak = np.zeros(len(self.SF_delay_x[0])) + self.s_gain = 0.0 # Gain found from sigmoid # --------------------------------------------------------------------------------# # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# @@ -198,9 +190,10 @@ class Main: pass # Callback drone pose - def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub + def dronePos_cb(self,msg): try: self.dr_pos = msg.pose + #self.dr_pos = msg.drone_pos except ValueError: pass @@ -229,17 +222,17 @@ class Main: except ValueError: pass -################################################################# -#TODO Will need to add a function that gets a message from # -# a node which lets refsignal_gen.py know there has been a # -# change in xd and therefore runs waypoints_srv_cb again # -################################################################# +###################################################################### +#TODO Will need to add a function that gets a message from # +# a node which lets refsignal_gen.py know there has been a # +# change in xd and therefore runs waypoints_srv_cb again # +###################################################################### # --------------------------------------------------------------------------------# -# ALGORITHM +# ALGORITHM # --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------# -# TRAJECTORY GENERATION BASED ON WAYPONTS xd +# TRAJECTORY GENERATION BASED ON WAYPONTS xd # --------------------------------------------------------------------------------# def statespace(self,y,t,xd): # Update initial conditions # @@ -290,7 +283,7 @@ class Main: elif cmd == 2: # Feedback if self.FB_idx < len(self.theta_fb): # First, fill up the delay array - self.theta_fb[self.FB_idx] = self.load_angles.theta + self.theta_fb[self.FB_idx] = self.load_angles.theta self.theta_vel_fb[self.FB_idx] = self.load_angles.thetadot self.theta_acc_fb[self.FB_idx] = self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1] @@ -300,19 +293,19 @@ class Main: else: # once array is filled, need to shift values w/ latest value at end - self.theta_fb[:] = np.roll(self.theta_fb[:],-1) + self.theta_fb[:] = np.roll(self.theta_fb[:],-1) self.theta_vel_fb[:] = np.roll(self.theta_vel_fb[:],-1) self.theta_acc_fb[:] = np.roll(self.theta_acc_fb[:],-1) - self.phi_fb[:] = np.roll(self.phi_fb[:],-1) + self.phi_fb[:] = np.roll(self.phi_fb[:],-1) self.phi_vel_fb[:] = np.roll(self.phi_vel_fb[:],-1) self.phi_acc_fb[:] = np.roll(self.phi_acc_fb[:],-1) - self.theta_fb[len(self.theta_fb)-1] = self.load_angles.theta # change final value + self.theta_fb[len(self.theta_fb)-1] = self.load_angles.theta # change final value self.theta_vel_fb[len(self.theta_fb)-1] = self.load_angles.thetadot self.theta_acc_fb[len(self.theta_fb)-1] = self.load_angles.thetadot - self.theta_vel_fb[len(self.theta_fb)-1] - self.phi_fb[len(self.phi_fb)-1] = self.load_angles.phi + self.phi_fb[len(self.phi_fb)-1] = self.load_angles.phi self.phi_vel_fb[len(self.theta_fb)-1] = self.load_angles.phidot self.phi_acc_fb[len(self.theta_fb)-1] = self.load_angles.phidot - self.phi_vel_fb[len(self.theta_fb)-1] @@ -343,17 +336,17 @@ class Main: self.delay(j,shapeFil) # Determine the delay (shapefilter) array if self.SF_idx < len(self.SF_delay_x[0]): - self.EPS_I[3*j] = self.x[1,j] + self.EPS_I[3*j] = self.x[1,j] self.EPS_I[3*j+1] = self.y[1,j] self.EPS_I[3*j+2] = self.z[1,j] else: self.EPS_I[3*j] = self.A1*self.x[1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) self.EPS_I[3*j+1] = self.A1*self.y[1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) - self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim + self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim self.delay(1,feedBack) # Detemine feedback array - self.sigmoid() # Determine sigmoid gain + self.sigmoid() # Determine sigmoid gain EPS_D = self.fback() # Feedback Epsilon for i in range(9): @@ -365,7 +358,8 @@ class Main: self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin self.ref_sig.position.x = self.EPS_F[0] self.ref_sig.position.y = self.EPS_F[1] - self.ref_sig.position.z = self.EPS_F[2] + #self.ref_sig.position.z = self.EPS_F[2] + self.ref_sig.position.z = 5.0 self.ref_sig.velocity.x = self.EPS_F[3] self.ref_sig.velocity.y = self.EPS_F[4] self.ref_sig.velocity.z = self.EPS_F[5] @@ -383,11 +377,11 @@ class Main: def fback(self): - xr = self.Gd*self.tetherL*math.sin(self.theta_fb[0]) + xr = self.Gd*self.tetherL*math.sin(self.theta_fb[0]) xdotr = self.Gd*self.tetherL*math.cos(self.theta_fb[0])*self.theta_vel_fb[0] xddotr = -self.Gd*self.tetherL*math.sin(self.theta_fb[0])*(self.theta_vel_fb[0]**2) + math.cos(self.theta_fb[0])*self.theta_acc_fb[0] - yr = -self.Gd*self.tetherL*math.sin(self.phi_fb[0]) + yr = -self.Gd*self.tetherL*math.sin(self.phi_fb[0]) ydotr = -self.Gd*self.tetherL*math.cos(self.phi_fb[0])*self.phi_vel_fb[0] yddotr = self.Gd*self.tetherL*math.sin(self.phi_fb[0])*self.phi_vel_fb[0]**2 + math.cos(self.phi_fb[0])*self.phi_acc_fb[0] @@ -398,13 +392,13 @@ class Main: def screen_output(self): # Feedback to user - #rospy.loginfo(' Var | x | y | z ') - #rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) - #rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) - #rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) - #rospy.loginfo('_______________________') + rospy.loginfo(' Var | x | y | z ') + rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) + rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) + rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) + rospy.loginfo('_______________________') - rospy.loginfo('xd = %.2f',self.xd.x) + #rospy.loginfo('xd = %.2f',self.xd.x) def publisher(self,pub_tim):