forked from cesar.alejandro/oscillation_ctrl
More improvements to controller
This commit is contained in:
parent
316c8a7855
commit
469accc8dc
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
@ -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}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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)"/>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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 ')
|
||||
rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
|
||||
rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
|
||||
rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
|
||||
rospy.loginfo('_______________________')
|
||||
|
||||
#rospy.loginfo('xd = %.2f',self.xd.x)
|
||||
# rospy.loginfo(' Var | x | y | z ')
|
||||
# rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
|
||||
# rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
|
||||
# rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
|
||||
# rospy.loginfo('_______________________')
|
||||
rospy.loginfo_once('Tether length: %.2f',self.tetherL)
|
||||
# rospy.loginfo('Theta: %.2f',self.load_angles.theta)
|
||||
# rospy.loginfo('Phi: %.2f',self.load_angles.phi)
|
||||
|
||||
def publisher(self,pub_tim):
|
||||
|
||||
|
|
@ -382,18 +396,16 @@ class Main:
|
|||
|
||||
# Publish reference signal
|
||||
self.pub_path.publish(self.ref_sig)
|
||||
#self.pub_ref.publish(self.ref_sig) # for geometric controller
|
||||
|
||||
# Give user feedback on published message:
|
||||
#self.screen_output()
|
||||
self.screen_output()
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
rospy.init_node('desPath_node',anonymous=True)
|
||||
try:
|
||||
Main() # create class object
|
||||
Main() # create class object
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
|
|
@ -31,11 +31,17 @@ class Main:
|
|||
self.xd.y = self.wpoints['y']
|
||||
self.xd.z = self.wpoints['z']
|
||||
self.param_exists = True
|
||||
elif rospy.has_param('mocap/waypoints'):
|
||||
self.wpoints = rospy.get_param('mocap/waypoints') # get waypoints
|
||||
self.xd.x = self.wpoints['x']
|
||||
self.xd.y = self.wpoints['y']
|
||||
self.xd.z = self.wpoints['z']
|
||||
self.param_exists = True
|
||||
|
||||
elif rospy.get_time() - self.tstart >= 3.0:
|
||||
elif rospy.get_time() - self.tstart >= 5.0:
|
||||
self.xd.x = 0.0
|
||||
self.xd.y = 0.0
|
||||
self.xd.z = 5.0
|
||||
self.xd.z = 1.5
|
||||
break
|
||||
|
||||
# service(s)
|
||||
|
|
|
|||
Loading…
Reference in New Issue