forked from cesar.alejandro/oscillation_ctrl
Up to date
This commit is contained in:
parent
5a6c18d927
commit
3d0b47ac04
|
|
@ -26,6 +26,7 @@ add_message_files(
|
|||
RefSignal.msg
|
||||
EulerAngles.msg
|
||||
LoadAngles.msg
|
||||
LoadStatus.msg
|
||||
)
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
|
|
@ -33,6 +34,7 @@ add_message_files(
|
|||
FILES
|
||||
WaypointTrack.srv
|
||||
BodyToWorld.srv
|
||||
UseCtrl.srv
|
||||
)
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
catkin build
|
||||
|
|
@ -1,15 +1,27 @@
|
|||
# Ros param when using Klausen Ctrl
|
||||
wait_time: 15 # parameter which can be set to run desired tests at a desired time
|
||||
#wait_time: 35 # parameter which can be set to run desired tests at a desired time
|
||||
wait_time: 20
|
||||
#drone_mass: 0.614 # weight with new battery
|
||||
drone_mass: 0.602
|
||||
|
||||
pload: true
|
||||
#tether_length: 2.0
|
||||
pload_mass: 0.2 # mass of payload.
|
||||
#pload_mass: 0.15 # Pload mass with 100g weight
|
||||
#pload_mass: 0.05 # Pload mass with just basket
|
||||
#pload_mass: 0.25
|
||||
change_mode: true # choose whether to switch to oscillation damping controller
|
||||
waypoints: {x: 0.0, y: 0.0, z: 2.0} # takeoff waypoints
|
||||
change_mode: true # choose whether to switch to oscillation damping controller
|
||||
#waypoints: {x: -2.5, y: -2.5, z: 2.15} # takeoff waypoints for square test
|
||||
waypoints: {x: 0.0, y: 0.0, z: 5.0}
|
||||
#waypoints: {x: 0.0, y: 0.0, z: 1.5} # step test - short tether
|
||||
# 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]
|
||||
|
||||
# SET DESIRED TEST
|
||||
square: false
|
||||
step: true
|
||||
|
||||
# SET DIRECTORY AND NAME OF BAGFILE
|
||||
directory: "/home/cesar/bagfiles/2023/04/10"
|
||||
bagfile: "testing_ctrl.bag"
|
||||
|
|
|
|||
|
|
@ -1,5 +1,8 @@
|
|||
# Ros param when using Klausen Ctrl
|
||||
wait_time: 30 # parameter which can be set to run desired tests at a desired time
|
||||
# wait_time: 45 # 30 at 2023-01-19 parameter which can be set to run desired tests at a desired time
|
||||
#wait_time: 35 # 30 at 2023-01-19 parameter which can be set to run desired tests at a desired time
|
||||
#wait_time: 35
|
||||
wait_time: 30
|
||||
|
||||
# DRONE MASSES
|
||||
#drone_mass: 0.614 # weight with new battery
|
||||
|
|
@ -7,24 +10,53 @@ drone_mass: 0.602
|
|||
|
||||
#PLOAD MASSES
|
||||
#pload_mass: 0.25
|
||||
#pload_mass: 0.20
|
||||
#pload_mass: 0.157 # IMU payload
|
||||
#pload_mass: 0.15 # Pload mass with 100g weight
|
||||
#pload_mass: 0.10 # Pload mass with 50g 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
|
||||
# pload_mass: 0.01 # No Pload
|
||||
pload: true
|
||||
|
||||
# TETHER LENGTH
|
||||
tether_length: 2.0
|
||||
|
||||
# SET DESIRED TEST
|
||||
square: false
|
||||
step: false
|
||||
hover: true
|
||||
|
||||
# CTRL PARAMETER - should be false to start always
|
||||
# use_ctrl: false # starts PX4 without attitude controller
|
||||
change_mode: false # choose whether to switch to oscillation damping controller
|
||||
change_mode: true # choose whether to switch to oscillation damping controller
|
||||
|
||||
waypoints: {x: -1.0, y: -1.0, z: 2.0} # takeoff waypoints
|
||||
# STARTING POSITION - Where drone will hover at until commanded otherwise
|
||||
|
||||
# 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]
|
||||
#waypoints: {x: 1.0, y: 0.0, z: 1.5} # debugging
|
||||
waypoints: {x: 0.0, y: 0.0, z: 2.5} # takeoff waypoints for step test
|
||||
#waypoints: {x: 0.0, y: -1.0, z: 2.15} # takeoff waypoints for step test
|
||||
#waypoints: {x: -0.75, y: -0.5, z: 2.15} # takeoff waypoints for square test
|
||||
#waypoints: {x: -0.5, y: 0.0, z: 2.5} # takeoff waypoints for step test
|
||||
#waypoints: {x: 0.0, y: 0.0, z: 2.0} # step test - short tether
|
||||
#waypoints: {x: 0.0, y: 0.0, z: 1.5} # step test - short tether
|
||||
#waypoints: {x: -0.5, y: 0.0, z: 1.5} # takeoff waypoints for square test # lab
|
||||
|
||||
# HOVER THROTTLE - Changes depending on mass of pload and drone
|
||||
# hover_throttle: 0.32 # Hover throttle with pload 0.15 kg
|
||||
#hover_throttle: 0.3 # Hover throttle with pload 0.16 kg (IMU) - This was found with full battery
|
||||
# hover_throttle: 0.27 # Hover throttle with pload 0.10 kg
|
||||
# hover_throttle: 0.26 # Hover throttle with pload 0.05 kg
|
||||
# hover_throttle: 0.23 # Hover throttle with no pload - about 0.23 with full battery
|
||||
|
||||
### NEW
|
||||
#hover_throttle: 0.24 # Hover throttle no pload
|
||||
hover_throttle: 0.275 # Hover throttle with pload 0.05 kg
|
||||
#hover_throttle: 0.305 # Hover throttle with pload 0.10 kg
|
||||
#hover_throttle: 0.310 # Hover throttle with pload 0.16 kg
|
||||
|
||||
#hover_throttle: 0.31 # Hover throttle with pload 0.20 kg good results
|
||||
#hover_throttle: 0.30 # Hover throttle with pload 0.20 kg
|
||||
#hover_throttle: 0.37 # Hover throttle with pload 0.20 kg # ????
|
||||
|
||||
# NEW BATTERY
|
||||
#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
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
# Set useful ROS parameters for simulation
|
||||
tether_length: 1.0 # length of tether
|
||||
mass: 0.160 # mass of payload
|
||||
start_pos: [0.38,0.86,1.1] # x, y, and z of start. Used to determine load angles (z is not used)
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
catkin build
|
||||
|
|
@ -11,6 +11,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
<param name="test_type" value="$(arg test)"/>
|
||||
</group>
|
||||
|
||||
|
||||
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
@ -61,6 +62,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
name="set_ploadmass"
|
||||
output="screen"
|
||||
/>
|
||||
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find oscillation_ctrl)/launch/$(arg model).launch"/>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
# Reports position of drone, payload
|
||||
std_msgs/Header header # Header
|
||||
geometry_msgs/Vector3 drone_pos # Drone position
|
||||
geometry_msgs/Vector3 pload_pos # Payload position
|
||||
geometry_msgs/Vector3 tethered_pos # Payload position relative to drone
|
||||
|
||||
|
|
@ -8,7 +8,7 @@ import rosservice
|
|||
import time
|
||||
import math
|
||||
from tf.transformations import *
|
||||
from oscillation_ctrl.msg import TetheredStatus, LoadAngles
|
||||
from oscillation_ctrl.msg import TetheredStatus, LoadAngles, LoadStatus
|
||||
from gazebo_msgs.srv import GetLinkState
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,80 @@
|
|||
#!/usr/bin/env python2.7
|
||||
|
||||
### Cesar Rodriguez March 2022
|
||||
### Reads waypoints from .yaml file and keeps track of them
|
||||
|
||||
import rospy, tf
|
||||
import rosservice
|
||||
from std_msgs.msg import Bool
|
||||
from oscillation_ctrl.srv import UseCtrl, UseCtrlResponse
|
||||
|
||||
class Main:
|
||||
|
||||
def __init__(self):
|
||||
|
||||
# Variables needed for testing start
|
||||
self.tstart = rospy.get_time() # Keep track of the start time
|
||||
while self.tstart == 0.0: # Need to make sure get_rostime works
|
||||
self.tstart = rospy.get_time()
|
||||
|
||||
# initialize variables
|
||||
self.use_ctrl = None
|
||||
self.ctrl_state = UseCtrlResponse()
|
||||
|
||||
for i in range(3):
|
||||
try:
|
||||
self.use_ctrl = rospy.get_param('/status/change_mode')
|
||||
except:
|
||||
rospy.loginfo('Waiting for parameter "status/change_mode" to become available...')
|
||||
rospy.loginfo('Trying {0} more time(s)'.format(2-i))
|
||||
rospy.sleep(0.1)
|
||||
|
||||
if self.use_ctrl is None:
|
||||
raise rospy.ROSInitException
|
||||
|
||||
# service(s)
|
||||
rospy.Service('/status/att_ctrl_srv', UseCtrl, self.ctrl_tracker)
|
||||
|
||||
# subscriber(s)
|
||||
|
||||
# publisher(s)
|
||||
self.state_pub = rospy.Publisher('/status/att_ctrl', Bool, queue_size=10)
|
||||
rospy.Timer(rospy.Duration(1.0/2.0), self.publisher)
|
||||
|
||||
def ctrl_tracker(self, request):
|
||||
""" Keeps track of controller state """
|
||||
# check if we want to change state or whether it is a check
|
||||
if request.query:
|
||||
# change use_ctrl param to requested state
|
||||
rospy.set_param('/status/use_ctrl', request.state)
|
||||
|
||||
# set result to new state
|
||||
self.ctrl_state.result = rospy.get_param('/status/use_ctrl')
|
||||
|
||||
# check if everything worked
|
||||
if request.state is self.ctrl_state.result:
|
||||
self.ctrl_state.success = True
|
||||
else:
|
||||
self.ctrl_state.success = False
|
||||
else:
|
||||
self.ctrl_state.success = True
|
||||
return self.ctrl_state
|
||||
|
||||
def publisher(self, timer):
|
||||
""" publish state so that nodes do not have query the service every time"""
|
||||
self.state_pub.publish(self.ctrl_state.result)
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
rospy.init_node('waypoints_server',anonymous=False)
|
||||
try:
|
||||
obj = Main() # create class object
|
||||
# s = rospy.Service('status/waypoint_tracker',WaypointTrack, obj.waypoint_relay)
|
||||
# rospy.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z)
|
||||
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
|
|
@ -86,6 +86,50 @@ class LiveLFilter(LiveFilter):
|
|||
self._ys.appendleft(y)
|
||||
|
||||
return y
|
||||
|
||||
class AltCtrlr():
|
||||
|
||||
def __init__(self, Kp, Ki, Kd, drone_mass):
|
||||
self.Kp = Kp
|
||||
self.Ki = Ki
|
||||
self.Kd = Kd
|
||||
self.m = drone_mass
|
||||
|
||||
self.e = 0.0
|
||||
self.e_p = 0.0
|
||||
self.edot = 0.0
|
||||
self.esum = 0.0
|
||||
|
||||
self.t_prev = 0.0
|
||||
|
||||
self.use_ki = False
|
||||
|
||||
# def update_thrust(self, pos_des, pos, dt):
|
||||
def update_thrust(self, pos_des, pos, orientation, t_curr):
|
||||
""" determines new thrust based on error"""
|
||||
|
||||
try:
|
||||
# determine error
|
||||
self.e = pos_des - pos
|
||||
self.edot = (self.e - self.e_p) / (t_curr - self.t_prev)
|
||||
# only accumulate error if we are actually using ths thrust controller
|
||||
if self.use_ki:
|
||||
self.esum += self.e * (t_curr - self.t_prev)
|
||||
else:
|
||||
self.esum = 0.0
|
||||
# get thrust
|
||||
# this formula ignores the path acc
|
||||
thrust = (self.Kp * self.e + self.Ki * self.esum + self.Kd * self.edot + self.m * 9.81) / (math.cos(orientation[0]) * math.cos(orientation[1]))
|
||||
|
||||
# update values
|
||||
self.e_p = self.e
|
||||
self.t_prev = t_curr
|
||||
|
||||
return thrust
|
||||
except ZeroDivisionError:
|
||||
print("ZeroDivisionError in update_thrust")
|
||||
return 0.0
|
||||
|
||||
class Main:
|
||||
|
||||
def __init__(self):
|
||||
|
|
@ -121,7 +165,7 @@ class Main:
|
|||
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 = False # 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]])
|
||||
|
|
@ -133,14 +177,14 @@ class Main:
|
|||
# Get tether length
|
||||
if rospy.get_param('status/pload', False):
|
||||
self.tetherL = self.get_tether()
|
||||
self.tether = True
|
||||
if self.tetherL == 0.0:
|
||||
self.tether = False
|
||||
else:
|
||||
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
|
||||
[self.drone_m, self.pl_m] = self.get_masses()
|
||||
rospy.loginfo('DRONE MASS: %.2f',self.drone_m)
|
||||
|
|
@ -148,7 +192,6 @@ class Main:
|
|||
|
||||
# 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.alphadot = np.zeros([5,1])
|
||||
self.a45 = np.zeros(2)
|
||||
|
|
@ -160,26 +203,40 @@ class Main:
|
|||
self.z_sum = np.zeros([3,1])
|
||||
|
||||
# Tuning gains
|
||||
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.K1 = np.identity(3)*1#*0.5 # 2.0
|
||||
self.K2 = np.identity(5)*1#2.15#1.5#*0.1 # increases velocity and acceleration, also increases pos error
|
||||
|
||||
# self.tune = 0.005 # Tuning parameter
|
||||
self.tune = 0.5 # Tuning parameter 0.1 gives better performance, 0.2 for Gazebo
|
||||
self.K2tune = 10 #1000 # 2023-04-18: Works great in Gazebo. Needs to be x1000 greater than the first 3 terms of K2
|
||||
# self.K2tune = 0.1 # Tuning parameter for last 3 terms of K2
|
||||
self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance
|
||||
# self.dist = np.array([0,0,0,0.0,0.0]) # 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]
|
||||
self.Kd = self.tot_m*self.K1 + self.K2[:3,:3] #+ self.tune*self.K2[:3,:3]
|
||||
self.Ki = self.tune*self.K1
|
||||
|
||||
# PD Thrust Controller terms
|
||||
# gains for thrust PD Controller
|
||||
### 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.Kp_thrust = 5 # 1.5 for lab # 3.5 for gazebo
|
||||
# self.Kp_thrust = 3.0 # 3.5 for PD controller not using trajectory tracking
|
||||
# self.Kd_thrust = 0.14 # 1.5 for lab # 2.5 for gazebo 2.0 if no tether?
|
||||
# self.Kd_thrust = 3.5 # 1.5 for lab # 2.5 for gazebo 2.0 if no tether?
|
||||
|
||||
self.alt_ctrl = AltCtrlr(Kp=3.0, Kd=1.75, Ki=2.0, drone_mass=self.tot_m)
|
||||
|
||||
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]])
|
||||
self.error_p = 0.0
|
||||
self.error_vel = 0.0
|
||||
self.t_curr_t = rospy.get_time() + 0.1
|
||||
self.t_prev_t = rospy.get_time()
|
||||
# Get scaling thrust factor, kf
|
||||
self.kf = self.get_kf()
|
||||
self.thrust = 0.0
|
||||
|
||||
self.service_list = rosservice.get_service_list()
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -194,14 +251,35 @@ class Main:
|
|||
# --------------------------------------------------------------------------------#
|
||||
# PUBLISHERS
|
||||
# --------------------------------------------------------------------------------#
|
||||
self.pub_att_targ = rospy.Publisher('/command/att_target',AttitudeTarget,queue_size = 10)
|
||||
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) # this was 5.0 rate before
|
||||
# rospy.Timer(rospy.Duration(2.0/rate), self.publisher) # this was 5.0 rate before
|
||||
# self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) # this was 5.0 rate before
|
||||
rospy.Timer(rospy.Duration(0.1/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
|
||||
rospy.Timer(rospy.Duration(2.0/rate), self.determine_throttle_new) # this was 5.0 rate before
|
||||
# self.throttle_timer = rospy.Timer(rospy.Duration(1.0/rate), self.determine_throttle) # this was 5.0 rate before
|
||||
# rospy.Timer(rospy.Duration(0.1/rate), self.determine_throttle) # this was 5.0 rate before # best performance when this runs faster than attitude ctrl
|
||||
|
||||
# timer for user feedback
|
||||
rospy.Timer(rospy.Duration(0.1), self.user_feedback)
|
||||
|
||||
# timer to check if we are using attitude controller
|
||||
self.timer = rospy.Timer(rospy.Duration(0.25), self.ctrl_cb)
|
||||
self.att_ctrl = False
|
||||
|
||||
def ctrl_cb(self, timer):
|
||||
""" Check if we are using attitude controller.
|
||||
This is done to stop querying ROS parameters in each loop
|
||||
"""
|
||||
try:
|
||||
if rospy.get_param('status/use_ctrl'):
|
||||
self.att_ctrl = True
|
||||
self.timer.shutdown() # shut down timer once we it is true
|
||||
except KeyError:
|
||||
pass
|
||||
# rospy.loginfo('Waiting for params to be ready')
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SETUP PARAM METHODS
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -258,12 +336,12 @@ class Main:
|
|||
|
||||
hover_throttle = rospy.get_param('status/hover_throttle')
|
||||
rospy.loginfo('Using hover throttle from config %.3f',hover_throttle)
|
||||
self.max_throttle = 0.5
|
||||
self.max_throttle = 0.45
|
||||
else:
|
||||
|
||||
rospy.loginfo('total mass: %.3f',self.tot_m)
|
||||
|
||||
hover_throttle = (self.tot_m*9.81 + 9.81)/34.84 # linear scaling dependent on mass
|
||||
hover_throttle = (self.tot_m*9.81 + 8.9)/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)
|
||||
|
|
@ -272,13 +350,6 @@ class Main:
|
|||
rospy.set_param('status/motor_constant',kf)
|
||||
return kf
|
||||
|
||||
# Check if vehicle has tether
|
||||
def tether_check(self):
|
||||
if self.tether == True:
|
||||
rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL)
|
||||
else:
|
||||
rospy.loginfo_once('NO TETHER DETECTED')
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# CALLBACK FUNCTIONS
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -288,9 +359,10 @@ class Main:
|
|||
try:
|
||||
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]])
|
||||
# self.PHI = np.array([[self.load_angles.theta,self.load_angles.phi],[self.load_angles.thetadot,self.load_angles.phidot]])
|
||||
self.PHI = np.array([[self.load_angles.phi,self.load_angles.theta],[self.load_angles.phidot,self.load_angles.thetadot]])
|
||||
|
||||
# self.PHI = np.array([[0.0,0.0],[0.0,0.0]])
|
||||
# self.PHI = np.array([[0.0,0.0],[0.0,0.0]]) # in case we want to test performance without load angles
|
||||
|
||||
# print('PHI:\n{0}'.format(self.PHI))
|
||||
# print('Angles:\n{0}'.format(self.PHI[0,:]))
|
||||
|
|
@ -342,10 +414,10 @@ class Main:
|
|||
try:
|
||||
resp = self.get_xd(False,self.empty_point)
|
||||
self.xd = resp.xd
|
||||
except ValueError:
|
||||
pass
|
||||
except ValueError as e:
|
||||
rospy.loginfo('{0}'.format(e))
|
||||
else:
|
||||
self.xd = DesiredPoint(0.0,0.0,1.5)
|
||||
self.xd = Point(x=0.0,y=0.0,z=2.0)
|
||||
|
||||
# ---------------------------------ODE SOLVER-------------------------------------#
|
||||
def statespace(self,y,t,Ka,Kb,Kc):
|
||||
|
|
@ -430,33 +502,89 @@ class Main:
|
|||
t4 = +1.0 - 2.0 * (y * y + z * z)
|
||||
yaw = math.atan2(t3, t4)
|
||||
|
||||
euler = [roll,pitch,yaw]
|
||||
euler = [roll, pitch, yaw]
|
||||
return euler
|
||||
|
||||
def determine_throttle(self,throttle_timer):
|
||||
def determine_throttle_old(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 = 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.reshape(3,1) - self.dr_vel # best performance
|
||||
|
||||
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.tot_m*(self.g + self.path_acc) + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel
|
||||
# thrust_vector = (self.g*self.tot_m + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel) * self.kf
|
||||
|
||||
self.thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
|
||||
# self.thrust = (thrust_vector[0] * b3[0] + thrust_vector[1] * b3[1] + thrust_vector[2] * b3[2]) * self.kf # best performance
|
||||
# self.thrust = (thrust_vector[0] * b3[0] + thrust_vector[1] * b3[1] + thrust_vector[2] * b3[2]) # best performance
|
||||
# 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(self.thrust * self.kf,self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
# 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))
|
||||
|
||||
# Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],self.thrust,0.0],dr_orientation_inv)) # Real part of Fd needs = 0
|
||||
|
||||
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))
|
||||
# b3 = np.array([0,0,1])
|
||||
|
||||
self.t_curr_t = rospy.get_time()
|
||||
|
||||
self.error = np.array([[0],
|
||||
[0],
|
||||
[self.path_pos[2] - self.dr_pos.position.z]]).reshape(3,1)
|
||||
self.e_sum += self.error
|
||||
|
||||
# self.error_vel = self.path_vel.reshape(3,1) - self.dr_vel # best performance
|
||||
self.error_vel = (self.error[2] - self.error_p) / (self.t_curr_t - self.t_prev_t)
|
||||
|
||||
|
||||
# thrust_vector = self.tot_m*(self.g[2] + self.path_acc[2]) + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2]
|
||||
thrust_vector = self.tot_m*(self.g[2] + self.path_acc[2]) + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel
|
||||
|
||||
# thrust_vector = self.tot_m*(self.g + self.path_acc) + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel
|
||||
# thrust_vector = (self.g*self.tot_m + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel) * self.kf
|
||||
|
||||
# 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
|
||||
self.thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
|
||||
# self.thrust = (thrust_vector[0] * b3[0] + thrust_vector[1] * b3[1] + thrust_vector[2] * b3[2]) * self.kf # best performance
|
||||
# self.thrust = (thrust_vector[0] * b3[0] + thrust_vector[1] * b3[1] + thrust_vector[2] * b3[2]) # best performance
|
||||
# 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,self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
self.att_targ.thrust = max(0.0, min(self.thrust * self.kf, self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
# 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))
|
||||
|
||||
# update
|
||||
self.error_p = self.error[2]
|
||||
self.t_prev_t = self.t_curr_t
|
||||
|
||||
def determine_throttle_new(self, throttle_timer):
|
||||
self.waypoints_srv_cb() # check for new waypoints
|
||||
|
||||
# check if we are using the controller
|
||||
self.alt_ctrl.use_ki = self.att_ctrl
|
||||
|
||||
# self.t_curr_t = rospy.get_time()
|
||||
self.thrust = self.alt_ctrl.update_thrust(self.path_pos[2], self.dr_pos.position.z, self.EulerPose ,rospy.get_time())
|
||||
# self.thrust = self.alt_ctrl.update_thrust(self.path_pos[2], self.dr_pos.position.z, rospy.get_time())/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
|
||||
self.att_targ.thrust = max(0.0, min(self.thrust * self.kf, self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
# self.att_targ.thrust = max(0.0, min(self.thrust, self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
|
||||
def determine_q(self):
|
||||
""" Determine attitude commands based on feedback and feedforward methods"""
|
||||
# Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi]
|
||||
|
|
@ -483,7 +611,8 @@ class Main:
|
|||
[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]]
|
||||
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]]
|
||||
# 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]] ##########
|
||||
|
||||
H = [[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0]]
|
||||
|
||||
|
|
@ -505,13 +634,14 @@ class Main:
|
|||
C_c = C[:3,3:5]
|
||||
|
||||
# Determine alpha
|
||||
# if not 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
|
||||
|
||||
# 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))
|
||||
Ka = -(D_a + C_a + self.K2tune*self.K2[3:5,3:5])
|
||||
Kb = -G_a + np.dot(self.K2tune*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))
|
||||
#
|
||||
M_aI = np.linalg.inv(M_a) # Inverse of M_a
|
||||
# print('M_aI was found to be:\n{0}\n'.format(M_aI))
|
||||
|
||||
|
|
@ -520,9 +650,10 @@ class Main:
|
|||
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))
|
||||
# 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
|
||||
|
||||
# Update a45_0 to new a45. Need to transpose to column vector
|
||||
self.a45_0 = self.a45_buff[0,:]
|
||||
# self.a45_0 = self.a45_buff[0,:]
|
||||
self.a45 = self.a45_buff[-1,:]
|
||||
# self.a45 = M_aI.dot(self.a45_buff[-1,:])
|
||||
|
||||
|
|
@ -530,8 +661,6 @@ class Main:
|
|||
|
||||
# Get alphadot_4:5
|
||||
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]])
|
||||
|
|
@ -553,13 +682,14 @@ class Main:
|
|||
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
|
||||
if self.att_ctrl:
|
||||
z_int = self.z_sum + self.z1 * 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
|
||||
# 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
|
||||
path_acc = np.array([self.path_acc[0],self.path_acc[1],self.path_acc[2]]).reshape(3,1) # need to add gravity for controller to work
|
||||
|
||||
# Desired body-oriented forces
|
||||
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)
|
||||
|
|
@ -570,18 +700,20 @@ class Main:
|
|||
self.z_sum = z_int
|
||||
|
||||
# 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],-self.thrust,0.0],dr_orientation_inv)) # Real part of Fd needs = 0
|
||||
# Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],self.thrust,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
|
||||
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],0.0,0.0],dr_orientation_inv)) # Real part of Fd needs = 0, ignoring F_z since assuming acc_z = 0
|
||||
|
||||
# rospy.loginfo('\nFd_tf:\n{0}'.format(Fd_tf))
|
||||
|
||||
# Convert forces to attiude *EulerAng[2] = yaw = 0
|
||||
# Convert forces to attitude *EulerAng[2] = yaw = 0
|
||||
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])
|
||||
|
||||
self.user_feedback(Fd,Fd_tf)
|
||||
# self.user_feedback(Fd,Fd_tf)
|
||||
|
||||
# Populate msg variable
|
||||
# Attitude control
|
||||
|
|
@ -593,24 +725,28 @@ class Main:
|
|||
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.x = self.thrust # for debuggin
|
||||
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')
|
||||
rospy.loginfo('thrust: %.6f' % self.att_targ.thrust)
|
||||
rospy.loginfo('roll: %.2f pitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14)
|
||||
rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', F_noTransform[0], F_noTransform[1], F_noTransform[2])
|
||||
rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', F_Transform[0],F_Transform[1],F_Transform[2])
|
||||
# self.att_targ.thrust = max(0.0,min(Fd_tf[2] * self.kf,self.max_throttle)) # will never want it to be more than 0.5 with LabDrone
|
||||
|
||||
def user_feedback(self, timer):
|
||||
# def user_feedback(self,F_noTransform, F_Transform):
|
||||
rospy.loginfo('\n----------------------')
|
||||
rospy.loginfo('thrust: %.6f' % self.thrust)
|
||||
rospy.loginfo('Attitude Mode: {0}'.format(self.att_ctrl))
|
||||
rospy.loginfo('roll: %.2f', self.EulerAng[0]*180/3.14)
|
||||
rospy.loginfo('pitch: %.2f', self.EulerAng[1]*180/3.14)
|
||||
# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', F_noTransform[0], F_noTransform[1], F_noTransform[2])
|
||||
# rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', F_Transform[0],F_Transform[1],F_Transform[2])
|
||||
# rospy.loginfo('a45 was found to be:{0}'.format(self.a45))
|
||||
# rospy.loginfo('a45dot was found to be:{0}'.format(self.a45dot))
|
||||
|
||||
def publisher(self,pub_time):
|
||||
# self.determine_throttle()
|
||||
self.determine_q()
|
||||
self.pub_att_targ.publish(self.att_targ)
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# ALGORITHM ENDS
|
||||
# --------------------------------------------------------------------------------#
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
|
|
|
|||
|
|
@ -15,17 +15,6 @@ from oscillation_ctrl.srv import WaypointTrack
|
|||
from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped
|
||||
from sensor_msgs.msg import Imu
|
||||
|
||||
class DesiredPoint():
|
||||
def __init__(self,x,y,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:
|
||||
|
||||
def __init__(self):
|
||||
|
|
@ -56,10 +45,10 @@ class Main:
|
|||
if rospy.get_param('status/pload'):
|
||||
self.tetherL = self.get_tether()
|
||||
else: self.tetherL = 0.0
|
||||
rospy.loginfo('tether length: {0}'.format(self.tetherL))
|
||||
rospy.loginfo('Tether length: {0}'.format(self.tetherL))
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SUBSCRIBERS #
|
||||
# SUBSCRIBERS #
|
||||
# --------------------------------------------------------------------------------#
|
||||
# Topic, msg type, and class callback method
|
||||
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
|
||||
|
|
@ -86,15 +75,17 @@ class Main:
|
|||
self.EPS_I = np.zeros(9) # Epsilon shapefilter
|
||||
|
||||
# Constants for smooth path generation
|
||||
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
|
||||
self.w_tune = 2 # 1.5 for lab # 1.5 for Gaz. Increase this to increase aggresiveness of trajectory i.e. higher accelerations
|
||||
# self.epsilon = 0.8 # Damping ratio
|
||||
self.epsilon = 0.707 # Damping ratio
|
||||
# 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 = 1.51 # 1.01 is the imperically determined nat freq in gazebo
|
||||
self.wn = math.sqrt(9.81/self.tetherL)
|
||||
# self.wn = 3 #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
|
||||
|
|
@ -103,7 +94,7 @@ class Main:
|
|||
self.k1 = (self.w_tune**4)/(self.k4*self.k3*self.k2)
|
||||
|
||||
# For saturation:
|
||||
self.max = [0, 5, 2.5, 3] #[0, 3, 1.5, 3] - lab testing
|
||||
self.max = [0, 7, 5, 3] #[0, 5, 3, 3] - lab testing
|
||||
|
||||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
self.empty_point = Point() # Needed to query waypoint_server
|
||||
|
|
@ -121,10 +112,16 @@ class Main:
|
|||
|
||||
# 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.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
|
||||
# From Klausen 2015
|
||||
self.t2 = math.pi/ self.wn
|
||||
self.K = 1
|
||||
self.A1 = 0.4948
|
||||
self.A2 = 0.5052
|
||||
rospy.loginfo('Regarding input shaping: K = {0}, A1 = {1}, A2 = {2}, and t2 = {3}'.format(self.K, self.A1, self.A2, self.t2))
|
||||
|
||||
# Need to determine how large of any array needs to be stored to use delayed functions
|
||||
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
|
||||
|
|
@ -145,11 +142,12 @@ class Main:
|
|||
# Constants for sigmoid
|
||||
self.at = 3 # acceleration theshold
|
||||
self.pc = 0.7 # From Klausen 2017
|
||||
# self.pc = 0.99 # Changed
|
||||
self.sk = self.SF_delay_x.shape[1] # from Klausen 2017
|
||||
self.ak = np.zeros(self.sk)
|
||||
self.s_gain = 0.0 # Gain found from sigmoid
|
||||
|
||||
self.service_list = rosservice.get_service_list()
|
||||
self.waypoint_service = False
|
||||
# --------------------------------------------------------------------------------#
|
||||
# CALLBACK FUNCTIONS
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -157,7 +155,8 @@ class Main:
|
|||
# Callback to get link names, states, and pload deflection angles
|
||||
def loadAngles_cb(self,msg):
|
||||
try:
|
||||
self.load_angles = msg
|
||||
self.load_angles = msg
|
||||
pass
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
|
|
@ -187,15 +186,24 @@ class Main:
|
|||
pass
|
||||
|
||||
def waypoints_srv_cb(self):
|
||||
if '/status/waypoint_tracker' in self.service_list:
|
||||
rospy.wait_for_service('/status/waypoint_tracker')
|
||||
if self.waypoint_service:
|
||||
try:
|
||||
resp = self.get_xd(False,self.empty_point)
|
||||
self.xd = resp.xd
|
||||
except ValueError:
|
||||
pass
|
||||
except ValueError as e:
|
||||
rospy.loginfo('{0}'.format(e))
|
||||
else:
|
||||
self.xd = DesiredPoint(0.0,0.0,2.0)
|
||||
if '/status/waypoint_tracker' in rosservice.get_service_list(): # dont want to always call this service
|
||||
# rospy.wait_for_service('/status/waypoint_tracker')
|
||||
self.waypoint_service = True
|
||||
try:
|
||||
resp = self.get_xd(False,self.empty_point)
|
||||
self.xd = resp.xd
|
||||
except ValueError as e:
|
||||
rospy.loginfo('{0}'.format(e))
|
||||
else:
|
||||
rospy.loginfo('Service not available, defaulting to save point:\nx = 0.0 m, y = 0.0 m, x = 2.0 m')
|
||||
self.xd = Point(x=0.0,y=0.0,z=2.0)
|
||||
|
||||
def get_tether(self):
|
||||
""" Get tether length based on set parameters"""
|
||||
|
|
@ -222,8 +230,6 @@ class Main:
|
|||
pos,vel,acc,jer = y
|
||||
|
||||
error = xd - pos
|
||||
# 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)]
|
||||
|
||||
|
|
@ -330,13 +336,13 @@ class Main:
|
|||
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.delay(1,FEEDBACK) # Detemine feedback array
|
||||
|
||||
self.sigmoid() # Determine sigmoid gain
|
||||
# self.sigmoid() # Determine sigmoid gain
|
||||
Eps_D = self.fback() # Feedback Epsilon
|
||||
|
||||
self.EPS_F = self.EPS_I + self.s_gain*Eps_D
|
||||
# self.EPS_F = self.EPS_I + Eps_D
|
||||
|
||||
# Populate msg with epsilon_final
|
||||
self.ref_sig.header.stamp = rospy.Time.now()
|
||||
|
|
@ -354,11 +360,13 @@ class Main:
|
|||
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.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
|
||||
# self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]]
|
||||
# self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]]
|
||||
|
||||
self.SF_idx += 1
|
||||
self.FB_idx += 1
|
||||
|
||||
|
||||
def fback(self):
|
||||
""" Uses first element in feedback array as this is what get replaced in each iteration """
|
||||
|
||||
|
|
@ -383,7 +391,6 @@ class Main:
|
|||
rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
|
||||
rospy.loginfo('_______________________')
|
||||
|
||||
|
||||
def publisher(self,pub_tim):
|
||||
|
||||
# Determine final ref signal
|
||||
|
|
@ -397,8 +404,10 @@ class Main:
|
|||
self.waypointTracker_pub.publish(self.xd)
|
||||
|
||||
# Give user feedback on published message:
|
||||
self.user_feeback()
|
||||
except AttributeError:
|
||||
# self.user_feeback()
|
||||
except AttributeError as e:
|
||||
rospy.loginfo('Error {0}'.format(e))
|
||||
|
||||
pass # catches case at beginning when self.xd is not properly initialized
|
||||
|
||||
if __name__=="__main__":
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ import rospy, tf
|
|||
import rosservice
|
||||
import time
|
||||
from geometry_msgs.msg import Point
|
||||
from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse
|
||||
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackResponse
|
||||
|
||||
class Main:
|
||||
|
||||
|
|
@ -41,7 +41,7 @@ class Main:
|
|||
|
||||
|
||||
# subscriber(s)
|
||||
rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb)
|
||||
# rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb)
|
||||
|
||||
# publisher(s)
|
||||
|
||||
|
|
@ -55,11 +55,11 @@ class Main:
|
|||
return self.resp
|
||||
|
||||
# Change desired position if there is a change
|
||||
def waypoints_cb(self,msg):
|
||||
try:
|
||||
self.xd = msg
|
||||
except ValueError or TypeError:
|
||||
pass
|
||||
# def waypoints_cb(self,msg):
|
||||
# try:
|
||||
# self.xd = msg
|
||||
# except ValueError or TypeError:
|
||||
# pass
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
bool state # What the desired state of /status/use_ctrl should be
|
||||
bool query # True if state is desired to be "query", False if client just wants to determine the state
|
||||
---
|
||||
bool success # Whether it was able to change modes
|
||||
bool result # Current state of /status/use_ctrl
|
||||
|
||||
Loading…
Reference in New Issue