Up to date

.gitignore

@@ -1,17 +1,26 @@
-config/mocap*
+*.rviz
+setup.txt
+px4_setup/protobuf_install.txt
+config/mocapGazebo_params.yaml
+config/mocapLab_params.yaml
 launch/cortex_bridge.launch
+launch/development_*
+launch/esp_*
 launch/headless_spiri_mocap.launch
 launch/headless_spiri_with_tether_mocap.launch
 launch/mocap_*
+launch/replay.launch
+msg/Marker.msg
+msg/Markers.msg
+srv/BodyToWorld.srv
 src/development_*
 src/killswitch_client.py
 src/land_client.py
 src/MoCap_*.py
 src/Mocap_*.py
 src/mocap_*
-msg/Marker.msg
-msg/Markers.msg
-*.rviz
-setup.txt
-px4_setup/protobuf_install.txt
+src/segmented_tether.py
+src/segmented_tether_fast.py
+
+
 

CMakeLists.txt

@@ -26,12 +26,15 @@ add_message_files(
   RefSignal.msg
   EulerAngles.msg
   LoadAngles.msg
+  LoadStatus.msg
 )
 
 ## Generate services in the 'srv' folder
  add_service_files(
    FILES
    WaypointTrack.srv
+   BodyToWorld.srv
+   UseCtrl.srv
  )
 
 ## Generate actions in the 'action' folder

README.md

@@ -2,8 +2,6 @@
 
 Repo containing oscillation damping controller for tether missions + instructions how to to set up
 
-WARNING: Currently repo is only for Ubuntu 18.04 and ROS Melodic, work is being made to be upgrade to Ubuntu 20.04 and ROS Noetic
-
 Cesar Rodriguez 
 
 cesar.rodriguez@spirirobotics.com
@@ -12,69 +10,13 @@ February 2022
 
 Steps to recreate stable PX4 environment + working repo
 
+### WARNING
+
+Currently repo is only for Ubuntu 18.04 and ROS Melodic, work is being made to be upgraded to Ubuntu 20.04 and ROS Noetic
+
 # Setup
 
-## 1) Installing ROS Melodic
-
-### Setup sources.list
-	sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
-### Setup keys
-    sudo apt install curl # if you haven't already installed curl
-    curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
-### Installation
-	sudo apt update
-	sudo apt install ros-melodic-desktop-full
-### Environment setup
-	echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
-### Dependencies
-	sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
-
-Initilize rosdep:
-
-	sudo apt install python-rosdep
-	sudo rosdep init
-	rosdep update
-### PX4 Dependencies
-	sudo apt-get install python-catkin-tools python-rosinstall-generator -y
-	wstool init ~/catkin_ws/src
-### MAVLINK
-	rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall
-### MAVROS
-	rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
-### Create workspace and deps
-	cd ~/catkin_ws
-	wstool merge -t src /tmp/mavros.rosinstall
-	wstool update -t src -j4
-	rosdep install --from-paths src --ignore-src -y
-### Install geographic datasets
-	cd ~/catkin_ws
-	sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
-### Build source
-	cd ~/catkin_ws	
-	catkin build
-	
-## 2) PX4 Environment Development
-Install PX4 firmware and ROS melodic dependencies:
-
-	git clone https://github.com/PX4/PX4-Autopilot.git --recursive
-	git checkout 601c588294973caf105b79a23f7587c6b991bb05
-	bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
-
-
-Install [PX4 Firmware](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html#rosgazebo). Then:
-
-#### Download QGroundControl:
-cd to desired directory to install QGroundControl. For example, Desktop
-
-	cd ~/<desired_directory>
-	wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
-	chmod +x QGroundControl.AppImage
-
-#### Build Gazebo Sim
-	cd ~/PX4-Autopilot
-	make px4_sitl gazebo
-
-## 3) Set up oscillation_ctrl
+## 1) Set up oscillation_ctrl
 ### Install xterm
 	sudo apt-get update -y
 	sudo apt-get install -y xterm
@@ -83,44 +25,64 @@ cd to desired directory to install QGroundControl. For example, Desktop
 	cd ~/catkin_ws/src
 	git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git
 
-To get the specific version of mavros_msgs for oscillation_ctrl:
+### Run bash script and build
+	bash /.oscillation_ctrl/px4_setup/ubuntu_sim_ros_melodic.sh
+This bash script is taken from PX4 Firmware but sets certain versions of dependencies to work with oscillation_ctrl. It also installs common ROS dependencies, ROS Melodic, Gazebo 9, and MAVROS. It should build oscillation_ctrl  and add the source path to your .bashrc. If it does not, then run:
 
-	cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/rosinstall.txt /tmp/mavros.rosinstall
-	wstool merge -t src /tmp/mavros.rosinstall
-	wstool update -t src -j4
-	rosdep install --from-paths src --ignore-src -y
+	cd ~/catkin_ws
 	catkin build
+	source devel/setup.bash
 
-#### Now, we finish building the PX4 package
+If you do not want to source every time, add  'source ~/catkin_ws/devel/setup.bash' to your .bashrc
+	
+## 2) PX4 Environment Development
+Install PX4 firmware and dependencies:
+
+	git clone https://github.com/PX4/PX4-Autopilot.git --recursive
+	git checkout 601c588294973caf105b79a23f7587c6b991bb05
+	bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
+
+There will be prompts on the screen, hit 'u' and 'enter' as they come up.
+
+### Add models, world, and airframe files
+To add the necessary Spiri Mu files to PX4, run:
+
+	cd ca
+	bash /.oscillation_ctrl/px4_setup/px4_bash.sh
+
+### Add airframes to cmake targets
+Add 'spiri' and 'spiri_with_tether' airframe names in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ under set(models... 
+
+__do not add their airframe number! e.i. 4000 or 4001__
+
+### Download QGroundControl:
+cd to desired directory to install QGroundControl. For example, Desktop.
+
+	cd ~/<example_Desktop>
+	wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
+	chmod +x QGroundControl.AppImage
+
+### Finish Building PX4 package
 	cd ~/PX4-Autopilot
 	DONT_RUN=1 make px4_sitl_default gazebo
+
+If if fails and you see
+
+	gzerver: symbol lookup error...
+
+then run:
+
+	sudo apt upgrade libignition-math2
+
+If all is good, run:
+
 	source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
 	export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
 	export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
 	roslaunch px4 posix_sitl.launch
 
-If this works, we can move on.
+PX4 should launch with MAVROS
 
-### Add files to tools/sitl_gazebo
-copy (or add) files in _oscillation_ctrl/models_ and _oscillation_ctrl/worlds_ to _PX4-Autopilot/Tools/sitl_gazebo/models_ and _PX4-Autopilot/Tools/sitl_gazebo/worlds_ respectively
-
-	cp -R ~/catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
-	cp -R ~/catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
-
-### Add files to ROMFS/px4mu_common
-copy (or add) files in _oscillation_ctrl/airframes_ to _PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes_
-
-	cp -R ~/catkin_ws/src/oscillation_ctrl/airframes/18.04/* ~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
-
-add model names to _CmakeLists.txt_ in same 'airframe' folder (with number... 4000_spiri and 4001_spiri_with_tether)
-
-add airframe name in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ (no number!)
-### Add necessary launch files
-this should not be a necessary step and will be changed in the future
-
-copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch'
-
-	cp -R ~/catkin_ws/src/oscillation_ctrl/px4_launch/* ~/PX4-Autopilot/launch
 #### Change devel/setup.bash
 In catkin_ws (or any working directory) add to devel/setup.bash:
 
@@ -144,23 +106,13 @@ Finally, source your setup.bash file
 - First two elements can be changed to tweak tether parameters
 	- number_elements: number of segments tether will be composed of
 	- tl: segment length (should be no shorter than 0.3 meters)
-- __IMPORTANT:__ in order for jinja file to work, the following needs to be added to the _CMakeLists.txt_ in the _~/PX4-Autopilot/Tools/sitl_gazebo_ folder. This creates a custom command to build the tether file whenever a change is done to it.
 
-		COMMAND
-			${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py models/spiri_with_tether/spiri_with_tether.sdf} ${CMAKE_CURRENT_SOURCE_DIR}
-		DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py models/spiri_with_tether/spiri_with_tether.sdf
-		VERBATIM
-
-_oscillation_ctrl/px4_setup/CMakeLists.txt has these changes therefore all that is needed is: 
-
-	cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/CMakeLists.txt ~/PX4-Autopilot/Tools/sitl_gazebo/
-
-# oscillation_ctrl info
+# Oscillation_ctrl Info
 Info pertaining to oscillation_ctrl repo such as what the ROS nodes do, different ROS parameters, etc.
 
 ## ROS Nodes
 ### LinkState.py 
-determines payload load angles and their rates (theta and phi) using Gazebo (needs to be made more robust), as well as determines tether length and keeps track of variables needed in case of a step or square test.
+Determines payload load angles and their rates (theta and phi) using Gazebo (needs to be made more robust), as well as determines tether length and keeps track of variables needed in case of a step or square test.
 
 __Publishes to__:
 
@@ -182,7 +134,7 @@ __Subscribes to__:
 	/reference/waypoints 
 
 ### ref_signalGen.py
-takes in desired position (xd) and determines smooth path trajectory.
+Takes in desired position (xd) and determines smooth path trajectory.
 
 __Publishes to__:
 
@@ -198,7 +150,7 @@ __Subscribes to__:
 	/reference/waypoints
 
 ### klausen_control.py
-determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands.
+Determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands.
 
 __Publishes to__:
 
@@ -213,9 +165,9 @@ __Subscribes to__:
 	/mavros/imu/data
 
 ### set_ploadmass.py
-sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_
+Sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_
 ### path_follow.cpp
-sets the vehicle in OFFBOARD mode (PX4) and takes off to a set height for 25 seconds before starting to publish attitude and thrust commands.
+Sets the vehicle in OFFBOARD mode (PX4) and takes off to a set height for 25 seconds before starting to publish attitude and thrust commands.
 
 __Publishes to__:
 
@@ -262,3 +214,19 @@ __pload_mass:__ sets the payload mass in simulation without having to change the
 
 ## Frequent Issues
 Will populate this section with frequently faces issues
+
+### No header files error:
+In oscillation_ctrl/CMakeLists.txt, change:
+
+	add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
+
+to:
+
+	add_dependencies(pathFollow_node oscillation_ctrl_generate_messages_cpp)
+
+### Cannot build px4_sitl
+Normally, this is solved by running:
+
+	make clean
+
+And redo command you were trying to run. If problem persists, rerun ubuntu_sim_ros_melodic.sh and ubuntu.sh and try again.

build/.built_by

@@ -0,0 +1 @@
+catkin build

config/mocapGazebo_params.yaml

@@ -1,14 +1,27 @@
 # Ros param when using Klausen Ctrl
-wait_time: 30 # 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_mass: 0.1  # mass of payload. Needs to be changed in spiri_with_tether file as well 
+
+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.10   # Pload mass with 50g weight
 #pload_mass: 0.05   # Pload mass with just basket
 #pload_mass: 0.25
-use_ctrl: false   # starts PX4 without attitude controller 
-waypoints: {x:  0.0, y:  0.0, z:  5.0} # takeoff waypoints
+change_mode: true # choose whether to switch to oscillation damping controller 
+#waypoints: {x:  -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"

config/mocapLab_params.yaml

@@ -1,27 +1,62 @@
 # 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
 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.05   # Pload mass with just basket
-#pload_mass: 0.25
+# 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 
+# use_ctrl: false   # starts PX4 without attitude controller 
+change_mode: true    # choose whether to switch to oscillation damping controller 
 
-waypoints: {x:  0.0, y:  0.0, z:  1.75} # 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.28 # Hover throttle with pload 0.10 kg
-# hover_throttle: 0.22 # Hover throttle with no pload
-#hover_throttle: 0.26 # Hover throttle with pload 0.05 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
+

config/mocapPendulum_params.yaml

@@ -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) 
+ 

config/spiri_params.yaml

@@ -2,6 +2,7 @@
 wait_time: 30     # parameter which can be set to run desired tests at a desired time
 drone_mass: 1.437 # mass of drone
 pload_mass: 0.25  # mass of payload
+pload: false
 use_ctrl: false   # starts PX4 without attitude controller - needs to be set to false to takeoff 
 waypoints: {x:  0.0, y:  0.0, z:  5.0} # takeoff waypoints
 # sets waypoints to run a square test

devel/.built_by

@@ -0,0 +1 @@
+catkin build

launch/headless_spiri_mocap.launch

@@ -1,54 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- MAVROS posix SITL environment launch script -->
-    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
-    <!-- vehicle pose -->
-    <arg name="x" default="0"/>
-    <arg name="y" default="0"/>
-    <arg name="z" default="0"/>
-    <arg name="R" default="0"/>
-    <arg name="P" default="0"/>
-    <arg name="Y" default="0"/>
-    <!-- vehicle model and world -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="spiri_mocap"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
-    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
-
-    <!-- gazebo configs -->
-    <arg name="gui" default="false"/>
-    <arg name="debug" default="false"/>
-    <arg name="verbose" default="false"/>
-    <arg name="paused" default="false"/>
-    <arg name="respawn_gazebo" default="false"/>
-    <!-- MAVROS configs -->
-    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
-    <arg name="respawn_mavros" default="false"/>
-    <!-- PX4 configs -->
-    <arg name="interactive" default="true"/>
-    <!-- PX4 SITL and Gazebo -->
-    <include file="$(find px4)/launch/posix_sitl.launch">
-        <arg name="x" value="$(arg x)"/>
-        <arg name="y" value="$(arg y)"/>
-        <arg name="z" value="$(arg z)"/>
-        <arg name="R" value="$(arg R)"/>
-        <arg name="P" value="$(arg P)"/>
-        <arg name="Y" value="$(arg Y)"/>
-        <arg name="world" value="$(arg world)"/>
-        <arg name="vehicle" value="$(arg vehicle)"/>
-        <arg name="sdf" value="$(arg sdf)"/>
-        <arg name="gui" value="$(arg gui)"/>
-        <arg name="interactive" value="$(arg interactive)"/>
-        <arg name="debug" value="$(arg debug)"/>
-        <arg name="verbose" value="$(arg verbose)"/>
-        <arg name="paused" value="$(arg paused)"/>
-        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
-    </include>
-    <!-- MAVROS -->
-    <include file="$(find mavros)/launch/px4.launch">
-        <!-- GCS link is provided by SITL -->
-        <arg name="gcs_url" value=""/>
-        <arg name="fcu_url" value="$(arg fcu_url)"/>
-        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
-    </include>
-</launch>

launch/headless_spiri_with_tether_mocap.launch

@@ -1,54 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- MAVROS posix SITL environment launch script -->
-    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
-    <!-- vehicle pose -->
-    <arg name="x" default="0"/>
-    <arg name="y" default="0"/>
-    <arg name="z" default="0"/>
-    <arg name="R" default="0"/>
-    <arg name="P" default="0"/>
-    <arg name="Y" default="0"/>
-    <!-- vehicle model and world -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="spiri_with_tether"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
-    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
-
-    <!-- gazebo configs -->
-    <arg name="gui" default="false"/>
-    <arg name="debug" default="false"/>
-    <arg name="verbose" default="false"/>
-    <arg name="paused" default="false"/>
-    <arg name="respawn_gazebo" default="false"/>
-    <!-- MAVROS configs -->
-    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
-    <arg name="respawn_mavros" default="false"/>
-    <!-- PX4 configs -->
-    <arg name="interactive" default="true"/>
-    <!-- PX4 SITL and Gazebo -->
-    <include file="$(find px4)/launch/posix_sitl.launch">
-        <arg name="x" value="$(arg x)"/>
-        <arg name="y" value="$(arg y)"/>
-        <arg name="z" value="$(arg z)"/>
-        <arg name="R" value="$(arg R)"/>
-        <arg name="P" value="$(arg P)"/>
-        <arg name="Y" value="$(arg Y)"/>
-        <arg name="world" value="$(arg world)"/>
-        <arg name="vehicle" value="$(arg vehicle)"/>
-        <arg name="sdf" value="$(arg sdf)"/>
-        <arg name="gui" value="$(arg gui)"/>
-        <arg name="interactive" value="$(arg interactive)"/>
-        <arg name="debug" value="$(arg debug)"/>
-        <arg name="verbose" value="$(arg verbose)"/>
-        <arg name="paused" value="$(arg paused)"/>
-        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
-    </include>
-    <!-- MAVROS -->
-    <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)"/>
-        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
-    </include>
-</launch>

launch/mocap_oscillation_damp.launch

@@ -1,88 +0,0 @@
-<?xml version="1.0"?>
-<!--
-Launch file to use klausen oscillaton damping ctrl in Gazebo
-/-->
-<launch>
-  <arg name='test' default="none"/>
-  <arg name="mav_name" default="spiri"/>
-  <arg name="command_input" default="1" />
-  <arg name="log_output" default="screen" />
-  <arg name="fcu_protocol" default="v2.0" />
-  <arg name="respawn_mavros" default="false" />
-  <arg name="gazebo_gui" default="false" />
-  <arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/>
-  <arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
-  <arg name="connection_type" default="wifi"/>
-
-  <group ns="status">
-		<rosparam file="$(find oscillation_ctrl)/config/mocapLab_params.yaml" />
-  	<param name="test_type" value="$(arg test)"/>
-  </group>
-
-	<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
-	<node
-		pkg="oscillation_ctrl"
-		type="Mocap_Bridge.py"
-		name="localize_node"
-		launch-prefix="xterm -e"
-	/>
-	<!-- DETERMINES DESIRED POSITION -->
-	<node
-		pkg="oscillation_ctrl"
-		type="wpoint_tracker.py"
-		name="waypoints_server"
-	/>
-	<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
-	<node
-		pkg="oscillation_ctrl"
-		type="ref_signalGen.py"
-		name="refSignal_node"
-		launch-prefix="xterm -e"
-	/>
-	<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
-	<node
-		pkg="oscillation_ctrl"
-		type="klausen_control.py"
-		name="klausenCtrl_node"
-		launch-prefix="xterm -e"
-		output='screen'
-	/>
-	<!-- PUBLISHES DESIRED COMMANDS -->
-	<node
-		pkg="oscillation_ctrl"
-		type="mocap_pathFollow_node"
-		name="mocap_pathFollow_node"
-		launch-prefix="xterm -e"
-	/>
-	<!-- RUNS DIFFERENT TESTS IF DESIRED -->
-	<group if="$(eval test != 'none')">
-		<node
-			pkg="oscillation_ctrl"
-			type="perform_test.py"
-			name="test_node"
-			launch-prefix="xterm -e"
-		/>
-	</group>
-
-	<!-- RUNS TEST -->
-	<node
-		pkg="oscillation_ctrl"
-		type="mocap_runTest.py"
-		name="mocap_Test"
-		launch-prefix="xterm -e"
-	/>
-
-	<!-- Cortex bridge launch  -->
-	<include file="$(find cortex_bridge)/launch/cortex_bridge.launch">
-		<!--param name="local_ip" value="$(param local_ip)" /-->
-	</include>
-
-	<!-- MAVROS launch  -->
-	<include file="$(find mavros)/launch/px4.launch">
-		<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
-		<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
-  		<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
-		<arg name="fcu_url" value="$(arg fcu_url)" />
-		<arg name="gcs_url" value="$(arg gcs_url)" />
-	</include>
-</launch>

launch/mocap_sim.launch

@@ -1,64 +0,0 @@
-<?xml version="1.0"?>
-<!--
-Launch file to use klausen oscillaton damping ctrl in Gazebo
-/-->
-<launch>
-  <group ns="status">
-	<rosparam file="$(find oscillation_ctrl)/config/mocapGazebo_params.yaml" />
-  </group>
-  <arg name="model" default="headless_spiri"/>
-	<node
-		pkg="oscillation_ctrl"
-		type="MoCap_Localization_fake.py"
-		name="fakeMocap_node"
-		launch-prefix="xterm -e"
-	/>
-	<!-- DETERMINES DESIRED POSITION -->
-	<node
-		pkg="oscillation_ctrl"
-		type="wpoint_tracker.py"
-		name="waypoints_server"
-	/>
-	<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
-	<node
-		pkg="oscillation_ctrl"
-		type="ref_signalGen.py"
-		name="refSignal_node"
-	/>
-	<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
-	<node
-		pkg="oscillation_ctrl"
-		type="klausen_control.py"
-		name="klausenCtrl_node"
-		launch-prefix="xterm -e"
-	/>
-	<!-- PUBLISHES DESIRED COMMANDS -->
-	<node
-		pkg="oscillation_ctrl"
-		type="mocap_pathFollow_node"
-		name="mocap_pathFollow_node"
-		launch-prefix="xterm -e"
-	/>
-	<!-- RUNS TEST -->
-	<node
-		pkg="oscillation_ctrl"
-		type="mocap_runTest.py"
-		name="mocap_Test"
-		launch-prefix="xterm -e"
-	/>
-	<!-- SETS PLOAD MASS -->
-	<node
-		pkg="oscillation_ctrl"
-		type="set_ploadmass.py"
-		name="set_ploadmass"
-		output="screen"
-	/>
-	<!-- PX4 LAUNCH -->
-	<include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
-	</include>
-	
-	<!-- Tf LAUNCH -->
-	<node pkg="tf" type="static_transform_publisher" name="map_to_pload_imu" args="0 0 0 0 0 0 map pload_imu 10" />
-	<node pkg="tf" type="static_transform_publisher" name="map_to_pload" args="0 0 0 0 0 0 map payload 10" />
-</launch>
-

launch/oscillation_damp.launch

@@ -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"
@@ -35,6 +36,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 		pkg="oscillation_ctrl"
 		type="klausen_control.py"
 		name="klausenCtrl_node"
+		clear_params="true"
 	/>
 	<!-- PUBLISHES DESIRED COMMANDS -->
 	<node
@@ -60,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>

msg/LoadStatus.msg

@@ -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
+

px4_setup/protobuf_install.txt

@@ -1,20 +0,0 @@
-sudo apt-get install libprotobuf-dev=3.0.0-9.1ubuntu1 libprotoc-dev=3.0.0-9.1ubuntu1 protobuf-compiler=3.0.0-9.1ubuntu1
-
-commit: 601c588294973caf105b79a23f7587c6b991bb05
-
-Qground control: 
-wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
-chmod +x QGroundControl.AppImage
-
-mavros/mavlink:
-cp -R ~/catkin_ws/src/oscillation_ctrl/bash_scripts/rosinstall.txt /tmp/mavros.rosinstall
-wstool merge -t src /tmp/mavros.rosinstall
-wstool update -t src -j4
-rosdep install --from-paths src --ignore-src -y
-catkin build
-
-steps:
-wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
-bash ubuntu_sim_ros_melodic.sh
-git clone https://github.com/PX4/PX4-Autopilot.git --recursive
-git checkout 601c588294973caf105b79a23f7587c6b991bb05

src/LinkState.py

@@ -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
 

src/MoCap_Localization_fake.py

@@ -1,243 +0,0 @@
-#!/usr/bin/env python2.7
-
-### Cesar Rodriguez Mar 2022
-### Script to simulate mocap readings and see how PX4 behaves
-
-import rospy,  tf
-import rosservice
-import time
-import math
-import random
-from tf.transformations import *
-from oscillation_ctrl.msg import TetheredStatus, LoadAngles
-from geometry_msgs.msg import PoseStamped
-from gazebo_msgs.srv import GetLinkState
-from std_msgs.msg import Bool
-
-class Main:
-
-	def __init__(self):
-
-		# rate(s)
-		pub_rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
-		loc_rate = 60 # rate we want to localize vehicle -- 60 Hz
-		self.dt = 1.0/loc_rate
-
-		self.user_fback = True
-
-		rospy.sleep(5) # Sleep for 5 sec. Need to give time to Gazebo to run
-
-		# 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.tetherL	= 0.0 # Tether length
-		self.has_run	= False		# Boolean to keep track of first run instance
-		self.phibuf	  =	0.0 # Need buffers to determine their rates
-		self.thetabuf = 0.0 #
-		self.pload		= False # Check if payload exists
-		# Max dot values to prevent 'blowup'
-		self.angledot_max   = 2.0
-		self.drone_eul = [0.0,0.0,0.0] 
-
-		# variables for message gen
-		#self.buff_pose1  = PoseStamped()
-		self.drone_pose     = PoseStamped()
-		self.pload_pose 	= PoseStamped()
-		self.load_angles    = LoadAngles()
-		self.twobody_status = TetheredStatus()
-		self.drone_id	  = 'spiri_with_tether::spiri::base'
-		self.pload_id	  = 'spiri_with_tether::mass::payload'
-
-		# service(s)
-		self.service1 = '/gazebo/get_link_state'
-		self.service2 = '/gazebo/set_link_properties'
-
-
-		# need service list to check if models have spawned
-		self.service_list = rosservice.get_service_list()
-
-		# wait for service to exist
-		while self.service1 not in self.service_list:
-			print ("Waiting for models to spawn...")
-			self.service_list = rosservice.get_service_list()
-			if rospy.get_time() - self.tstart >= 10.0:
-				break
-
-		# publisher(s)
-		self.twobody_pub = rospy.Publisher('/status/twoBody_status', TetheredStatus, queue_size=1)
-		self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1)
-		#self.pub_wd	 	 = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
-
-		#self.pub_timer  = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
-		#self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
-
-		### Since there is no tether, we can publish directly to mavros
-		self.visionPose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
-		
-		self.loc_timer  = rospy.Timer(rospy.Duration(1.0/loc_rate), self.mocap_localize)
-		self.pub_timer  = rospy.Timer(rospy.Duration(1.0/pub_rate), self.publisher)
-
-		# subscriber(s)
-
-	def euler_array(self,orientation):
-		"""
-		Takes in pose msg object and outputs array of euler angs:
-			eul[0] = Roll
-			eul[1] = Pitch
-			eul[2] = Yaw
-		"""
-		eul = euler_from_quaternion([orientation.x,
-															orientation.y,
-															orientation.z,
-															orientation.w])
-		return eul		
-	
-	def mocap_localize(self,loc_timer):
-		"""
-		Uses Gazebo to simulate MoCap 
-		"""
-		try:
-			# State which service we are querying
-			get_P = rospy.ServiceProxy(self.service1,GetLinkState)
-
-			# Set reference frame
-			reference	= '' # world ref frame
-
-			# Establish links needed --> Spiri base and payload
-			# P = Position vector
-			drone_P = get_P(self.drone_id,reference)
-
-			# Check if payload is part of simulation
-			if not drone_P.success:
-				self.drone_id	= 'spiri_mocap::base'
-				drone_P = get_P(self.drone_id,reference) # i.e. no payload
-
-			self.drone_P = drone_P
-			pload_P = get_P(self.pload_id,reference)
-			if pload_P.success: self.pload = True
-			if not self.has_run:
-				if self.pload == True:
-					# Get tether length based off initial displacement
-					self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - 
-												pload_P.link_state.pose.position.x)**2 +
-											 (drone_P.link_state.pose.position.y - 
-												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('status/tether_length',self.tetherL)
-
-				else:
-					self.tetherL = 0.0
-				self.has_run = True
-
-			# Need to detemine their location to get angle of deflection
-			# Drone
-			drone_Px = drone_P.link_state.pose.position.x
-			drone_Py = drone_P.link_state.pose.position.y
-			# Get drone orientation
-
-			if self.pload == True: # If there is payload, determine the variables
-				self.twobody_status.pload = True
-				# Pload
-				pload_Px = pload_P.link_state.pose.position.x
-				pload_Py = pload_P.link_state.pose.position.y	
-
-				# Determine theta (pitch)
-				x_sep = pload_Px - drone_Px
-
-				if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
-					self.load_angles.theta = 0
-				else:
-					self.load_angles.theta = math.asin(x_sep/self.tetherL)
-
-				# Determine thetadot
-				# self.load_angles.thetadot = min(self.angledot_max,max((self.load_angles.theta - self.thetabuf)/self.dt,-self.angledot_max))
-				self.load_angles.thetadot = (self.load_angles.theta - self.thetabuf)/self.dt
-				self.thetabuf = self.load_angles.theta
-			
-				# Determine phi (roll)
-				y_sep = pload_Py - drone_Py
-
-				if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
-					self.load_angles.phi = 0
-				else:
-					self.load_angles.phi = -math.asin(y_sep/self.tetherL)
-
-				# Determine phidot
-				# self.load_angles.phidot = min(self.angledot_max,max((self.load_angles.phi - self.phibuf)/self.dt,-self.angledot_max))
-				self.load_angles.phidot = (self.load_angles.phi - self.phibuf)/self.dt
-				self.phibuf = self.load_angles.phi	# Update buffer
-
-				# save pload position
-				self.twobody_status.pload_pos = pload_P.link_state.pose
-				self.pload_pose.pose = self.twobody_status.pload_pos
-			else: # Otherwise, vars = 0			
-				x_sep = self.load_angles.phi = self.load_angles.phidot = self.load_angles.theta = self.load_angles.thetadot = 0
-
-			# Populate message
-			#self.status.drone_pos = drone_P.link_state.pose
-			self.drone_pose.pose = drone_P.link_state.pose
-			self.twobody_status.drone_pos = drone_P.link_state.pose
-
-		except rospy.ServiceException as e:
-			rospy.loginfo("Get Link State call failed: {0}".format(e))
-
-	def add_noise(self):
-		# self.drone_pose.pose.position.x = self.drone_pose.pose.position.x
-		# self.drone_pose.pose.position.y = self.drone_pose.pose.position.y
-		# self.drone_pose.pose.position.z = self.drone_pose.pose.position.z
-		self.drone_pose.pose.orientation.x = self.drone_pose.pose.orientation.x + random.uniform(0,0.004)
-		self.drone_pose.pose.orientation.y = self.drone_pose.pose.orientation.y + random.uniform(0,0.004)
-		self.drone_pose.pose.orientation.z = self.drone_pose.pose.orientation.z + random.uniform(0,0.004)
-		self.drone_pose.pose.orientation.w = self.drone_pose.pose.orientation.w + random.uniform(0,0.004)
-	
-	def publisher(self,pub_timer):
-		# add noise to signal
-		self.add_noise()
-		# fill out necesssary fields
-		self.drone_pose.header.frame_id = "/map"
-		self.drone_pose.header.stamp = rospy.Time.now()
-		# load angles
-		self.load_angles.header.stamp = rospy.Time.now()
-		# twobody status
-		self.twobody_status.header.frame_id = "/map"
-		self.twobody_status.header.stamp = rospy.Time.now()
-		# publish
-		self.visionPose_pub.publish(self.drone_pose)  # publish pose to mavros
-		self.loadAng_pub.publish(self.load_angles)    # publish load angles to controller
-		self.twobody_pub.publish(self.twobody_status) # actual pose. Redundant but nice to have  
-		# get euler array for user feedback
-		self.drone_eul = self.euler_array(self.drone_pose.pose.orientation) 
-		self.user_feedback()
-
-	def user_feedback(self):
-		if self.user_fback:
-			print ("\n")
-			rospy.loginfo("")
-			print ("drone pos.x:    " + str(round(self.drone_pose.pose.position.x,2)))
-			print ("drone pos.y:    " + str(round(self.drone_pose.pose.position.y,2)))
-			print ("drone pos.z:    " + str(round(self.drone_pose.pose.position.z,2)))
-			print ("Roll:           " + str(round(self.drone_eul[0]*180/3.14,2)))
-			print ("Pitch:          " + str(round(self.drone_eul[1]*180/3.14,2)))
-			print ("Yaw:            " + str(round(self.drone_eul[2]*180/3.14,2)))
-			if self.pload:
-				print("Tether length:  " + str(round(self.tetherL,2)))
-				print("Theta:          " + str(round(self.load_angles.theta*180/3.14,2)))
-				print("Phi:            " + str(round(self.load_angles.phi*180/3.14,2)))
-		else:
-			rospy.loginfo_once(self.tetherL)
-			
-if __name__=="__main__":
-
-	# Initiate ROS node
-	rospy.init_node('MoCap_node',anonymous=False)
-	try:
-		Main() # create class object
-		rospy.spin() # loop until shutdown signal
-
-	except rospy.ROSInterruptException:
-	        pass
-

src/ctrl_tracker.py

@@ -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
+

src/klausen_control.py

@@ -20,30 +20,133 @@ from oscillation_ctrl.srv import WaypointTrack
 from geometry_msgs.msg 	  import Pose, Point, TwistStamped, PoseStamped
 from sensor_msgs.msg 	  import Imu
 from mavros_msgs.msg 	  import AttitudeTarget
+from collections          import deque
 
-class DesiredPoint():
+class DesiredPoint(Point):
 	def __init__(self,x,y,z):
 		self.x = x
 		self.y = y
 		self.z = z
 
+class LiveFilter:
+    """Base class for live filters.
+    """
+    def process(self, x):
+        # do not process NaNs
+        # if numpy.isnan(x):
+        #     return x
+
+        return self._process(x)
+
+    def __call__(self, x):
+        return self.process(x)
+
+    def _process(self, x):
+        raise NotImplementedError("Derived class must implement _process")
+
+class LiveLFilter(LiveFilter):
+    def __init__(self, b, a):
+        """Initialize live filter based on difference equation.
+
+        Args:
+            b (array-like): numerator coefficients obtained from scipy.
+            a (array-like): denominator coefficients obtained from scipy.
+            n (int): How many dimensions are we dealing with?
+        """
+        self.b = b
+        self.a = a
+        n = len(b)
+        # 3 because x,y,z
+        # self.n because that is how many terms we need
+        self._xs = deque(np.zeros(n), maxlen = n) # unfiltered data
+        self._ys = deque(np.zeros(n - 1), maxlen = n - 1) # filtered signal
+
+    def _process(self, x):
+        """
+        Filter incoming data with standard difference equations*:
+            a_0*y[n] = b_0*x[n] + b_1*x[n-1] + b_2*x[n-2] - a_1*y[n-1] - a_2*y[n-2]
+            *Changes depending on order of filter
+        """
+        # y = numpy.empty(3)
+        # self._xs.appendleft(x)
+        # # get every x val (idx = 0), then y val (idx=1), then z val (idx=2) to determine y
+        # for i in range(3):
+        #     last_x_vals = [val_x[i] for val_x in self._xs]
+        #     last_y_vals = [val_y[i] for val_y in self._ys]
+        #     y[i] = numpy.dot(self.b, last_x_vals) - numpy.dot(self.a[1:], last_y_vals)
+
+        # y = y / self.a[0]
+        # self._ys.appendleft(y)
+        
+        # return y
+
+        self._xs.appendleft(x)
+        y = np.dot(self.b, self._xs) - np.dot(self.a[1:], self._ys)
+        y = y / self.a[0]
+        self._ys.appendleft(y)
+
+        return y
+
+class 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):
 
 		# rate(s)
-		rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz #25
+		rate = 20 # rate for the publisher method, specified in Hz -- 50 Hz #25
 
 		# initialize variables
 		
 		# time variables
 		self.dt 	= 1.0/rate
 		self.tmax	= self.dt
-		self.n		= self.tmax/self.dt + 1
+		self.n		= self.tmax/self.dt + 9
 		self.t 		= np.linspace(0, self.tmax, self.n) # Time array
 		self.tstart	= rospy.get_time() # Keep track of the start time
 		while self.tstart == 0.0:				 # Need to make sure get_rostime works
-			self.tstart		= rospy.get_time()
+			self.tstart = rospy.get_time()
 		
 		# Msgs types
 		self.vel_data	 = TwistStamped() # This is needed to get drone vel from gps
@@ -55,14 +158,14 @@ class Main:
 		self.att_targ    = AttitudeTarget()	# used to send quaternion attitude commands
 		self.load_angles = LoadAngles()
 		self.EulerAng	 = [0,0,0] # Will find the euler angles, and then convert to q
-		self.EulerPose = [0,0,0]
+		self.EulerPose   = [0,0,0]
 
 		# Service var
 		self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)		
 		self.empty_point = Point() # Needed to query waypoint_server
 
 		# Drone var
-		self.has_run	 = 0 # Bool to keep track of first run instance
+		self.has_run	= 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]]) 
@@ -72,11 +175,15 @@ class Main:
 
 		# Tether var - Check if current method is used
 		# Get tether length
-		self.param_exists = False
-		self.tetherL = self.get_tether()
-		self.tether = True if self.tetherL > 0.01 else False
-		# Check if tether was correctly detected
-		self.tether_check()
+		if rospy.get_param('status/pload', False):
+			self.tetherL = self.get_tether()
+			if self.tetherL == 0.0:
+				self.tether = False
+			else:
+				self.tether = True
+		else: 
+			self.tetherL = 0.0
+			self.tether = False
 
 		# Retrieve drone and payload masses from config file
 		[self.drone_m, self.pl_m] = self.get_masses()
@@ -85,34 +192,51 @@ 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	  = self.alpha[3:5]
+		self.a45	  = np.zeros(2)
 		self.a45dot	  = np.array([[0],[0]])
 
 		# Error terms
 		self.z1		= np.zeros([3,1]) # dr_pos - ref_sig_pos
 		self.z2 	= np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha
+		self.z_sum  = np.zeros([3,1])
 	
 		# Tuning gains
-		self.K1	= np.identity(3)
-		self.K2	= np.identity(5)
-		self.tune	  = 0.1 # Tuning parameter
-		self.dist	  =	np.array([0,0,0,0.1,0.1]) # Wind disturbance 
+		self.K1	= np.identity(3)*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
-		self.Kp_thrust 	= 1.5 
-		self.Kd_thrust 	= 1.0
+		### Failed when both gains were set to 1.0
+		# 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()
 # --------------------------------------------------------------------------------#		
@@ -124,16 +248,38 @@ class Main:
 		rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
 		rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
 		rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
-		
 # --------------------------------------------------------------------------------#		
 # 									PUBLISHERS
 # --------------------------------------------------------------------------------#
-
-		self.pub_att_targ = rospy.Publisher('/command/att_target',AttitudeTarget,queue_size = 10)
+		self.pub_att_targ = rospy.Publisher('/command/att_target', AttitudeTarget, queue_size=10)
 
 		# timer(s), used to control method loop freq(s) as defined by the rate(s)
-		self.pub_time  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) #f this was 5.0 rate before
+		# 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
 
+		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
 # --------------------------------------------------------------------------------#
@@ -142,6 +288,7 @@ class Main:
 		""" Get tether length based on set parameters"""
 		param_exists = False
 		while param_exists == False:
+			rospy.loginfo('Waiting for tether length...')
 			if rospy.has_param('status/tether_length'):
 				tether_length	= rospy.get_param('status/tether_length') # Tether length
 				rospy.loginfo('TETHER LENGTH IN CONFG FILE')
@@ -167,6 +314,7 @@ class Main:
 		if self.tether:
 			param_exists = False
 			while param_exists == False:
+				rospy.loginfo('Waiting for pload mass...')
 				if rospy.has_param('status/pload_mass'):
 					pl_m	= rospy.get_param('status/pload_mass') # wait time
 					rospy.loginfo('PLOAD MASS FOUND')
@@ -183,19 +331,24 @@ class Main:
 		
 	def get_kf(self):
 		if rospy.has_param('status/hover_throttle'):
-			hover_throttle = rospy.get_param('status/hover_throttle')
-		else:
-			hover_throttle = (self.tot_m*9.81 + 11.2)/34.84 # linear scaling depending on dependent on mass
-			rospy.set_param('status/hover_throttle',hover_throttle)
-		kf = hover_throttle/(self.tot_m*9.81)
-		return kf
 
-	# Check if vehicle has tether
-	def tether_check(self):
-		if self.tether == True:
-			rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL)
+			rospy.loginfo('total mass: %.3f',self.tot_m)
+
+			hover_throttle = rospy.get_param('status/hover_throttle')
+			rospy.loginfo('Using hover throttle from config %.3f',hover_throttle)
+			self.max_throttle = 0.45
 		else:
-			rospy.loginfo_once('NO TETHER DETECTED')
+
+			rospy.loginfo('total mass: %.3f',self.tot_m)
+
+			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)
+			self.max_throttle = 0.8
+		kf = hover_throttle/(self.tot_m*9.81)
+		rospy.set_param('status/motor_constant',kf)
+		return kf
 
 # --------------------------------------------------------------------------------#		
 # 								CALLBACK FUNCTIONS
@@ -206,45 +359,54 @@ 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]])
-		except ValueError:
-			pass
+			# 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]]) # 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,:]))
+			# print('Ang_dot:\n{0}'.format(self.PHI[1,:]))
+
+		except ValueError as e:
+			rospy.loginfo('Load angles callback failed due to: {0}'.format(e))
 
 	# Callback drone pose
 	def dronePos_cb(self,msg): 
+		""" Subscribed to /mavros/local_position/pose, gets PoseStamped msgs """
 		try:
 			self.dr_pos = msg.pose
 			self.EulerPose = self.convert2eul(self.dr_pos.orientation)
 			# self.dr_pos = msg.drone_pos
-		except ValueError:
-			pass
+		except ValueError as e:
+			rospy.loginfo('Drone pos callback failed due to: {0}'.format(e))
 
-	# Callback for drone velocity ####### IS THIS ON? ##########
+	# Callback for drone velocity
 	def droneVel_cb(self,msg):
 		try:
-			self.dr_vel 	= np.array([[msg.twist.linear.x],[msg.twist.linear.y],[msg.twist.linear.z]])
+			self.dr_vel 	= np.array([msg.twist.linear.x,msg.twist.linear.y,msg.twist.linear.z]).reshape(3,1)
 			self.dr_angVel	= np.array([[msg.twist.angular.x],[msg.twist.angular.y],[msg.twist.angular.z]])
 
-		except ValueError or TypeError:
-			pass
+		except (ValueError or TypeError) as e:
+			rospy.loginfo('Drone vel callback failed due to: {0}'.format(e))
 
 	# Callback for drone accel from IMU data
 	def droneAcc_cb(self,msg):
 		try:
 			self.dr_acc = np.array([[msg.linear_acceleration.x],[msg.linear_acceleration.y],[msg.linear_acceleration.z]])
 
-		except ValueError:
-			pass
+		except ValueError as e:
+			rospy.loginfo('Drone accel. callback failed due to: {0}'.format(e))
 
 	# Callback reference signal
 	def refsig_cb(self,msg):
 		try:
 			self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]])
 			self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]])
-			self.path_acc =	np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO
+			self.path_acc =	np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]])
 
-		except ValueError:
-			pass
+		except ValueError as e:
+			rospy.loginfo('Reference signal callback failed due to: {0}'.format(e))
 
 	def waypoints_srv_cb(self):
 		if '/status/waypoint_tracker' in self.service_list:
@@ -252,20 +414,23 @@ 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):
 		# Need the statespace array:
-		a1,a2 = y
-		K = np.dot(Ka,[[a1],[a2]]) + Kb
+		_y = np.array(y).reshape(2,1)
+		# print('Ka:\n{0}\nKb:\n{1}'.format(Ka, Kb))		
+
+		K = np.dot(Ka,_y) + Kb # Kb should be 2x1
 
 		# Derivative of statespace array:
-		dydt = np.dot(Kc,K)		
-		dydt = [dydt[0][0], dydt[1][0]]	# og dydt is list of arrays, need list of floats	
+		dydt = np.dot(Kc,K)
+		# print('dydt:\n{0}'.format(dydt))		
+		dydt = [dydt[0][0], dydt[1][0]]
 		
 		return dydt
 # --------------------------------------------------------------------------------#
@@ -314,7 +479,7 @@ class Main:
 		Convers quaternion in pose message into euler angles
 
 		Input
-		:param Q: orientatiom pose message
+		:param Q: orientation pose message
 
 		Output
 		:return:  Array of euler angles
@@ -337,29 +502,88 @@ 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):
-		# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
-		# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
-		self.waypoints_srv_cb()
-		self.R = self.quaternion_rotation_matrix()
-		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)
+	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.error_vel = self.path_vel - self.R.dot(self.dr_vel)
+		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))
 
-		# determine Rotation Matrix
-		self.R_e3 =  np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
+		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)
 
-		thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf
-		thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
+		self.error_vel = self.path_vel.reshape(3,1) - self.dr_vel # best performance
+
+		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(thrust,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]))
+		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))
+
+		# 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"""
@@ -378,16 +602,17 @@ class Main:
 		M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta],
 			 [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta],
 			 [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta],
-			 [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.01*s_theta**2, 0],
+			 [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.001*s_theta**2, 0],
 			 [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]]
 
 		C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta],
 			 [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta  + self.load_angles.thetadot*c_theta*s_phi)],
 			 [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)],
-			 [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)],
+			 [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.001*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)],
 			 [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]]
 
-		G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]]
+		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]]
 
@@ -407,90 +632,121 @@ class Main:
 		M_b  = M[3:5,:3] # M_4:5,1:3 - rows 4 to 5 and columns 1 to 3
 		M_c	 = M[:3,3:5] # M_1:3,4:5 - rows 1 to 3 and columns 4 to 5
 		C_c  = C[:3,3:5]
-
-		# Constants from Eq. 49
-		Ka = -(D_a + C_a + self.K2[3:5,3:5])
-		Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[:,1]) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel))
-		
+	
 		# Determine alpha
-		if self.tether: 
+		# if 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)) 
+			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))
 
 			# SOLVE ODE (get alpha)
-			# Populate buffer
-			self.a45_buff  = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI))
+			# Populate buffer 
+			self.a45_buff  = odeint(self.statespace,self.a45,self.t,args=(Ka,Kb,M_aI))
+			# self.a45_buff  = odeint(self.statespace,self.a45,self.t,args=(Ka,Kb,np.identity(2))) # spits out a 2,2 matrix
+			# print('a45_0 was found to be:\n{0}\na45 was found to be:\n{1}\n'.format(self.a45_0,self.a45))
+			# 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[-1,:]
-			self.a45   = np.array([[self.a45_0[0]],[self.a45_0[1]]])
+			# self.a45_0 = self.a45_buff[0,:]
+			self.a45 = self.a45_buff[-1,:]
+			# self.a45 = M_aI.dot(self.a45_buff[-1,:])
+
+			# print('a45_0 was found to be {0}'.format(self.a45_0))
 
 			# Get alphadot_4:5
-			self.a45dot = self.statespace(self.a45_0,1,Ka,Kb,M_aI) # Do not need 't' and that's why it is a 1
-			self.a45dot = np.array([[self.a45dot[0]],[self.a45dot[1]]])
+			self.a45dot = np.array(self.statespace(self.a45,1,Ka,Kb,M_aI)) # Do not need 't' and that's why it is a 1
 
 		else: # if no tether, alpha_4:5 = 0 
 			self.a45	= np.array([[0],[0]])
 			self.a45dot = np.array([[0],[0]])
 
-		# Determine a_1:3
-		self.alpha[:3] = self.path_vel - np.dot(self.K1,p - self.path_pos)
-		self.alpha[3:5] = self.a45
-
 		# populate error terms
-		self.z1 = p - self.path_pos
-		z1_dot	= self.dr_vel - self.path_vel
+		self.z1 = p.reshape(3,1) - self.path_pos.reshape(3,1)
+		# z1_dot	= self.dr_vel - self.path_vel
+		z1_dot	= self.R.dot(-self.path_vel) + self.dr_vel
 		self.z2 = g - self.alpha
 
-		B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot)
+		# Determine alpha
+		self.alpha[:3] = self.path_vel - np.dot(self.K1,self.z1)
+		self.alpha[3:5] = self.a45.reshape(2,1)
 
+		# determine stability factor
+		B = np.dot(C_c,self.a45.reshape(2,1)) + np.dot(M_c,self.a45dot.reshape(2,1))
 
 		dr_orientation     = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w]
 		dr_orientation_inv = quaternion_inverse(dr_orientation)
 		
+		if 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]]).reshape(3,1) # need to add gravity for controller to work
+
 		# Desired body-oriented forces
-		Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) -  np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p))
+		Fd = B + G[:3] + self.tot_m*path_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) -  np.dot(self.Ki,z_int)
+		# rospy.loginfo('\nFd:\n{0}'.format(Fd))
 		
 		# Update self.z1_p for integration		
 		self.z1_p = self.z1
+		self.z_sum = z_int
 
-		# Covert Fd into drone frame
-		Fd_tf = Fd
+		# Covert Fd into drone frame, do not need Fz since we have a seperate altitude controller  
+		# Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],-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, ignoring F_z since assuming acc_z = 0 
 
-		Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0
+		# rospy.loginfo('\nFd_tf:\n{0}'.format(Fd_tf))
 		
-		# Convert forces to attiude *EulerAng[2] = yaw = 0
-		self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch
-		self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll	
+		# 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
 		self.att_targ.header.stamp = rospy.Time.now()
 		self.att_targ.header.frame_id = '/map'
-		self.att_targ.type_mask = 1|2|4 
+		self.att_targ.type_mask = 1|2|4 # ignore body rate command
 		self.att_targ.orientation.x = q[0] 
 		self.att_targ.orientation.y = q[1]
 		self.att_targ.orientation.z = q[2]
 		self.att_targ.orientation.w = q[3]
-
-	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.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
+		# 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.determine_throttle()
 		self.pub_att_targ.publish(self.att_targ)
 
-# --------------------------------------------------------------------------------#		
-# 							ALGORITHM ENDS 
-# --------------------------------------------------------------------------------#
 if __name__=="__main__":
 
 	# Initiate ROS node

src/mocap_path_follow.cpp

@@ -1,221 +0,0 @@
-/**
- * @file path_follow.cpp
- * @brief Offboard path trajectory tracking
-*/
-
-#include <ros/ros.h>
-#include <sensor_msgs/NavSatFix.h>
-#include <oscillation_ctrl/RefSignal.h>
-#include <oscillation_ctrl/EulerAngles.h>
-#include <oscillation_ctrl/WaypointTrack.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Quaternion.h>
-#include <mavros_msgs/CommandBool.h>
-#include <mavros_msgs/SetMode.h>
-#include <mavros_msgs/State.h>
-#include <mavros_msgs/CommandTOL.h>
-#include <mavros_msgs/AttitudeTarget.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <iostream>
-#include <cmath> // for std::abs
-
-/********* CALLBACK FUNCTIONS **********************/
-
-// Callback function which will save the current state of the autopilot. 
-// Allows to check connection, arming, and Offboard tags*/
-mavros_msgs::State current_state;
-bool land = false;
-void state_cb(const mavros_msgs::State::ConstPtr& msg){
-    current_state = *msg;
-}
-
-// Cb to determine attitude target
-mavros_msgs::AttitudeTarget att_targ;
-void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
-    att_targ = *msg;
-}
-
-// Cb to recieve pose information
-geometry_msgs::PoseStamped buff_pose;
-geometry_msgs::Quaternion q_init;
-geometry_msgs::PoseStamped mavPose;
-bool gps_read = false;
-double current_altitude;
-void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
-	mavPose = *msg;
-	current_altitude = mavPose.pose.position.z;
-	while(gps_read == false){
-		q_init = mavPose.pose.orientation;
-		if(q_init.x == 0.0 && q_init.w == 0.0){
-			ROS_INFO("Waiting...");
-		} else {
-			buff_pose.pose.orientation.x = 0.0;
-			buff_pose.pose.orientation.y = 0.0;
-			buff_pose.pose.orientation.z = 0.0;
-			buff_pose.pose.orientation.w = 1.0;
-			gps_read = true;
-		}
-	}
-}
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "path_follow");
-    ros::NodeHandle nh;
-    
-    /********************** SUBSCRIBERS **********************/
-    // Get current state
-    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
-	("mavros/state", 10, state_cb);
-
-    // Get attitude target from klausen control
-    ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
-	("command/att_target",10,att_targ_cb);
-
-    // Pose subscriber
-    ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
-	("mavros/local_position/pose",10,mavPose_cb);
-
-    /********************** PUBLISHERS **********************/
-    // Initiate publisher to publish commanded local position
-    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
-	("mavros/setpoint_position/local", 10);
-
-    // Publish attitude and thrust commands
-    ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
-	("mavros/setpoint_raw/attitude",10);
-
-    // Service Clients
-    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
-	("mavros/cmd/arming");
-    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
-    ("mavros/set_mode");
-    ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
-	("mavros/cmd/takeoff");
-	ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
-	("status/waypoint_tracker");
-
-    //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
-    ros::Rate rate_wait(20.0);
-
-    // wait for FCU connection
-    while(ros::ok() && !current_state.connected){
-        ros::spinOnce();
-		ROS_INFO("Waiting for FCU connection");
-        rate_wait.sleep();
-    }
-
-	if (current_state.connected){
-		ROS_INFO("Connected to FCU");
-	} else {
-		ROS_INFO("Never Connected");
-	}
-   
-    /*********** Initiate variables ************************/
-	//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
-    ros::Rate rate_pub(25.0);
-
-    // Desired mode is offboard
-    mavros_msgs::SetMode offb_set_mode;
-    offb_set_mode.request.custom_mode = "OFFBOARD";
-
-    // Arm UAV
-    mavros_msgs::CommandBool arm_cmd;
-    arm_cmd.request.value = true;
-
-    // Take off command    
-    bool takeoff = false, att_cmds = false;
-	bool oscillation_damp = true;
-
-    // Keep track of time since requests
-    ros::Time tkoff_req = ros::Time::now();
-    ros::Time last_request = ros::Time::now();
-
-    //send a few setpoints before starting
-    for(int i = 100; ros::ok() && i > 0; --i){
-        local_pos_pub.publish(buff_pose);
-        ros::spinOnce();
-		ROS_INFO("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 
-		// 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;
-	} else {
-		ROS_INFO("NO waypoints received");
-	}
-	double alt_des = wpoint_srv.response.xd.z; // Desired height	
-    while(ros::ok()){
-		if(current_state.mode == "AUTO.LAND"){
-			land = true;
-				while(land == true){
-					ROS_INFO("Des Altitude: LANDING");
-				}
-		} else {
-			if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
-				if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
-				} else {
-					ROS_INFO("Could not enter offboard mode");
-				}
-				//last_request = ros::Time::now();
-			} else {
-				if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
-					if( arming_client.call(arm_cmd) && arm_cmd.response.success){
-						ROS_INFO("Vehicle armed");
-					}
-					last_request = ros::Time::now();
-				}
-			}
-			if(current_state.mode == "OFFBOARD" && current_state.armed){
-				ROS_INFO_ONCE("Spiri is taking off");
-				if(!takeoff){		
-					tkoff_req = ros::Time::now();
-					takeoff = true;
-				}
-			}
-			if (waypoint_cl.call(wpoint_srv)){
-			// check if waypoints have changed
-			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;
-			alt_des = wpoint_srv.response.xd.z;
-			}
-			// Check if we want to use oscillation controller
-			if (ros::param::has("/status/use_ctrl")){
-				ros::param::get("/status/use_ctrl", oscillation_damp);
-				if(oscillation_damp){
-					ROS_INFO("USING ATTITUDE CTRL");
-					att_targ.header.stamp = ros::Time::now();
-					att_targ_pub.publish(att_targ);
-				} else {
-				// Check if waypoints have changed
-				if (waypoint_cl.call(wpoint_srv))
-				{
-					// populate desired waypoints
-					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("USING POSITION CTRL");
-				}
-			}
-			ROS_INFO("Des Altitude: %.2f", alt_des);
-			ROS_INFO("Cur Altitude: %.2f", current_altitude);
-			ROS_INFO("---------------------------");				
-			ros::spinOnce();
-			rate_pub.sleep();
-		}
-    }
-
-    return 0;
-}

src/mocap_runTest.py

@@ -1,104 +0,0 @@
-#! /usr/bin/env python2.7
-# Cesar Rodriguez Aug 2022
-# Sets attitude mode as well as new waypoints
-
-import rospy
-import time
-
-from oscillation_ctrl.srv import WaypointTrack, WaypointTrackResponse
-from geometry_msgs.msg import Point
-
-class Main:
-    def __init__(self):
-
-        # 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()	
-
-        # set up client
-        self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
-
-        # Set up desired waypoints for test
-        self.xd1 = Point()
-        self.xd1.x =  0.0
-        self.xd1.y = -0.5
-        self.xd1.z =  1.75
-
-        self.xd2 = Point()
-        self.xd2.x =  0.0
-        self.xd2.y =  2.0
-        self.xd2.z =  1.75
-        
-        # Determine if we want to run test with or without controller
-        ################# CHANGE THIS TO CHANGE TYPE Of TEST ###############################
-
-        self.change_mode = True # True = Change to oscillation damping mode after wait time
-        self.multiple_setpoins = True # True - will send multiple setpoints
-                                                                                        
-        #####################################################################################
-        if self.change_mode: self.loginfo_string = 'Attitude mode in...' 
-        else: self.loginfo_string = 'Staying in Position mode.'
-
-        self.get_wait_time() # get wait time
-        self.run_test()      # runs the test
-
-    def get_wait_time(self):
-        """ Determine desired wait time based on ros params"""
-        self.param_exists = False
-        while self.param_exists == False:
-            rospy.loginfo_once('Getting wait time')
-            if rospy.has_param('status/wait_time'):
-                self.wait_time	= rospy.get_param('status/wait_time') # Tether length
-                self.param_exists = True
-                rospy.loginfo('Wait time: %.2f',self.wait_time)
-            elif rospy.get_time() - self.tstart >= 30:
-                break
-
-    def run_test(self):
-        """ Waits desired amount before setting UAV to appropriate mode, and then sets up desired waypoints"""
-        run_test = False
-        use_ctrl = False
-        waypoint_sent = False
-        while not run_test:
-            time_left = self.wait_time - (rospy.get_time() - self.tstart)
-            if not rospy.get_time() - self.tstart > self.wait_time:
-                rospy.loginfo(self.loginfo_string + ' %.2f',time_left)
-            elif rospy.get_time() - self.tstart >= self.wait_time and not use_ctrl:
-                rospy.set_param('status/use_ctrl',self.change_mode)
-                self.t_param = rospy.get_time()
-                use_ctrl = True
-            if use_ctrl:
-                time_until_test1 = 25.0 - rospy.get_time() + self.t_param
-                time_until_test2 = 30.0 - rospy.get_time() + self.t_param
-                if time_until_test1 >= 0.0 and not waypoint_sent:
-                    rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test1,self.xd1.x,self.xd1.y,self.xd1.z)
-                elif not waypoint_sent and time_until_test1 < 0.0:
-                    waypoint_sent = True
-                    self.set_waypoint(self.xd1)
-
-                if time_until_test2 >= 0.0 and waypoint_sent:
-                    rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test2,self.xd2.x,self.xd2.y,self.xd2.z)
-                elif waypoint_sent and time_until_test2 < 0.0:
-                    self.set_waypoint(self.xd2)
-                    run_test = True
-                    break
-    
-    def set_waypoint(self,xd):
-        """ Set waypoints for oscillation controller """
-        rospy.wait_for_service('/status/waypoint_tracker')
-        try:
-            self.get_xd(True,xd)
-        except ValueError:
-            pass
-
-if __name__=="__main__":
-
-	# Initiate ROS node
-	rospy.init_node('MoCap_node',anonymous=False)
-	try:
-		Main() # create class object
-		rospy.spin() # loop until shutdown signal
-
-	except rospy.ROSInterruptException:
-	        pass

src/path_follow.cpp

@@ -37,25 +37,11 @@ void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
 
 // Cb to recieve pose information
 geometry_msgs::PoseStamped buff_pose;
-geometry_msgs::Quaternion q_init;
 geometry_msgs::PoseStamped mavPose;
-bool gps_read = false;
 double current_altitude;
 void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
 	mavPose = *msg;
 	current_altitude = mavPose.pose.position.z;
-	// while(gps_read == false){
-	// 	q_init = mavPose.pose.orientation;
-	// 	if(q_init.x == 0.0 && q_init.w == 0.0){
-	// 		ROS_INFO("Waiting...");
-	// 	} else {
-	// 		buff_pose.pose.orientation.x = q_init.x;
-	// 		buff_pose.pose.orientation.y = q_init.y;
-	// 		buff_pose.pose.orientation.z = q_init.z;
-	// 		buff_pose.pose.orientation.w = q_init.w;
-	// 		gps_read = true;
-	// 	}
-	// }
 }
 
 int main(int argc, char **argv)
@@ -90,8 +76,6 @@ int main(int argc, char **argv)
 		("mavros/cmd/arming");
     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
     	("mavros/set_mode");
-    ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
-		("mavros/cmd/takeoff");
 	ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
 		("status/waypoint_tracker");
 

src/ref_signalGen.py

@@ -15,12 +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.x = x
-		self.y = y
-		self.z = z
-
 class Main:
 
 	def __init__(self):                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      
@@ -44,14 +38,17 @@ class Main:
 		self.ref_sig	 = RefSignal()	  # Smooth Signal
 		self.load_angles = LoadAngles()
 
-		self.has_run	= 0		# Bool to keep track of first run instance
 		self.dr_pos 	= Pose()
 		self.dr_vel		= self.vel_data.twist.linear
 		self.dr_acc		= self.imu_data.linear_acceleration
 	
-		self.tetherL = self.get_tether()		
+		if rospy.get_param('status/pload'):
+			self.tetherL = self.get_tether()
+		else: self.tetherL = 0.0
+		rospy.loginfo('Tether length: {0}'.format(self.tetherL))	
+
 # --------------------------------------------------------------------------------#		
-# 							SUBSCRIBERS 										  #
+# 								SUBSCRIBERS 									  #
 # --------------------------------------------------------------------------------#
 		# Topic, msg type, and class callback method
 		rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
@@ -78,24 +75,26 @@ class Main:
 		self.EPS_I 		= np.zeros(9)	# Epsilon shapefilter
 		
 		# Constants for smooth path generation
-		self.w_tune		= 1    # Increase this to increase aggresiveness of trajectory i.e. higher accelerations
-		self.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	= 7
+			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
 		self.k3	= ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4
 		self.k2	= (4*self.epsilon*self.w_tune**3)/(self.k4*self.k3)
-		self.k1	= (self.w_tune**4)/(self.k2*self.k3*self.k4)
+		self.k1	= (self.w_tune**4)/(self.k4*self.k3*self.k2)
 		
 		# For saturation:
-		self.max  =	[0, 3, 1.5, 3]  #[0, 5, 1.5, 3]
+		self.max  =	[0, 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
@@ -113,14 +112,20 @@ 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([4,int(round(self.t2/self.dt))]) # Shapefilter delay; 4 - p,v,a,j
-		self.SF_delay_y = np.zeros([4,int(round(self.t2/self.dt))])
+		self.SF_delay_x = np.zeros([3,int(round(self.t2/self.dt))]) # Shapefilter delay; 4 - p,v,a since we do not need j
+		self.SF_delay_y = np.zeros([3,int(round(self.t2/self.dt))])
 
 		self.phi_fb     = np.zeros(int(round(self.td/self.dt))) # Feedback delay
 		self.phi_vel_fb = np.zeros(int(round(self.td/self.dt))) 
@@ -137,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
 # --------------------------------------------------------------------------------#
@@ -149,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
 
@@ -179,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"""
@@ -196,7 +212,7 @@ class Main:
 			if rospy.has_param('status/tether_length'):
 				tether_length	= rospy.get_param('status/tether_length') # Tether length
 				self.param_exists = True
-			elif rospy.get_time() - self.tstart >= 15:
+			elif rospy.get_time() - self.tstart >= 3:
 				tether_length = 0.0
 				break
 		return tether_length
@@ -214,11 +230,9 @@ class Main:
 		pos,vel,acc,jer = y
 
 		error = xd - pos
-		if abs(error) <= 0.01: error = 0.0
-
 		# Derivative of statesape array:		
-		dydt = [vel, acc, jer, 
-						self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)]
+		dydt = [vel, acc, jer, self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)]
+
 		return dydt
 
 	# Sigmoid
@@ -261,11 +275,11 @@ class Main:
 				# First, fill up the delay array
 				self.theta_fb[self.FB_idx] 	   = self.load_angles.theta
 				self.theta_vel_fb[self.FB_idx] = self.load_angles.thetadot
-				self.theta_acc_fb[self.FB_idx] = self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1]
+				self.theta_acc_fb[self.FB_idx] = (self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1])/self.dt
 
 				self.phi_fb[self.FB_idx] 	   = self.load_angles.phi
 				self.phi_vel_fb[self.FB_idx]   = self.load_angles.phidot
-				self.phi_acc_fb[self.FB_idx]   = self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1]
+				self.phi_acc_fb[self.FB_idx]   = (self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1])/self.dt
 
 			else:
 				# once array is filled, need to shift values w/ latest value at end
@@ -277,13 +291,13 @@ class Main:
 				self.phi_vel_fb[:] = np.roll(self.phi_vel_fb[:],-1)
 				self.phi_acc_fb[:] = np.roll(self.phi_acc_fb[:],-1)			
 		
-				self.theta_fb[len(self.theta_fb)-1] 	= self.load_angles.theta # change final value
-				self.theta_vel_fb[len(self.theta_fb)-1] = self.load_angles.thetadot
-				self.theta_acc_fb[len(self.theta_fb)-1] = self.load_angles.thetadot - self.theta_vel_fb[len(self.theta_fb)-1]
+				self.theta_fb[-1] 	  = self.load_angles.theta # change final value
+				self.theta_vel_fb[-1] = self.load_angles.thetadot
+				self.theta_acc_fb[-1] = (self.load_angles.thetadot - self.theta_vel_fb[-1])/self.dt
 
-				self.phi_fb[len(self.phi_fb)-1] 		= self.load_angles.phi
-				self.phi_vel_fb[len(self.theta_fb)-1]   = self.load_angles.phidot
-				self.phi_acc_fb[len(self.theta_fb)-1]   = self.load_angles.phidot - self.phi_vel_fb[len(self.theta_fb)-1]
+				self.phi_fb[-1] 	= self.load_angles.phi
+				self.phi_vel_fb[-1] = self.load_angles.phidot
+				self.phi_acc_fb[-1] = (self.load_angles.phidot - self.phi_vel_fb[-1])/self.dt
 
 		else:
 			print('No delay')
@@ -308,6 +322,7 @@ class Main:
 			self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i])
 			self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i])
 			
+		# convolution	
 		for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
 
 			self.delay(j,SHAPEFIL) # Determine the delay (shapefilter) array
@@ -319,18 +334,18 @@ class Main:
 				
 			else:
 				self.EPS_I[3*j]   = self.A1*self.x[-1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x)
-				self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) 									   
-
+				self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y)
+ 									   
 			self.delay(1,FEEDBACK) # Detemine feedback array
 			
-		self.sigmoid()        # Determine sigmoid gain
-		EPS_D = self.fback()  # Feedback Epsilon
+		# 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  + 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()
-		#self.ref_sig.type_mask  = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
 		self.ref_sig.position.x = self.EPS_F[0]
 		self.ref_sig.position.y = self.EPS_F[1]
 		self.ref_sig.position.z = self.EPS_F[2]
@@ -341,14 +356,17 @@ class Main:
 		self.ref_sig.acceleration.y	= self.EPS_F[7]
 		self.ref_sig.acceleration.z	= self.EPS_F[8]
 		
+		# update initial states
 		self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]]
 		self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]]
-		self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]]   
-
+		self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]]  
+		# self.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 """
 		
@@ -360,9 +378,9 @@ class Main:
 		ydotr  = -self.Gd*self.tetherL*math.cos(self.phi_fb[0])*self.phi_vel_fb[0]
 		yddotr =  self.Gd*self.tetherL*math.sin(self.phi_fb[0])*self.phi_vel_fb[0]**2 + math.cos(self.phi_fb[0])*self.phi_acc_fb[0]
 
-		EPS_D  = np.array([xr,yr,0,xdotr,ydotr,0,xddotr,yddotr,0])
+		Eps_D  = np.array([xr,yr,0,xdotr,ydotr,0,xddotr,yddotr,0])
 
-		return EPS_D 
+		return Eps_D 
 
 	def user_feeback(self):
 
@@ -373,20 +391,24 @@ 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		
-		self.convo()
+		# Determine final ref signal
+		try:		
+			self.convo()
 
-		# Publish reference signal
-		self.pub_path.publish(self.ref_sig)
+			# Publish reference signal
+			self.pub_path.publish(self.ref_sig)
 
-		# Publish what the setpoints are
-		self.waypointTracker_pub.publish(self.xd)
+			# Publish what the setpoints are
+			self.waypointTracker_pub.publish(self.xd)
 
-		# Give user feedback on published message:
-		self.user_feeback()
+			# Give user feedback on published message:
+			# self.user_feeback()
+		except AttributeError as e:
+			rospy.loginfo('Error {0}'.format(e))
+
+			pass # catches case at beginning when self.xd is not properly initialized
 
 if __name__=="__main__":
 

src/wpoint_tracker.py

@@ -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__":
 

srv/UseCtrl.srv

@@ -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
+