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):