More improvements to controller

This commit is contained in:
cesar 2022-08-18 15:29:26 -03:00
parent 316c8a7855
commit 469accc8dc
17 changed files with 417 additions and 262 deletions

3
.gitignore vendored
View File

@ -1,3 +1,4 @@
config/mocap_*
launch/cortex_bridge.launch
launch/debug.launch
launch/klausen_dampen.launch
@ -7,10 +8,12 @@ src/killswitch_client.py
src/land_client.py
src/MoCap_*.py
src/Mocap_*.py
src/mocap_*
src/segmented_tether.py
src/segmented_tether_fast.py
msg/Marker.msg
msg/Markers.msg
*.rviz
CMakeLists.txt
setup.txt

View File

@ -15,39 +15,10 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
@ -78,26 +49,6 @@ generate_messages(
sensor_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
@ -112,7 +63,7 @@ catkin_package(
# LIBRARIES oscillation_ctrl
CATKIN_DEPENDS CATKIN_DEPENDS roscpp mavros geometry_msgs message_runtime
# DEPENDS system_lib
)
###########
@ -138,6 +89,18 @@ target_link_libraries(pathFollow_node ${catkin_LIBRARIES})
add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
#add_executable(mocap_offb_node src/mocap_offb_node.cpp)
#target_link_libraries(mocap_offb_node ${catkin_LIBRARIES})
#add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
#add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
#target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
#add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/oscillation_ctrl.cpp

View File

@ -1,5 +1,8 @@
# Ros param when using Klausen Ctrl
wait_time: 30
#drone_mass: 0.5841
drone_mass: 1.437
pload_mass: 0.50
use_ctrl: false
wait: 40
waypoints: {x: 0.0, y: 0.0, z: 5.0}

View File

@ -0,0 +1,4 @@
# Ros param when not using Klausen Ctrl
waypoints: {x: 0.0, y: 0.0, z: 1.5}
square_x: [0.5,1,1,1,0.5,0,0]
square_y: [0,0,0.5,1,1,1,0.5]

View File

@ -1,4 +1,7 @@
# Ros param when not using Klausen Ctrl
wait_time: 40
drone_mass: 0.5841
#drone_mass: 1.437
pload_mass: 0.10
wait: 55
waypoints: {x: 0.0, y: 0.0, z: 5.0}

View File

@ -5,17 +5,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<launch>
<arg name="model" default="headless_spiri_with_tether"/>
<arg name="test" default="none"/>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
</group>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
</group>
<group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
<param name="test_type" value="$(arg test)"/>
</group>
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
<node
pkg="oscillation_ctrl"
@ -40,7 +37,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
pkg="oscillation_ctrl"
type="klausen_control.py"
name="klausenCtrl_node"
output="screen"
launch-prefix="xterm -e"
/>
<!-- PUBLISHES DESIRED COMMANDS -->
<node

View File

@ -2,10 +2,12 @@
<launch>
<arg name="model" default="headless_spiri_with_tether"/>
<arg name='test' default="none"/>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
</group>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
</group>
<group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
<param name="test_type" value="$(arg test)"/>
</group>

View File

@ -1,6 +1,6 @@
# Reports position of drone, payload, and the roll angle between them
# Reports position of drone, payload
std_msgs/Header header # Header
std_msgs/Bool bool # Boolean, True = Payload
bool pload # Boolean, True = Payload
geometry_msgs/Pose drone_pos # Drone pose
geometry_msgs/Pose pload_pos # Payload pose

View File

@ -45,7 +45,7 @@
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<include file="$(find oscillation_ctrl)/launch/px4.launch">
<!-- GCS link is provided by SITL -->
<arg name="gcs_url" value=""/>
<arg name="fcu_url" value="$(arg fcu_url)"/>

View File

@ -19,7 +19,7 @@ class Main:
# rate(s)
rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz
self.dt = 1.0/rate
self.dt = 1.0/rate
rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru
@ -30,7 +30,7 @@ class Main:
# How long should we wait before before starting test
self.param_exists = False
while self.param_exists == False:
if rospy.has_param('sim/wait'):
if rospy.has_param('wait_time'):
self.wait = rospy.get_param('sim/wait') # wait time
self.param_exists = True
elif rospy.get_time() - self.tstart >= 3.0:
@ -143,7 +143,7 @@ class Main:
pload_P.link_state.pose.position.y)**2 +
(drone_P.link_state.pose.position.z -
pload_P.link_state.pose.position.z)**2)
rospy.set_param('sim/tether_length',self.tetherL)
rospy.set_param('status/tether_length',self.tetherL)
else:
self.tetherL = 0

65
src/gazebo_setMasses.py Executable file
View File

@ -0,0 +1,65 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez Jul 2022
### Sets mass of links in gazebo to desired values
import rospy
from gazebo_msgs.srv import SetLinkProperties
class Main:
def __init__(self):
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()
# Constants
self.param_exists = False
while self.param_exists == False:
if rospy.has_param('drone_mass'):
self.drone_m = rospy.get_param('drone_mass') # wait time
self.param_exists = True
elif rospy.has_param('sim/drone_mass'):
self.drone_m = rospy.get_param('sim/drone_mass') # wait time
self.param_exists = True
elif rospy.get_time() - self.tstart >= 3.0:
self.drone_m = 1.0
rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
break
self.param_exists = False
while self.param_exists == False:
if rospy.has_param('pload_mass'):
self.pl_m = rospy.get_param('pload_mass') # wait time
self.param_exists = True
elif rospy.has_param('sim/pload_mass'):
self.pl_m = rospy.get_param('sim/pload_mass') # wait time
self.param_exists = True
elif rospy.get_time() - self.tstart >= 3.0:
self.pl_m = 0.0
rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
break
self.drone_ids = ['spiri_with_tether::spiri::base', 'spiri::base']
self.pload_id = 'spiri_with_tether::mass::payload'
self.service = '/gazebo/set_link_properties'
self.set_mass = rospy.ServiceProxy(self.service,SetLinkProperties)
for name in self.drone_ids:
self.set_drone_mass = self.set_mass(link_name=name,mass=self.drone_m)
if self.set_drone_mass.success:
rospy.loginfo('Set "%s" mass to: %.2f kg', name,self.drone_m)
break
self.set_pload_mass = self.set_mass(link_name=self.pload_id,mass=self.pl_m)
if self.set_pload_mass.success:
rospy.loginfo('Set "%s" mass to: %.2f kg', self.pload_id,self.pl_m)
if __name__=="__main__":
# Initiate ROS node
rospy.init_node('SetMassNode',anonymous=False)
Main() # create class object

View File

@ -11,21 +11,28 @@ import rospy, tf
import numpy as np
import time
import math
import rosservice
from tf.transformations import *
from scipy.integrate import odeint
from oscillation_ctrl.msg import RefSignal, LoadAngles
from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus
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
class DesiredPoint():
def __init__(self,x,y,z):
self.x = x
self.y = y
self.z = z
class Main:
def __init__(self):
# rate(s)
rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz
# initialize variables
@ -54,7 +61,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 = 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]])
@ -65,19 +72,13 @@ class Main:
# Tether var - Check if current method is used
# Get tether length
self.param_exists = False
while self.param_exists == False: # Need to wait until param is ready
if rospy.has_param('sim/tether_length'):
self.tetherL = rospy.get_param('sim/tether_length') # Tether length
self.param_exists = True
self.tether = True
elif rospy.get_time() - self.tstart >= 10.0:
self.tetherL = 0.0
rospy.loginfo('waiting for tether length')
break
self.tetherL = self.get_tether()
self.tether = True if self.tetherL > 0.01 else False
# Tuning gains
self.K1 = np.identity(3)
self.K2 = np.identity(5)
# 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)
self.tot_m = self.drone_m + self.pl_m
# Values which require updates. *_p = prior
self.z1_p = np.zeros([3,1])
@ -90,33 +91,42 @@ class Main:
# 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
# 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 # np.array([0,0,0,0.01,0.01])
# 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.Ki = self.tune*self.K1
# 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
# PD Thrust Controller terms
# gains for thrust PD Controller
#self.Kp = 2.7
#self.Kp = 3.0
#self.Kd = 3
self.Kp = 3.0
self.Kd = 3
self.max_a = 14.2
self.Kp_thrust = 1.5
self.Kd_thrust = 1
self.max_a = 14.2 #TODO
self.max_t = self.tot_m*self.max_a
self.R = np.empty([3,3]) # rotation matrix
self.e3 = np.array([[0],[0],[1]])
self.thrust_offset = 1.0 # There was found to be a constant offset 0.7
self.gravity = np.array([[0],[0],[9.81]])
self.thrust_offset = 0.0 #1.0 # There was found to be a constant offset 0.7
# Get scaling thrust factor, kf
self.kf = self.get_kf()
self.service_list = rosservice.get_service_list()
# --------------------------------------------------------------------------------#
# SUBSCRIBERS
# --------------------------------------------------------------------------------#
# Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb)
#rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
# rospy.Subscriber('/status/twoBody_status', TetheredStatus, 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)
@ -130,6 +140,58 @@ class Main:
# 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
# --------------------------------------------------------------------------------#
# SETUP PARAM METHODS
# --------------------------------------------------------------------------------#
def get_tether(self):
""" Get tether length based on set parameters"""
param_exists = False
while param_exists == False:
if rospy.has_param('status/tether_length'):
tether_length = rospy.get_param('status/tether_length') # Tether length
param_exists = True
elif rospy.get_time() - self.tstart >= 5.0:
tether_length = 0.0
break
return tether_length
def get_masses(self):
""" Use parameters to retrieve drone and payload mass set in config file and sets total mass of bodies"""
# Constants
param_exists = False # reset to False to get body masses
while param_exists == False:
if rospy.has_param('status/drone_mass'):
drone_m = rospy.get_param('status/drone_mass') # wait time
param_exists = True
elif rospy.get_time() - self.tstart >= 3.0:
drone_m = 0.5841
rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
break
if self.tether:
param_exists = False
while param_exists == False:
if rospy.has_param('status/pload_mass'):
pl_m = rospy.get_param('status/pload_mass') # wait time
param_exists = True
elif rospy.get_time() - self.tstart >= 3.0:
pl_m = 0.0
rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
break
else:
pl_m = 0.0
rospy.loginfo('PLOAD MASS: %.2f',pl_m)
return [drone_m, pl_m]
def get_kf(self):
if rospy.has_param('mocap/hover_throttle'):
hover_throttle = rospy.get_param('mocap/hover_throttle')
else:
hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass
kf = hover_throttle/(self.tot_m*9.81)
return kf
# --------------------------------------------------------------------------------#
# CALLBACK FUNCTIONS
# --------------------------------------------------------------------------------#
@ -138,20 +200,16 @@ class Main:
def loadAngles_cb(self,msg):
try:
self.load_angles = msg
# Populate self.PHI
self.PHI = np.array([[self.load_angles.theta,self.load_angles.thetadot],[self.load_angles.phi,self.load_angles.phidot]])
except ValueError:
pass
# Callback drone pose
def dronePos_cb(self,msg):
try:
self.dr_pos = msg.pose
#self.dr_pos = msg.drone_pos
# self.dr_pos = msg.drone_pos
except ValueError:
pass
@ -177,7 +235,8 @@ 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]]) #TODO
# self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO
except ValueError:
pass
@ -190,12 +249,15 @@ class Main:
rospy.loginfo_once('NO TETHER DETECTED')
def waypoints_srv_cb(self):
rospy.wait_for_service('/status/waypoint_tracker')
try:
resp = self.get_xd(False,self.empty_point)
self.xd = resp.xd
except ValueError:
pass
if '/status/waypoint_tracker' in self.service_list:
rospy.wait_for_service('/status/waypoint_tracker')
try:
resp = self.get_xd(False,self.empty_point)
self.xd = resp.xd
except ValueError:
pass
else:
self.xd = DesiredPoint(0.0,0.0,1.5)
# ---------------------------------ODE SOLVER-------------------------------------#
def statespace(self,y,t,Ka,Kb,Kc):
@ -253,29 +315,29 @@ class Main:
# 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.error = np.array([[self.dr_pos.position.x - self.xd.x],
[self.dr_pos.position.y - self.xd.y],
[self.dr_pos.position.z - self.xd.z - self.thrust_offset]])
self.R = self.quaternion_rotation_matrix()
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.dr_vel - self.path_vel
self.error_vel = self.path_vel - self.R.dot(self.dr_vel)
self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix
# determine Rotation Matrix
self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t
#thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t
#thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t
thrust_vector = (9.81*self.tot_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
#thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
#thrust = thrust_vector[2]
# test which one is better:
# thrust_vector = (9.81*self.tot_m*self.e3 + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel - self.tot_m*self.path_acc)*self.kf
# thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
### OR:
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.EulerAng[0]*self.EulerAng[1])) #####
# Value needs to be between 0 - 1.0
self.att_targ.thrust = max(0.0,min(thrust,1.0))
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]
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]])
@ -287,12 +349,6 @@ class Main:
s_theta = math.sin(self.load_angles.theta)
s_phi = math.sin(self.load_angles.phi)
# Check for tether
if L <= 0.01:
self.tether = False
else:
self.tether = True
# Check if tether was correctly detected
self.tether_check()
@ -311,7 +367,7 @@ class Main:
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]]
H = [[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0]]
D = np.diag(self.dist) # Already in array format
@ -355,6 +411,7 @@ class Main:
# 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
@ -363,52 +420,60 @@ class Main:
B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot)
# Gain terms
Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3)
Kd = self.tot_m*self.K1 + self.K2[:3,:3]
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,0.5*self.dt*(self.z1 - self.z1_p))
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*self.path_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))
# 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_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
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 = [Fd[0],Fd[1],Fd[2]]
# 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
# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f' % Fd[0],Fd[1],Fd[2])
# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', Fd[0], Fd[1], Fd[2])
# rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', Fd_tf[0],Fd_tf[1],Fd_tf[2])
# Convert Euler angles to quaternions
# for i,val in enumerate(self.EulerAng):
# if val*-1 < 0.0:
# sign = 1
# else:
# sign = -1
# if abs(val) >=0.44:
# self.EulerAng[i] = 0.44*sign
q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2])
# Populate msg variable
# Attitude control
self.att_targ.header.stamp = rospy.Time.now()
self.att_targ.header.frame_id = 'map'
self.att_targ.header.frame_id = '/map'
self.att_targ.type_mask = 1|2|4
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]
def user_fback(self):
rospy.loginfo('thrust: %.6f' % self.att_targ.thrust)
rospy.loginfo('\nroll: %.2f\npitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14)
def publisher(self,pub_time):
self.determine_q()
self.determine_throttle()
self.pub_att_targ.publish(self.att_targ)
#rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1])
self.user_fback()
# --------------------------------------------------------------------------------#
# ALGORITHM
# ALGORITHM ENDS
# --------------------------------------------------------------------------------#
if __name__=="__main__":
# Initiate ROS node

View File

@ -48,16 +48,15 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
ROS_INFO("Waiting...");
} else {
pose_read = true;
pose.pose.orientation.x = q_init.x;
pose.pose.orientation.y = q_init.y;
pose.pose.orientation.z = q_init.z;
pose.pose.orientation.w = q_init.w;
// pose.pose.orientation.x = q_init.x;
// pose.pose.orientation.y = q_init.y;
// pose.pose.orientation.z = q_init.z;
// pose.pose.orientation.w = q_init.w;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
@ -66,7 +65,7 @@ int main(int argc, char **argv)
/********************** SUBSCRIBERS **********************/
// Get current state
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
("mavros/state", 10, state_cb);
// Pose subscriber
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
@ -91,7 +90,7 @@ int main(int argc, char **argv)
// Publish attitude commands
ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
("/mavros/setpoint_attitude/attitude",10);
("/mavros/setpoint_attitude/attitude",10);
// Service Clients
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
@ -99,7 +98,7 @@ int main(int argc, char **argv)
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");
("mavros/cmd/takeoff");
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate(20.0);
@ -163,12 +162,16 @@ int main(int argc, char **argv)
last_request = ros::Time::now();
}
}
// Update desired waypoints
// check if waypoints have changed desired waypoints
waypoint_cl.call(wpoint_srv);
pose.pose.position.x = wpoint_srv.response.xd.x;
pose.pose.position.y = wpoint_srv.response.xd.y;
pose.pose.position.z = wpoint_srv.response.xd.z;
// User info
ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z);
ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z);
ROS_INFO("---------------------------");
local_pos_pub.publish(pose);

View File

@ -4,7 +4,7 @@
*/
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Bool.h>
#include <oscillation_ctrl/RefSignal.h>
#include <oscillation_ctrl/EulerAngles.h>
#include <oscillation_ctrl/WaypointTrack.h>
@ -115,21 +115,6 @@ int main(int argc, char **argv)
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate_pub(25.0);
// Retrieve desired waypoints
oscillation_ctrl::WaypointTrack wpoint_srv;
wpoint_srv.request.query = false;
if (waypoint_cl.call(wpoint_srv))
{
ROS_INFO("Waypoints received");
}
// populate desired waypoints - this is only for original hover as
// a change of waypoints is handled by ref_signal.py
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
double alt_des = buff_pose.pose.position.z; // Desired height
// Desired mode is offboard
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
@ -139,7 +124,8 @@ int main(int argc, char **argv)
arm_cmd.request.value = true;
// Take off command
bool takeoff = false, att_cmds = false;
bool takeoff = false, att_cmds = false;
bool oscillation_damp = true;
// Keep track of time since requests
ros::Time tkoff_req = ros::Time::now();
@ -149,10 +135,26 @@ int main(int argc, char **argv)
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(buff_pose);
ros::spinOnce();
ROS_INFO("Publishing position setpoints");
ROS_INFO_ONCE("Publishing position setpoints");
rate_wait.sleep();
}
// Retrieve desired waypoints
oscillation_ctrl::WaypointTrack wpoint_srv;
wpoint_srv.request.query = false;
if (waypoint_cl.call(wpoint_srv))
{
ROS_INFO("Waypoints received");
// populate desired waypoints - this is only for original hover as
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
} else {
ROS_INFO("NO waypoints received");
}
double alt_des = wpoint_srv.response.xd.z; // Desired height
while(ros::ok()){
if(gps_read == true){
if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
@ -177,26 +179,44 @@ int main(int argc, char **argv)
takeoff = true;
}
}
// Check if we want to use oscillation controller
//if (ros::param::get("/use_ctrl", oscillation_damp) == true){
if (ros::param::has("/status/use_ctrl")){
ros::param::get("/status/use_ctrl", oscillation_damp);
if(oscillation_damp == true){
ROS_INFO("ATTITUDE CTRL");
}
}
// Determine which messages to send
if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
// if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff && oscillation_damp){
// att_targ.header.stamp = ros::Time::now();
// att_targ_pub.publish(att_targ);
// att_cmds = true;
if(oscillation_damp){
att_targ.header.stamp = ros::Time::now();
att_targ_pub.publish(att_targ);
att_cmds = true;
ROS_INFO("ATTITUDE CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
} else {
// Check if waypoints have changed
if (waypoint_cl.call(wpoint_srv))
{
// populate desired waypoints - this is only for original hover as
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
}
local_pos_pub.publish(buff_pose);
ROS_INFO("POSITION CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
}
}
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
ROS_INFO("---------------------------");
ros::spinOnce();
rate_pub.sleep();
}
}
return 0;
}
}

View File

@ -27,8 +27,8 @@ class Main:
# Test parameters
# Square
self.xarray = [1,2,2,2,1,0,0]
self.yarray = [0,0,1,2,2,2,1]
# self.xarray = [1,2,2,2,1,0,0]
# self.yarray = [0,0,1,2,2,2,1]
self.i = 0
self.j = 0
self.buffer = 10
@ -46,6 +46,16 @@ class Main:
self.test = 'none'
break
if rospy.has_param('sim/square_x'):
self.xarray = rospy.get_param('sim/square_x')
self.yarray = rospy.get_param('sim/square_y')
elif rospy.has_param('mocap/square_x'):
self.xarray = rospy.get_param('mocap/square_x')
self.yarray = rospy.get_param('mocap/square_y')
else:
self.xarray = [1,2,2,2,1,0,0]
self.yarray = [0,0,1,2,2,2,1]
# get waypoints
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
@ -77,12 +87,11 @@ class Main:
if self.i >= len(self.xarray):
self.Point.x = 0
self.Point.y = 0
else:
self.Square_Point.x = self.xarray[self.i]
self.Square_Point.y = self.yarray[self.i]
self.Point.x = self.xarray[self.i]
self.Point.y = self.yarray[self.i]
self.Point = self.Square_Point
# self.Point = self.Square_Point
self.j += 1
self.i = self.j // self.buffer
@ -91,7 +100,7 @@ class Main:
self.Point.x = self.step_size # STEP (meters)
def user_feedback(self):
rospy.loginfo("Sending [Point x] %d [Point y] %d",
rospy.loginfo("Sending [Point x] %.2f [Point y] %.2f",
self.Point.x, self.Point.y)
def determine_test(self):

View File

@ -7,30 +7,38 @@ import rospy, tf
import numpy as np
import time
import math
import rosservice
from scipy.integrate import odeint
from oscillation_ctrl.msg import RefSignal, LoadAngles
from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus
from oscillation_ctrl.srv import WaypointTrack
from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped
from sensor_msgs.msg import Imu
from mavros_msgs.msg import State
class DesiredPoint():
def __init__(self,x,y,z):
self.x = x
self.y = y
self.z = z
class Main:
def __init__(self):
# rate(s)
rate = 60 # rate for the publisher method, specified in Hz -- 10 Hz
rate = 50 # rate for the publisher method, specified in Hz -- 10 Hz
# initialize variables
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.dt = 1.0/rate
self.tmax = self.dt # used to get desired pos, vel, and acc for next time step (tmax)
self.n = self.tmax/self.dt + 1
self.t = np.linspace(0, self.tmax, self.n) # Time array
# self.t = np.linspace(0, 1.0, 10) # Time array
# Message generation/ collection
self.vel_data = TwistStamped() # This is needed to get drone vel from gps
@ -42,24 +50,15 @@ class Main:
self.dr_pos = Pose()
self.dr_vel = self.vel_data.twist.linear
self.dr_acc = self.imu_data.linear_acceleration
# Get tether length
self.param_exists = False
while self.param_exists == False:
if rospy.has_param('sim/tether_length'):
self.tetherL = rospy.get_param('sim/tether_length') # Tether length
self.param_exists = True
elif rospy.get_time() - self.tstart >= 10.0:
self.tetherL = 0.0
break
self.tetherL = self.get_tether()
# --------------------------------------------------------------------------------#
# SUBSCRIBERS #
# SUBSCRIBERS #
# --------------------------------------------------------------------------------#
# 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', TetheredStatus, self.dronePos_cb)
# rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
@ -79,11 +78,11 @@ class Main:
# --------------------------------------------------------------------------------#
# Smooth path variables
self.EPS_F = np.zeros(9) # Final Epsilon/ signal
self.EPS_I = np.zeros(9) # Epsilon shapefilter
self.EPS_F = np.zeros(9) # Final Epsilon/ signal
self.EPS_I = np.zeros(9) # Epsilon shapefilter
# Constants for smooth path generation
self.w_tune = 3 # 3.13 works well? #########################################################################
self.w_tune = 1 # 1 also works well :) 3.13 works well? #########################################################################
self.epsilon = 0.7 # Damping ratio
# need exception if we do not have tether:
@ -91,7 +90,7 @@ class Main:
self.wn = self.w_tune
else:
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
@ -99,12 +98,7 @@ class Main:
self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4)
# For saturation:
self.jmax = [3, 3, 1]
self.amax = [5, 5, 1]
self.vmax = [10, 10, 1] #[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
self.max = [0, 8, 1.5, 3] #[0, 10, 1.5, 3]
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
self.empty_point = Point() # Needed to query waypoint_server
@ -146,9 +140,11 @@ class Main:
# 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.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()
# --------------------------------------------------------------------------------#
# CALLBACK FUNCTIONS
# --------------------------------------------------------------------------------#
@ -156,7 +152,7 @@ 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
except ValueError:
pass
@ -164,7 +160,7 @@ class Main:
def dronePos_cb(self,msg):
try:
self.dr_pos = msg.pose
#self.dr_pos = msg.drone_pos
# self.dr_pos = msg.drone_pos
except ValueError:
pass
@ -186,34 +182,46 @@ class Main:
pass
def waypoints_srv_cb(self):
rospy.wait_for_service('/status/waypoint_tracker')
try:
resp = self.get_xd(False,self.empty_point)
self.xd = resp.xd
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 #
######################################################################
if '/status/waypoint_tracker' in self.service_list:
rospy.wait_for_service('/status/waypoint_tracker')
try:
resp = self.get_xd(False,self.empty_point)
self.xd = resp.xd
except ValueError:
pass
else:
self.xd = DesiredPoint(0.0,0.0,2.0)
def get_tether(self):
""" Get tether length based on set parameters"""
self.param_exists = False
while self.param_exists == False:
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:
tether_length = 0.0
break
return tether_length
# --------------------------------------------------------------------------------#
# ALGORITHM
# --------------------------------------------------------------------------------#
# --------------------------------------------------------------------------------#
# TRAJECTORY GENERATION BASED ON WAYPONTS xd
# --------------------------------------------------------------------------------#
def statespace(self,y,t,xd):
# Update initial conditions #
# Need the statespace array:
pos,vel,acc,jer = y
error = xd - pos
if abs(error) <= 0.25: error = 0.0
# Derivative of statesape array:
dydt = [vel, acc, jer,
self.k4*(self.k3*(self.k2*(self.k1*(xd - pos) - vel) - acc) - jer)]
self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)]
return dydt
# Sigmoid
@ -244,7 +252,7 @@ class Main:
else:
# once array is filled, we start using the first value every time
# x
self.SF_delay_x[j,:] = np.roll(self.SF_delay_x[j,:],-1) # makes the first value last
self.SF_delay_x[j,:] = np.roll(self.SF_delay_x[j,:],-1) # makes the first value last
self.SF_delay_x[j,len(self.SF_delay_x[0])-1] = self.x[1,j] # updates last value to current x
# y
@ -285,12 +293,13 @@ class Main:
# Convolution of shape filter and trajectory, and feedback
def convo(self):
# check if waypoints have changed
self.waypoints_srv_cb()
# needed for delay function
# 1 = determine shapefilter array
# 2 = determine theta/phi_fb
shapeFil = 1
feedBack = 2
SHAPEFIL = 1
FEEDBACK = 2
# SOLVE ODE (get ref pos, vel, accel)
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
@ -304,25 +313,26 @@ class Main:
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
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+1] = self.y[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] = 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.delay(1,feedBack) # Detemine feedback array
self.delay(1,FEEDBACK) # Detemine feedback array
self.sigmoid() # Determine sigmoid gain
EPS_D = self.fback() # Feedback Epsilon
for i in range(9):
# for i in range(9):
# Populate EPS_F buffer with desired change based on feedback
self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i]
# self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i]
self.EPS_F = self.EPS_I + self.s_gain*EPS_D
# Populate msg with epsilon_final
self.ref_sig.header.stamp = rospy.Time.now()
@ -342,15 +352,18 @@ class Main:
#self.ref_sig.velocity.z = self.EPS_F[5]
#self.ref_sig.acceleration.z = self.EPS_F[8]
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.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.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.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 """
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]
@ -367,13 +380,14 @@ class Main:
def screen_output(self):
# Feedback to user
rospy.loginfo(' Var | x | y | z ')