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 YOUR 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 LIB 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/GAZEBO: Gets 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) oscillation_ctrl Dependencies

INSTALL XTERM

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

INSTALL MAVROS_CONTROLLERS

cd ~/catkin_ws/src
#clone repo:
git clone https://github.com/Jaeyoung-Lim/mavros_controllers
#Download dependencies: 
cd ~/catkin_ws
wstool merge -t src src/mavros_controllers/dependencies.rosinstall
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO
catkin build
source ~/catkin_ws/devel/setup.bash
#####Troubleshooting: https://github.com/Jaeyoung-Lim/mavros_controllers

CLONE OSCILLATION_CTRL

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

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

ROMFS/PX4FMU_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!)

LAUNCH FILES

• 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), 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

• 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 klausen_dampen.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 # default spiri
  headless_spiri_with_tether # headless mode: launches with no Gazebo GUI headless_spiri -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 klausen_dampen.launch model:=headless_spiri_with_tether test:=step