forked from cesar.alejandro/oscillation_ctrl
Updated alititude controller
This commit is contained in:
parent
f39d7c993d
commit
5a6c18d927
|
|
@ -1,8 +1,18 @@
|
|||
config/mocap*
|
||||
*.rviz
|
||||
setup.txt
|
||||
px4_setup/protobuf_install.txt
|
||||
config/mocapGazebo_params.yaml
|
||||
config/mocapLab_params.yaml
|
||||
launch/cortex_bridge.launch
|
||||
launch/development_*
|
||||
launch/esp_*
|
||||
launch/headless_spiri_mocap.launch
|
||||
launch/headless_spiri_with_tether_mocap.launch
|
||||
launch/mocap_*
|
||||
launch/replay.launch
|
||||
msg/Marker.msg
|
||||
msg/Markers.msg
|
||||
srv/BodyToWorld.srv
|
||||
src/development_*
|
||||
src/killswitch_client.py
|
||||
src/land_client.py
|
||||
|
|
@ -11,9 +21,6 @@ src/Mocap_*.py
|
|||
src/mocap_*
|
||||
src/segmented_tether.py
|
||||
src/segmented_tether_fast.py
|
||||
msg/Marker.msg
|
||||
msg/Markers.msg
|
||||
*.rviz
|
||||
setup.txt
|
||||
px4_setup/protobuf_install.txt
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ add_message_files(
|
|||
add_service_files(
|
||||
FILES
|
||||
WaypointTrack.srv
|
||||
BodyToWorld.srv
|
||||
)
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
|
|
|
|||
|
|
@ -1,14 +1,15 @@
|
|||
# Ros param when using Klausen Ctrl
|
||||
wait_time: 30 # parameter which can be set to run desired tests at a desired time
|
||||
wait_time: 15 # parameter which can be set to run desired tests at a desired time
|
||||
#drone_mass: 0.614 # weight with new battery
|
||||
drone_mass: 0.602
|
||||
pload_mass: 0.1 # mass of payload. Needs to be changed in spiri_with_tether file as well
|
||||
|
||||
pload: true
|
||||
pload_mass: 0.2 # mass of payload.
|
||||
#pload_mass: 0.15 # Pload mass with 100g weight
|
||||
pload_mass: 0.10 # Pload mass with 50g weight
|
||||
#pload_mass: 0.05 # Pload mass with just basket
|
||||
#pload_mass: 0.25
|
||||
use_ctrl: false # starts PX4 without attitude controller
|
||||
waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints
|
||||
change_mode: true # choose whether to switch to oscillation damping controller
|
||||
waypoints: {x: 0.0, y: 0.0, z: 2.0} # takeoff waypoints
|
||||
# sets waypoints to run a square test
|
||||
square_x: [0.5,1,1,1,0.5,0,0]
|
||||
square_y: [0,0,0.5,1,1,1,0.5]
|
||||
|
|
|
|||
|
|
@ -6,15 +6,18 @@ wait_time: 30 # parameter which can be set to run desired tests at a desired tim
|
|||
drone_mass: 0.602
|
||||
|
||||
#PLOAD MASSES
|
||||
#pload_mass: 0.15 # Pload mass with 100g weight
|
||||
pload_mass: 0.10 # Pload mass with 50g weight
|
||||
#pload_mass: 0.05 # Pload mass with just basket
|
||||
#pload_mass: 0.25
|
||||
#pload_mass: 0.15 # Pload mass with 100g weight
|
||||
#pload_mass: 0.10 # Pload mass with 50g weight
|
||||
#pload_mass: 0.05 # Pload mass with just basket
|
||||
pload_mass: 0.01 # No Pload
|
||||
pload: false
|
||||
|
||||
# CTRL PARAMETER - should be false to start always
|
||||
use_ctrl: false # starts PX4 without attitude controller
|
||||
# use_ctrl: false # starts PX4 without attitude controller
|
||||
change_mode: false # choose whether to switch to oscillation damping controller
|
||||
|
||||
waypoints: {x: 0.0, y: 0.0, z: 1.75} # takeoff waypoints
|
||||
waypoints: {x: -1.0, y: -1.0, z: 2.0} # takeoff waypoints
|
||||
|
||||
# sets waypoints to run a square test
|
||||
square_x: [0.5,1,1,1,0.5,0,0]
|
||||
|
|
@ -22,6 +25,6 @@ square_y: [0,0,0.5,1,1,1,0.5]
|
|||
|
||||
# HOVER THROTTLE - Changes depending on mass of pload and drone
|
||||
# hover_throttle: 0.32 # Hover throttle with pload 0.15 kg
|
||||
hover_throttle: 0.28 # Hover throttle with pload 0.10 kg
|
||||
# hover_throttle: 0.22 # Hover throttle with no pload
|
||||
#hover_throttle: 0.28 # Hover throttle with pload 0.10 kg
|
||||
hover_throttle: 0.23 # Hover throttle with no pload - about 0.23 with full battery
|
||||
#hover_throttle: 0.26 # Hover throttle with pload 0.05 kg
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
wait_time: 30 # parameter which can be set to run desired tests at a desired time
|
||||
drone_mass: 1.437 # mass of drone
|
||||
pload_mass: 0.25 # mass of payload
|
||||
pload: false
|
||||
use_ctrl: false # starts PX4 without attitude controller - needs to be set to false to takeoff
|
||||
waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints
|
||||
# sets waypoints to run a square test
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
pkg="oscillation_ctrl"
|
||||
type="klausen_control.py"
|
||||
name="klausenCtrl_node"
|
||||
clear_params="true"
|
||||
/>
|
||||
<!-- PUBLISHES DESIRED COMMANDS -->
|
||||
<node
|
||||
|
|
|
|||
|
|
@ -20,30 +20,89 @@ from oscillation_ctrl.srv import WaypointTrack
|
|||
from geometry_msgs.msg import Pose, Point, TwistStamped, PoseStamped
|
||||
from sensor_msgs.msg import Imu
|
||||
from mavros_msgs.msg import AttitudeTarget
|
||||
from collections import deque
|
||||
|
||||
class DesiredPoint():
|
||||
class DesiredPoint(Point):
|
||||
def __init__(self,x,y,z):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
|
||||
class LiveFilter:
|
||||
"""Base class for live filters.
|
||||
"""
|
||||
def process(self, x):
|
||||
# do not process NaNs
|
||||
# if numpy.isnan(x):
|
||||
# return x
|
||||
|
||||
return self._process(x)
|
||||
|
||||
def __call__(self, x):
|
||||
return self.process(x)
|
||||
|
||||
def _process(self, x):
|
||||
raise NotImplementedError("Derived class must implement _process")
|
||||
|
||||
class LiveLFilter(LiveFilter):
|
||||
def __init__(self, b, a):
|
||||
"""Initialize live filter based on difference equation.
|
||||
|
||||
Args:
|
||||
b (array-like): numerator coefficients obtained from scipy.
|
||||
a (array-like): denominator coefficients obtained from scipy.
|
||||
n (int): How many dimensions are we dealing with?
|
||||
"""
|
||||
self.b = b
|
||||
self.a = a
|
||||
n = len(b)
|
||||
# 3 because x,y,z
|
||||
# self.n because that is how many terms we need
|
||||
self._xs = deque(np.zeros(n), maxlen = n) # unfiltered data
|
||||
self._ys = deque(np.zeros(n - 1), maxlen = n - 1) # filtered signal
|
||||
|
||||
def _process(self, x):
|
||||
"""
|
||||
Filter incoming data with standard difference equations*:
|
||||
a_0*y[n] = b_0*x[n] + b_1*x[n-1] + b_2*x[n-2] - a_1*y[n-1] - a_2*y[n-2]
|
||||
*Changes depending on order of filter
|
||||
"""
|
||||
# y = numpy.empty(3)
|
||||
# self._xs.appendleft(x)
|
||||
# # get every x val (idx = 0), then y val (idx=1), then z val (idx=2) to determine y
|
||||
# for i in range(3):
|
||||
# last_x_vals = [val_x[i] for val_x in self._xs]
|
||||
# last_y_vals = [val_y[i] for val_y in self._ys]
|
||||
# y[i] = numpy.dot(self.b, last_x_vals) - numpy.dot(self.a[1:], last_y_vals)
|
||||
|
||||
# y = y / self.a[0]
|
||||
# self._ys.appendleft(y)
|
||||
|
||||
# return y
|
||||
|
||||
self._xs.appendleft(x)
|
||||
y = np.dot(self.b, self._xs) - np.dot(self.a[1:], self._ys)
|
||||
y = y / self.a[0]
|
||||
self._ys.appendleft(y)
|
||||
|
||||
return y
|
||||
class Main:
|
||||
|
||||
def __init__(self):
|
||||
|
||||
# rate(s)
|
||||
rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz #25
|
||||
rate = 20 # rate for the publisher method, specified in Hz -- 50 Hz #25
|
||||
|
||||
# initialize variables
|
||||
|
||||
# time variables
|
||||
self.dt = 1.0/rate
|
||||
self.tmax = self.dt
|
||||
self.n = self.tmax/self.dt + 1
|
||||
self.n = self.tmax/self.dt + 9
|
||||
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()
|
||||
self.tstart = rospy.get_time()
|
||||
|
||||
# Msgs types
|
||||
self.vel_data = TwistStamped() # This is needed to get drone vel from gps
|
||||
|
|
@ -55,14 +114,14 @@ class Main:
|
|||
self.att_targ = AttitudeTarget() # 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.EulerPose = [0,0,0]
|
||||
self.EulerPose = [0,0,0]
|
||||
|
||||
# Service var
|
||||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
self.empty_point = Point() # Needed to query waypoint_server
|
||||
|
||||
# Drone var
|
||||
self.has_run = 0 # Bool to keep track of first run instance
|
||||
self.has_run = 0 # Bool to keep track of first run instance
|
||||
|
||||
# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
|
||||
self.PHI = np.array([[0,0],[0,0]])
|
||||
|
|
@ -72,10 +131,14 @@ class Main:
|
|||
|
||||
# Tether var - Check if current method is used
|
||||
# Get tether length
|
||||
self.param_exists = False
|
||||
self.tetherL = self.get_tether()
|
||||
self.tether = True if self.tetherL > 0.01 else False
|
||||
# Check if tether was correctly detected
|
||||
if rospy.get_param('status/pload', False):
|
||||
self.tetherL = self.get_tether()
|
||||
self.tether = True
|
||||
else:
|
||||
self.tetherL = 0.0
|
||||
self.tether = False
|
||||
|
||||
# Check if tether was correctly detected or not
|
||||
self.tether_check()
|
||||
|
||||
# Retrieve drone and payload masses from config file
|
||||
|
|
@ -88,18 +151,19 @@ class Main:
|
|||
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.a45 = np.zeros(2)
|
||||
self.a45dot = np.array([[0],[0]])
|
||||
|
||||
# Error terms
|
||||
self.z1 = np.zeros([3,1]) # dr_pos - ref_sig_pos
|
||||
self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha
|
||||
self.z_sum = np.zeros([3,1])
|
||||
|
||||
# Tuning gains
|
||||
self.K1 = np.identity(3)
|
||||
self.K2 = np.identity(5)
|
||||
self.tune = 0.1 # Tuning parameter
|
||||
self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance
|
||||
self.K1 = np.identity(3)*0.75 # 0.75
|
||||
self.K2 = np.identity(5)*0.35 # 0.35
|
||||
self.tune = 1 # Tuning parameter 1
|
||||
self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance
|
||||
# Gain terms
|
||||
self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3)
|
||||
self.Kd = self.tot_m*self.K1 + self.tune*self.K2[:3,:3]
|
||||
|
|
@ -107,9 +171,12 @@ class Main:
|
|||
|
||||
# PD Thrust Controller terms
|
||||
# gains for thrust PD Controller
|
||||
self.Kp_thrust = 1.5
|
||||
self.Kd_thrust = 1.0
|
||||
### Failed when both gains were set to 1.0
|
||||
##### Works better when Kp_thrust is higher? Try 10
|
||||
self.Kp_thrust = 3.5 # 1.5 for lab # 2.5 for gazebo
|
||||
self.Kd_thrust = 2.5 # 1.5 for lab # 2.5 for gazebo 2.0 if no tether?
|
||||
self.R = np.empty([3,3]) # rotation matrix
|
||||
self.g = np.array([0,0,9.81]).reshape(3,1)
|
||||
self.e3 = np.array([[0],[0],[1]])
|
||||
# Get scaling thrust factor, kf
|
||||
self.kf = self.get_kf()
|
||||
|
|
@ -124,15 +191,16 @@ class Main:
|
|||
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)
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# PUBLISHERS
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
||||
self.pub_att_targ = rospy.Publisher('/command/att_target',AttitudeTarget,queue_size = 10)
|
||||
|
||||
# timer(s), used to control method loop freq(s) as defined by the rate(s)
|
||||
self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) #f this was 5.0 rate before
|
||||
self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) # this was 5.0 rate before
|
||||
|
||||
self.throttle_timer = rospy.Timer(rospy.Duration(2.0/rate), self.determine_throttle) # this was 5.0 rate before
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SETUP PARAM METHODS
|
||||
|
|
@ -142,6 +210,7 @@ class Main:
|
|||
""" Get tether length based on set parameters"""
|
||||
param_exists = False
|
||||
while param_exists == False:
|
||||
rospy.loginfo('Waiting for tether length...')
|
||||
if rospy.has_param('status/tether_length'):
|
||||
tether_length = rospy.get_param('status/tether_length') # Tether length
|
||||
rospy.loginfo('TETHER LENGTH IN CONFG FILE')
|
||||
|
|
@ -167,6 +236,7 @@ class Main:
|
|||
if self.tether:
|
||||
param_exists = False
|
||||
while param_exists == False:
|
||||
rospy.loginfo('Waiting for pload mass...')
|
||||
if rospy.has_param('status/pload_mass'):
|
||||
pl_m = rospy.get_param('status/pload_mass') # wait time
|
||||
rospy.loginfo('PLOAD MASS FOUND')
|
||||
|
|
@ -183,11 +253,23 @@ class Main:
|
|||
|
||||
def get_kf(self):
|
||||
if rospy.has_param('status/hover_throttle'):
|
||||
|
||||
rospy.loginfo('total mass: %.3f',self.tot_m)
|
||||
|
||||
hover_throttle = rospy.get_param('status/hover_throttle')
|
||||
rospy.loginfo('Using hover throttle from config %.3f',hover_throttle)
|
||||
self.max_throttle = 0.5
|
||||
else:
|
||||
hover_throttle = (self.tot_m*9.81 + 11.2)/34.84 # linear scaling depending on dependent on mass
|
||||
|
||||
rospy.loginfo('total mass: %.3f',self.tot_m)
|
||||
|
||||
hover_throttle = (self.tot_m*9.81 + 9.81)/34.84 # linear scaling dependent on mass
|
||||
|
||||
rospy.set_param('status/hover_throttle',hover_throttle)
|
||||
rospy.loginfo('Determined hover throttle to be %.3f',hover_throttle)
|
||||
self.max_throttle = 0.8
|
||||
kf = hover_throttle/(self.tot_m*9.81)
|
||||
rospy.set_param('status/motor_constant',kf)
|
||||
return kf
|
||||
|
||||
# Check if vehicle has tether
|
||||
|
|
@ -207,44 +289,52 @@ class Main:
|
|||
self.load_angles = msg
|
||||
# Populate self.PHI
|
||||
self.PHI = np.array([[self.load_angles.theta,self.load_angles.phi],[self.load_angles.thetadot,self.load_angles.phidot]])
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
# self.PHI = np.array([[0.0,0.0],[0.0,0.0]])
|
||||
|
||||
# print('PHI:\n{0}'.format(self.PHI))
|
||||
# print('Angles:\n{0}'.format(self.PHI[0,:]))
|
||||
# print('Ang_dot:\n{0}'.format(self.PHI[1,:]))
|
||||
|
||||
except ValueError as e:
|
||||
rospy.loginfo('Load angles callback failed due to: {0}'.format(e))
|
||||
|
||||
# Callback drone pose
|
||||
def dronePos_cb(self,msg):
|
||||
""" Subscribed to /mavros/local_position/pose, gets PoseStamped msgs """
|
||||
try:
|
||||
self.dr_pos = msg.pose
|
||||
self.EulerPose = self.convert2eul(self.dr_pos.orientation)
|
||||
# self.dr_pos = msg.drone_pos
|
||||
except ValueError:
|
||||
pass
|
||||
except ValueError as e:
|
||||
rospy.loginfo('Drone pos callback failed due to: {0}'.format(e))
|
||||
|
||||
# Callback for drone velocity ####### IS THIS ON? ##########
|
||||
# Callback for drone velocity
|
||||
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]).reshape(3,1)
|
||||
self.dr_angVel = np.array([[msg.twist.angular.x],[msg.twist.angular.y],[msg.twist.angular.z]])
|
||||
|
||||
except ValueError or TypeError:
|
||||
pass
|
||||
except (ValueError or TypeError) as e:
|
||||
rospy.loginfo('Drone vel callback failed due to: {0}'.format(e))
|
||||
|
||||
# Callback for drone accel from IMU data
|
||||
def droneAcc_cb(self,msg):
|
||||
try:
|
||||
self.dr_acc = np.array([[msg.linear_acceleration.x],[msg.linear_acceleration.y],[msg.linear_acceleration.z]])
|
||||
|
||||
except ValueError:
|
||||
pass
|
||||
except ValueError as e:
|
||||
rospy.loginfo('Drone accel. callback failed due to: {0}'.format(e))
|
||||
|
||||
# Callback reference signal
|
||||
def refsig_cb(self,msg):
|
||||
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]]) #TODO
|
||||
self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]])
|
||||
|
||||
except ValueError:
|
||||
pass
|
||||
except ValueError as e:
|
||||
rospy.loginfo('Reference signal callback failed due to: {0}'.format(e))
|
||||
|
||||
def waypoints_srv_cb(self):
|
||||
if '/status/waypoint_tracker' in self.service_list:
|
||||
|
|
@ -260,12 +350,15 @@ class Main:
|
|||
# ---------------------------------ODE SOLVER-------------------------------------#
|
||||
def statespace(self,y,t,Ka,Kb,Kc):
|
||||
# Need the statespace array:
|
||||
a1,a2 = y
|
||||
K = np.dot(Ka,[[a1],[a2]]) + Kb
|
||||
_y = np.array(y).reshape(2,1)
|
||||
# print('Ka:\n{0}\nKb:\n{1}'.format(Ka, Kb))
|
||||
|
||||
K = np.dot(Ka,_y) + Kb # Kb should be 2x1
|
||||
|
||||
# Derivative of statespace array:
|
||||
dydt = np.dot(Kc,K)
|
||||
dydt = [dydt[0][0], dydt[1][0]] # og dydt is list of arrays, need list of floats
|
||||
dydt = np.dot(Kc,K)
|
||||
# print('dydt:\n{0}'.format(dydt))
|
||||
dydt = [dydt[0][0], dydt[1][0]]
|
||||
|
||||
return dydt
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -314,7 +407,7 @@ class Main:
|
|||
Convers quaternion in pose message into euler angles
|
||||
|
||||
Input
|
||||
:param Q: orientatiom pose message
|
||||
:param Q: orientation pose message
|
||||
|
||||
Output
|
||||
:return: Array of euler angles
|
||||
|
|
@ -340,26 +433,29 @@ class Main:
|
|||
euler = [roll,pitch,yaw]
|
||||
return euler
|
||||
|
||||
def determine_throttle(self):
|
||||
# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
|
||||
# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
|
||||
self.waypoints_srv_cb()
|
||||
self.R = self.quaternion_rotation_matrix()
|
||||
def determine_throttle(self,throttle_timer):
|
||||
""" Thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) Taeyoung Lee, Melvin Leok, and N. Harris McClamroch"""
|
||||
|
||||
self.waypoints_srv_cb() # check for new waypoints
|
||||
self.R = self.quaternion_rotation_matrix() # get rotation matrix
|
||||
b3 = self.R.dot(np.array([0,0,1]).reshape(3,1))
|
||||
|
||||
self.error = np.array([ [self.path_pos[0] - self.dr_pos.position.x],
|
||||
[self.path_pos[1] - self.dr_pos.position.y],
|
||||
[self.path_pos[2] - self.dr_pos.position.z]]).reshape(3,1)
|
||||
|
||||
self.error_vel = self.path_vel - self.R.dot(self.dr_vel)
|
||||
|
||||
self.error_vel = self.path_vel.reshape(3,1) - self.dr_vel # best performance
|
||||
|
||||
# determine Rotation Matrix
|
||||
self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
|
||||
thrust_vector = self.tot_m*self.g + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel + self.tot_m*self.path_acc
|
||||
# thrust_vector = (self.g*self.tot_m + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel) * self.kf
|
||||
|
||||
thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf
|
||||
thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
|
||||
# thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
|
||||
thrust = (thrust_vector[0] * b3[0] + thrust_vector[1] * b3[1] + thrust_vector[2] * b3[2]) * self.kf # best performance
|
||||
|
||||
# Value needs to be between 0 - 1.0
|
||||
self.att_targ.thrust = max(0.0,min(thrust,1.0))
|
||||
|
||||
self.att_targ.thrust = max(0.0,min(thrust,self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
# print('thrust_vector:\n{0}\nthrust:{1}'.format(thrust_vector, self.att_targ.thrust))
|
||||
|
||||
def determine_q(self):
|
||||
""" Determine attitude commands based on feedback and feedforward methods"""
|
||||
|
|
@ -378,13 +474,13 @@ class Main:
|
|||
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],
|
||||
[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.001*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.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.001*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]]
|
||||
|
|
@ -407,60 +503,81 @@ class Main:
|
|||
M_b = M[3:5,:3] # M_4:5,1:3 - rows 4 to 5 and columns 1 to 3
|
||||
M_c = M[:3,3:5] # M_1:3,4:5 - rows 1 to 3 and columns 4 to 5
|
||||
C_c = C[:3,3:5]
|
||||
|
||||
# Constants from Eq. 49
|
||||
Ka = -(D_a + C_a + self.K2[3:5,3:5])
|
||||
Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[:,1]) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel))
|
||||
|
||||
|
||||
# Determine alpha
|
||||
if self.tether:
|
||||
if self.tether:
|
||||
# Constants from Eq. 49
|
||||
Ka = -(D_a + C_a + self.K2[3:5,3:5])
|
||||
Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[1,:].reshape(2,1)) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel))
|
||||
|
||||
#TODO divide by M_a after finding a45_buff
|
||||
|
||||
M_aI = np.linalg.inv(M_a) # Inverse of M_a
|
||||
# print('M_aI was found to be:\n{0}\n'.format(M_aI))
|
||||
|
||||
# SOLVE ODE (get alpha)
|
||||
# Populate buffer
|
||||
self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI))
|
||||
# Populate buffer
|
||||
self.a45_buff = odeint(self.statespace,self.a45,self.t,args=(Ka,Kb,M_aI))
|
||||
# self.a45_buff = odeint(self.statespace,self.a45,self.t,args=(Ka,Kb,np.identity(2))) # spits out a 2,2 matrix
|
||||
# print('a45_0 was found to be:\n{0}\na45 was found to be:\n{1}\n'.format(self.a45_0,self.a45))
|
||||
|
||||
# Update a45_0 to new a45. Need to transpose to column vector
|
||||
self.a45_0 = self.a45_buff[-1,:]
|
||||
self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]])
|
||||
self.a45_0 = self.a45_buff[0,:]
|
||||
self.a45 = self.a45_buff[-1,:]
|
||||
# self.a45 = M_aI.dot(self.a45_buff[-1,:])
|
||||
|
||||
# print('a45_0 was found to be {0}'.format(self.a45_0))
|
||||
|
||||
# Get alphadot_4:5
|
||||
self.a45dot = self.statespace(self.a45_0,1,Ka,Kb,M_aI) # Do not need 't' and that's why it is a 1
|
||||
self.a45dot = np.array([[self.a45dot[0]],[self.a45dot[1]]])
|
||||
self.a45dot = np.array(self.statespace(self.a45,1,Ka,Kb,M_aI)) # Do not need 't' and that's why it is a 1
|
||||
# self.a45dot_buff = self.statespace(self.a45,1,Ka,Kb,np.identity(2)) # Do not need 't' and that's why it is a 1
|
||||
# self.a45dot = M_aI.dot(np.array([[self.a45dot_buff[0]],[self.a45dot_buff[1]]]))
|
||||
|
||||
else: # if no tether, alpha_4:5 = 0
|
||||
self.a45 = np.array([[0],[0]])
|
||||
self.a45dot = np.array([[0],[0]])
|
||||
|
||||
# Determine a_1:3
|
||||
self.alpha[:3] = self.path_vel - np.dot(self.K1,p - self.path_pos)
|
||||
self.alpha[3:5] = self.a45
|
||||
|
||||
# populate error terms
|
||||
self.z1 = p - self.path_pos
|
||||
z1_dot = self.dr_vel - self.path_vel
|
||||
self.z1 = p.reshape(3,1) - self.path_pos.reshape(3,1)
|
||||
# z1_dot = self.dr_vel - self.path_vel
|
||||
z1_dot = self.R.dot(-self.path_vel) + self.dr_vel
|
||||
self.z2 = g - self.alpha
|
||||
|
||||
B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot)
|
||||
# Determine alpha
|
||||
self.alpha[:3] = self.path_vel - np.dot(self.K1,self.z1)
|
||||
self.alpha[3:5] = self.a45.reshape(2,1)
|
||||
|
||||
# determine stability factor
|
||||
B = np.dot(C_c,self.a45.reshape(2,1)) + np.dot(M_c,self.a45dot.reshape(2,1))
|
||||
|
||||
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)
|
||||
|
||||
if rospy.get_param('status/use_ctrl', False):
|
||||
z_int = self.z_sum + self.z1_p * self.dt + 0.5 * (self.z1 - self.z1_p) * self.dt
|
||||
else:
|
||||
z_int = np.zeros(3).reshape(3,1)
|
||||
|
||||
# path_acc = self.R.dot(self.path_acc)
|
||||
path_acc = np.array([self.path_acc[0],self.path_acc[1],self.path_acc[2] + 9.81]).reshape(3,1) # need to add gravity for controller to work
|
||||
|
||||
# Desired body-oriented forces
|
||||
Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p))
|
||||
Fd = B + G[:3] + self.tot_m*path_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,z_int)
|
||||
# rospy.loginfo('\nFd:\n{0}'.format(Fd))
|
||||
|
||||
# Update self.z1_p for integration
|
||||
self.z1_p = self.z1
|
||||
self.z_sum = z_int
|
||||
|
||||
# Covert Fd into drone frame
|
||||
Fd_tf = Fd
|
||||
# Covert Fd into drone frame, do not need Fz since we have a seperate altitude controller
|
||||
# Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0
|
||||
Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],0.0,0.0],dr_orientation_inv)) # Real part of Fd needs = 0
|
||||
|
||||
Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0
|
||||
# rospy.loginfo('\nFd_tf:\n{0}'.format(Fd_tf))
|
||||
|
||||
# Convert forces to attiude *EulerAng[2] = yaw = 0
|
||||
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
|
||||
self.EulerAng[1] = -math.atan(-Fd_tf[0]/(self.tot_m*9.81)) # Pitch
|
||||
self.EulerAng[0] = -math.atan(Fd_tf[1]*math.cos(self.EulerAng[1])/(self.tot_m*9.81)) # Roll
|
||||
|
||||
q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2])
|
||||
|
||||
|
|
@ -470,11 +587,14 @@ class Main:
|
|||
# Attitude control
|
||||
self.att_targ.header.stamp = rospy.Time.now()
|
||||
self.att_targ.header.frame_id = '/map'
|
||||
self.att_targ.type_mask = 1|2|4
|
||||
self.att_targ.type_mask = 1|2|4 # ignore body rate command
|
||||
self.att_targ.orientation.x = q[0]
|
||||
self.att_targ.orientation.y = q[1]
|
||||
self.att_targ.orientation.z = q[2]
|
||||
self.att_targ.orientation.w = q[3]
|
||||
self.att_targ.body_rate.x = 0.0
|
||||
self.att_targ.body_rate.y = 0.0
|
||||
self.att_targ.body_rate.z = 0.0
|
||||
|
||||
def user_feedback(self,F_noTransform, F_Transform):
|
||||
print('\n')
|
||||
|
|
@ -484,8 +604,8 @@ class Main:
|
|||
rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', F_Transform[0],F_Transform[1],F_Transform[2])
|
||||
|
||||
def publisher(self,pub_time):
|
||||
# self.determine_throttle()
|
||||
self.determine_q()
|
||||
self.determine_throttle()
|
||||
self.pub_att_targ.publish(self.att_targ)
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
|
|||
|
|
@ -37,25 +37,11 @@ void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
|
|||
|
||||
// Cb to recieve pose information
|
||||
geometry_msgs::PoseStamped buff_pose;
|
||||
geometry_msgs::Quaternion q_init;
|
||||
geometry_msgs::PoseStamped mavPose;
|
||||
bool gps_read = false;
|
||||
double current_altitude;
|
||||
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
|
||||
mavPose = *msg;
|
||||
current_altitude = mavPose.pose.position.z;
|
||||
// while(gps_read == false){
|
||||
// q_init = mavPose.pose.orientation;
|
||||
// if(q_init.x == 0.0 && q_init.w == 0.0){
|
||||
// ROS_INFO("Waiting...");
|
||||
// } else {
|
||||
// buff_pose.pose.orientation.x = q_init.x;
|
||||
// buff_pose.pose.orientation.y = q_init.y;
|
||||
// buff_pose.pose.orientation.z = q_init.z;
|
||||
// buff_pose.pose.orientation.w = q_init.w;
|
||||
// gps_read = true;
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
|
|
@ -90,8 +76,6 @@ int main(int argc, char **argv)
|
|||
("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
||||
("mavros/set_mode");
|
||||
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
||||
("mavros/cmd/takeoff");
|
||||
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
||||
("status/waypoint_tracker");
|
||||
|
||||
|
|
|
|||
|
|
@ -17,9 +17,14 @@ from sensor_msgs.msg import Imu
|
|||
|
||||
class DesiredPoint():
|
||||
def __init__(self,x,y,z):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
self.point = Point()
|
||||
self.point.x = x
|
||||
self.point.y = y
|
||||
self.point.z = z
|
||||
self.return_xd()
|
||||
|
||||
def return_xd(self):
|
||||
return self.point
|
||||
|
||||
class Main:
|
||||
|
||||
|
|
@ -44,12 +49,15 @@ class Main:
|
|||
self.ref_sig = RefSignal() # Smooth Signal
|
||||
self.load_angles = LoadAngles()
|
||||
|
||||
self.has_run = 0 # Bool to keep track of first run instance
|
||||
self.dr_pos = Pose()
|
||||
self.dr_vel = self.vel_data.twist.linear
|
||||
self.dr_acc = self.imu_data.linear_acceleration
|
||||
|
||||
self.tetherL = self.get_tether()
|
||||
if rospy.get_param('status/pload'):
|
||||
self.tetherL = self.get_tether()
|
||||
else: self.tetherL = 0.0
|
||||
rospy.loginfo('tether length: {0}'.format(self.tetherL))
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SUBSCRIBERS #
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -78,7 +86,7 @@ class Main:
|
|||
self.EPS_I = np.zeros(9) # Epsilon shapefilter
|
||||
|
||||
# Constants for smooth path generation
|
||||
self.w_tune = 1 # Increase this to increase aggresiveness of trajectory i.e. higher accelerations
|
||||
self.w_tune = 1.0 # 0.5 for Gaz. Increase this to increase aggresiveness of trajectory i.e. higher accelerations
|
||||
self.epsilon = 0.7 # Damping ratio
|
||||
|
||||
# need exception if we do not have tether:
|
||||
|
|
@ -86,16 +94,16 @@ class Main:
|
|||
self.wn = self.w_tune
|
||||
else:
|
||||
self.wn = math.sqrt(9.81/self.tetherL)
|
||||
# self.wn = 7
|
||||
# self.wn = 1.51 # 1.01 is the imperically determined nat freq in gazebo
|
||||
|
||||
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.k1 = (self.w_tune**4)/(self.k4*self.k3*self.k2)
|
||||
|
||||
# For saturation:
|
||||
self.max = [0, 3, 1.5, 3] #[0, 5, 1.5, 3]
|
||||
self.max = [0, 5, 2.5, 3] #[0, 3, 1.5, 3] - lab testing
|
||||
|
||||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
self.empty_point = Point() # Needed to query waypoint_server
|
||||
|
|
@ -119,8 +127,8 @@ class Main:
|
|||
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
|
||||
self.SF_delay_y = np.zeros([4,int(round(self.t2/self.dt))])
|
||||
self.SF_delay_x = np.zeros([3,int(round(self.t2/self.dt))]) # Shapefilter delay; 4 - p,v,a since we do not need j
|
||||
self.SF_delay_y = np.zeros([3,int(round(self.t2/self.dt))])
|
||||
|
||||
self.phi_fb = np.zeros(int(round(self.td/self.dt))) # Feedback delay
|
||||
self.phi_vel_fb = np.zeros(int(round(self.td/self.dt)))
|
||||
|
|
@ -196,7 +204,7 @@ class Main:
|
|||
if rospy.has_param('status/tether_length'):
|
||||
tether_length = rospy.get_param('status/tether_length') # Tether length
|
||||
self.param_exists = True
|
||||
elif rospy.get_time() - self.tstart >= 15:
|
||||
elif rospy.get_time() - self.tstart >= 3:
|
||||
tether_length = 0.0
|
||||
break
|
||||
return tether_length
|
||||
|
|
@ -214,11 +222,11 @@ class Main:
|
|||
pos,vel,acc,jer = y
|
||||
|
||||
error = xd - pos
|
||||
if abs(error) <= 0.01: error = 0.0
|
||||
# if abs(error) <= 0.2: error = 0.0 # works well without saturation
|
||||
|
||||
# Derivative of statesape array:
|
||||
dydt = [vel, acc, jer,
|
||||
self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)]
|
||||
dydt = [vel, acc, jer, self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)]
|
||||
|
||||
return dydt
|
||||
|
||||
# Sigmoid
|
||||
|
|
@ -261,11 +269,11 @@ class Main:
|
|||
# First, fill up the delay array
|
||||
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]
|
||||
self.theta_acc_fb[self.FB_idx] = (self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1])/self.dt
|
||||
|
||||
self.phi_fb[self.FB_idx] = self.load_angles.phi
|
||||
self.phi_vel_fb[self.FB_idx] = self.load_angles.phidot
|
||||
self.phi_acc_fb[self.FB_idx] = self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1]
|
||||
self.phi_acc_fb[self.FB_idx] = (self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1])/self.dt
|
||||
|
||||
else:
|
||||
# once array is filled, need to shift values w/ latest value at end
|
||||
|
|
@ -277,13 +285,13 @@ class Main:
|
|||
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_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.theta_fb[-1] = self.load_angles.theta # change final value
|
||||
self.theta_vel_fb[-1] = self.load_angles.thetadot
|
||||
self.theta_acc_fb[-1] = (self.load_angles.thetadot - self.theta_vel_fb[-1])/self.dt
|
||||
|
||||
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]
|
||||
self.phi_fb[-1] = self.load_angles.phi
|
||||
self.phi_vel_fb[-1] = self.load_angles.phidot
|
||||
self.phi_acc_fb[-1] = (self.load_angles.phidot - self.phi_vel_fb[-1])/self.dt
|
||||
|
||||
else:
|
||||
print('No delay')
|
||||
|
|
@ -308,6 +316,7 @@ class Main:
|
|||
self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i])
|
||||
self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i])
|
||||
|
||||
# convolution
|
||||
for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
|
||||
|
||||
self.delay(j,SHAPEFIL) # Determine the delay (shapefilter) array
|
||||
|
|
@ -319,18 +328,18 @@ class Main:
|
|||
|
||||
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+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y)
|
||||
|
||||
|
||||
self.delay(1,FEEDBACK) # Detemine feedback array
|
||||
|
||||
self.sigmoid() # Determine sigmoid gain
|
||||
EPS_D = self.fback() # Feedback Epsilon
|
||||
Eps_D = self.fback() # Feedback Epsilon
|
||||
|
||||
self.EPS_F = self.EPS_I + self.s_gain*EPS_D
|
||||
self.EPS_F = self.EPS_I + self.s_gain*Eps_D
|
||||
|
||||
# Populate msg with epsilon_final
|
||||
self.ref_sig.header.stamp = rospy.Time.now()
|
||||
#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]
|
||||
|
|
@ -341,9 +350,10 @@ class Main:
|
|||
self.ref_sig.acceleration.y = self.EPS_F[7]
|
||||
self.ref_sig.acceleration.z = self.EPS_F[8]
|
||||
|
||||
# update initial states
|
||||
self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]]
|
||||
self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]]
|
||||
self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]]
|
||||
self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]]
|
||||
|
||||
self.SF_idx += 1
|
||||
self.FB_idx += 1
|
||||
|
|
@ -360,9 +370,9 @@ class Main:
|
|||
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]
|
||||
|
||||
EPS_D = np.array([xr,yr,0,xdotr,ydotr,0,xddotr,yddotr,0])
|
||||
Eps_D = np.array([xr,yr,0,xdotr,ydotr,0,xddotr,yddotr,0])
|
||||
|
||||
return EPS_D
|
||||
return Eps_D
|
||||
|
||||
def user_feeback(self):
|
||||
|
||||
|
|
@ -376,17 +386,20 @@ class Main:
|
|||
|
||||
def publisher(self,pub_tim):
|
||||
|
||||
# Determine final ref signal
|
||||
self.convo()
|
||||
# Determine final ref signal
|
||||
try:
|
||||
self.convo()
|
||||
|
||||
# Publish reference signal
|
||||
self.pub_path.publish(self.ref_sig)
|
||||
# Publish reference signal
|
||||
self.pub_path.publish(self.ref_sig)
|
||||
|
||||
# Publish what the setpoints are
|
||||
self.waypointTracker_pub.publish(self.xd)
|
||||
# Publish what the setpoints are
|
||||
self.waypointTracker_pub.publish(self.xd)
|
||||
|
||||
# Give user feedback on published message:
|
||||
self.user_feeback()
|
||||
# Give user feedback on published message:
|
||||
self.user_feeback()
|
||||
except AttributeError:
|
||||
pass # catches case at beginning when self.xd is not properly initialized
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue