oscillation_ctrl

Repo containing oscillation damping controller for tether missions + instructions how to to set up

Cesar Rodriguez

cesar.rodriguez@spirirobotics.com

February 2022

Steps to recreate stable PX4 environment + working repo

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

Download PX4 source code

git clone https://github.com/PX4/PX4-Autopilot.git --recursive

Run ubuntu.sh

bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
#Restart computer after it is done

Build ROS and Gazebo - This defaults to Gazebo9

wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
bash ubuntu_sim_ros_melodic.sh

Download QGroundControl from:

https://docs.qgroundcontrol.com/master/en/releases/daily_builds.html

Build Gazebo Sim

cd ~/PX4-Autopilot
make px4_sitl gazebo

Create px4 package

cd ~/PX4-Autopilot
DONT_RUN=1 make px4_sitl_default gazebo
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

3) Set up oscillation_ctrl

Install xterm

sudo apt-get update -y
sudo apt-get install -y xterm

Clone oscillation_ctrl

cd ~catkin_ws/src
git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git

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/* 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:

CURRENT_DIR=$(pwd)
cd ~/PX4-Autopilot
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

cd $CURRENT_DIR

JINJA TETHER FILE

• spiri_with_tether.sdf.jinja can be altered to create desired tether model

• changes need to be made in px4 directory and will only take effect after running: "make px4_sitl gazebo"

  • can do "DONT_RUN=1 make px4_sitl gazebo" to avoid starting px4 shell and gazebo

• 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 (Ln 288 - may change in future) in the ~/PX4-Autopilot/Tools/sitl_gazebo folder:

    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
    )
  

• This should be added in the line below VERBATIM in the add_custom_command function, which should be Ln 288)

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 step test.

Publishes to:

/status/twoBody_status # localization and angles
/status/load_angles    # payload angles (and tates) relative to vehicle
/status/path_follow    # boolean to run trajectory test

Subscribes to:

none

wpoint_tracker.py

Sets original waypoints to be (in meters): [x=0,y=0,z=5]. This node listens to topic to keep track of desired waypoints. If any other node wants to change the waypoints, they publish to "reference/waypoints" and wpoint_tracker creates these new waypoints.

Publishes to:

none

Subscribes to:

/reference/waypoints 

ref_signalGen.py

takes in desired position (xd) and determines smooth path trajectory.

Publishes to:

/reference/path         # smooth path
/reference/flatsetpoint # needed to determine thrust

Subscribes to:

/status/load_angles
/mavros/local_position/pose
/mavros/local_position/velocity_body
/mavros/imu/data
/mavros/state
/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.

Publishes to:

/command/quaternions # attitude commands

Subscribes to:

/status/load_angles
/reference/path
/mavros/local_position/pose
/mavros/local_position/velocity_body
/mavros/imu/data

• node from mavros_controllers/geometric_controller subscribes to /reference/flatsetpoint to determine thrust commands which are published to command/bodyrate_command by default

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.

Publishes to:

mavros/setpoint_position/local # needed to hover @ set height	
mavros/setpoint_raw/attitude   # attitude and thrust commands 	

Subscribes to:

/command/quaternions	
/command/bodyrate_command	
/mavros/state

Launching simulation

To launch a simulation, run the following command:

roslaunch oscillation_ctrl oscillation_damp.launch

This simulation is set to have a Spiri Mu hover at an alitude of 5 m. The launch file itself has two usable arguments:

model:

spiri_with_tether           # Spiri Mu with a tethered payload			
spiri                       # Spiri Mu without tethered paylaod
headless_spiri_with_tether  # headless mode: launches with no Gazebo GUI. This is the default model

test:

none   # default
step   # step input - default is 5 m
square # square trajectory

To run the simulation with a tethered payload headless mode and perform a step test:

roslaunch oscillation_ctrl oscillation_damp.launch model:=headless_spiri_with_tether test:=step

Whenever the oscillation_ctrl is used, the scripts are written such that the vehicle will hover for about 30 seconds in "Position Mode". This is used to take advantage of the takeoff procedure PX4 has, as this controller assumes the vehicle is already in flight when determining the necessary thrust.