Creating a custom dynamic positioning controller
To facilitate the implementation of new control algorithms for the vehicle modelled in this package, a few Python modules were created as an interface to the vehicle's thruster manager, the local planner and setup the necessary publishers and subscribers needed to receive trajectory messages and send thruster commands. The module also includes an implementation of Fossen's equations of motion that can be used by model-based controllers. To create a controller based on this Python modules, see the following steps. All the files discussed below are available in the package uuv_tutorial_dp_controller in the uuv_tutorials folder.
First, a new catkin package will be created to include the scripts and launch files necessary for this implementation. Replace the name of the catkin workspace catkin_ws below if yours is named differently.
cd ~/catkin_ws/src
catkin_create_pkg uuv_tutorial_dp_controllerThis command will create the necessary files for the new package which will be edited further on in this tutorial. Next, two folders are needed, one launch folder to the launch files and one scripts folder, where the implementation of the custom controller will be stored.
cd ~/catkin_ws/src/uuv_tutorial_dp_controller
mkdir launch scriptsIn the scripts folder, a Python file has to be created, here we will name it tutorial_dp_controller.py.
touch scripts/tutorial_dp_controller.py
This file will have the implementation of the controller named TutorialDPController, a very simple PID controller, as shown below.
#!/usr/bin/env python
import rospy
import numpy as np
from uuv_control_interfaces import DPControllerBase
class TutorialDPController(DPControllerBase):
def __init__(self):
super(TutorialDPController, self).__init__(self)
self._Kp = np.zeros(shape=(6, 6))
self._Kd = np.zeros(shape=(6, 6))
self._Ki = np.zeros(shape=(6, 6))
self._int = np.zeros(shape=(6,))
self._error_pose = np.zeros(shape=(6,))
# Do the same for the other two matrices
if rospy.get_param('~Kp'):
diag = rospy.get_param('~Kp')
if len(diag) == 6:
self._Kp = np.diag(diag)
print 'Kp=\n', self._Kp
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Kp diagonal matrix, 6 coefficients are needed')
if rospy.get_param('~Kd'):
diag = rospy.get_param('~Kd')
if len(diag) == 6:
self._Kd = np.diag(diag)
print 'Kd=\n', self._Kd
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Kd diagonal matrix, 6 coefficients are needed')
if rospy.get_param('~Ki'):
diag = rospy.get_param('~Ki')
if len(diag) == 6:
self._Ki = np.diag(diag)
print 'Ki=\n', self._Ki
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Ki diagonal matrix, 6 coefficients are needed')
def _reset_controller(self):
super(TutorialDPController, self)._reset_controller()
self._error_pose = np.zeros(shape=(6,))
self._int = np.zeros(shape=(6,))
def update_controller(self):
if not self.odom_is_init:
return
self._int = self._int + 0.5 * (self.error_pose_euler + self._error_pose) * self._dt
self._error_pose = self.error_pose_euler
tau = np.dot(self._Kp, self.error_pose_euler) + np.dot(self._Kd, self._errors['vel']) + np.dot(self._Ki, self._int)
self.publish_control_wrench(tau)
if __name__ == '__main__':
print('Tutorial - DP Controller')
rospy.init_node('tutorial_dp_controller')
try:
node = TutorialDPController()
rospy.spin()
except rospy.ROSInterruptException:
print('caught exception')
print('exiting')Analyzing the file in detail, for the controller to use the controller modules from uuv_control_interfaces, it has to inherit the DPControllerBase class and initialize it:
class TutorialDPController(DPControllerBase):
def __init__(self):
super(TutorialDPController, self).__init__(self)This will setup the super class to initialize, for example, odometry message subscribers and thruster manager topic publishers. For model-based controllers, the super class constructor has to be called as
super(TutorialDPController, self).__init__(self, is_model_based=True)The controller's parameters, in this case Kp, Kd and Ki should retrieved from the parameter server to allow the controller to be used in different configurations. One alternative is to read this information from the node's private parameter namespace as shown below. This is done by adding a ~ in front of the parameters's tag. The parameter has to be provided at the controller's launch file accordingly, which will be discussed later on.
if rospy.get_param('~Kd'):
diag = rospy.get_param('~Kd')
if len(diag) == 6:
self._Kd = np.diag(diag)
print 'Kd=\n', self._Kd
else:
# If the vector provided has the wrong dimension, raise an exception
raise rospy.ROSException('For the Kd diagonal matrix, 6 coefficients are needed')The _reset_controller method can be overriden in case internal variables must be reset when the reset service call is received. It is important to also call the super class' reset method as seen below to ensure that error and reference vectors are also going to be cleared.
super(TutorialDPController, self)._reset_controller()When using the super class DPControllerBase, there is no need to add an controller update sequence or a timer. Once the method update_controller is implemented in the controller class, it will be given as a callback function to the odometry update method. This update method should include the controller's algorithm and generate the control effort vector (in this case tau) and use the super class function publish_control_wrench to publish it to the thruster manager input.
The last part of the file is necessary for the ROS node to be executed.
if __name__ == '__main__':
print('Tutorial - DP Controller')
rospy.init_node('tutorial_dp_controller')
try:
node = TutorialDPController()
rospy.spin()
except rospy.ROSInterruptException:
print('caught exception')
print('exiting')
Once the custom controller is done, you have to turn your Python script into an executable and it can be done as follows, otherwise you will not be able to start the ROS node.
cd ~/catkin_ws/src/uuv_tutorial_dp_controller/scripts
chmod 777 tutorial_dp_controller.pyNext step is to setup the launch file for the new controller. Create a new launch file as follows
cd ~/catkin_ws/src/uuv_tutorial_dp_controller/launch
touch start_tutorial_dp_controller.launch
Edit the file to include the following
<launch>
<arg name="uuv_name"/>
<arg name="model_name" default="$(arg uuv_name)"/>
<arg name="saturation" default="5000"/>
<arg name="Kp" default="11993.888,11993.888,11993.888,19460.069,19460.069,19460.069"/>
<arg name="Kd" default="9077.459,9077.459,9077.459,18880.925,18880.925,18880.925"/>
<arg name="Ki" default="321.417,321.417,321.417,2096.951,2096.951,2096.951"/>
<include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
<arg name="uuv_name" value="$(arg uuv_name)"/>
<arg name="model_name" value="$(arg model_name)"/>
</include>
<group ns="$(arg uuv_name)">
<node pkg="uuv_control_utils"
type="trajectory_marker_publisher.py"
name="trajectory_marker_publisher"
output="screen">
<remap from="trajectory" to="dp_controller/trajectory"/>
<remap from="waypoints" to="dp_controller/waypoints"/>
</node>
<node pkg="uuv_tutorial_dp_controller"
type="tutorial_dp_controller.py"
name="tutorial_dp_controller"
output="screen">
<remap from="odom" to="pose_gt"/>
<remap from="trajectory" to="dp_controller/trajectory"/>
<remap from="input_trajectory" to="dp_controller/input_trajectory"/>
<remap from="waypoints" to="dp_controller/waypoints"/>
<remap from="error" to="dp_controller/error"/>
<remap from="reference" to="dp_controller/reference"/>
<remap from="thruster_output" to="thruster_manager/input"/>
<rosparam subst_value="true">
saturation: $(arg saturation)
Kp: [$(arg Kp)]
Kd: [$(arg Kd)]
Ki: [$(arg Ki)]
</rosparam>
</node>
</group>
</launch>The most important parts of the launch file to notice is that the vehicle namespace uuv_name must always be provided, since the simulation per default will have nodes specific to the operation of each vehicle created inside their namespaces. The thruster manager must also be initialized (for more information on how to setup the thruster manager, check this tutorial).
Finally, the controller node has to be called, along with the correct parameters set by the arguments Kp, Kd and Ki in this example. You can pass this parameters by command line or set default vectors as seen above, but there should be no spaces between commas and values. They have to be set in the rosparam block. The trajectory_marker_publisher is an optional node used only to publish visual markers.
To start a small demonstration using the RexROV vehicle, you can create another launch file as follows
cd ~/catkin_ws/src/uuv_tutorial_dp_controller/launch
touch start_tutorial_dp_controller_demo.launch
and initialize a world, the vehicle and the RViz visualization tool as follows
<launch>
<!-- If you want to generate a ROS bag with the recorded simulated data, set this flag to true -->
<arg name="record" default="false"/>
<!-- Start the a underwater world simulation scenario -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="worlds/ocean_waves.world"/>
<arg name="paused" value="false"/>
</include>
<!-- Add the RexROV vehicle to the simulation (namespace: rexrov) -->
<include file="$(find uuv_descriptions)/models/rexrov/launch/upload_rexrov.launch">
<arg name="x" default="20"/>
<arg name="y" default="0"/>
<arg name="z" default="-20"/>
<arg name="yaw" default="0"/>
</include>
<!-- Start the controller -->
<include file="$(find uuv_tutorial_dp_controller)/launch/start_tutorial_dp_controller.launch">
<arg name="uuv_name" value="rexrov"/>
<arg name="model_name" value="rexrov"/>
</include>
<!-- Start the recording node -->
<include file="$(find uuv_gazebo)/launch/controller_demos/record_demo.launch">
<arg name="record" value="$(arg record)"/>
</include>
<!-- Open RViz for visualization of sensor data and visualization markers -->
<node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find uuv_gazebo)/rviz/controller_demo.rviz"/>
</launch>Before you can run this demo, the package has to be configured.
To allow catkin to install all your modules, you can open the CMakeLists.txt file from your catkin package and edit it to look like the following
cmake_minimum_required(VERSION 2.8.3)
project(uuv_tutorial_dp_controller)
find_package(catkin REQUIRED)
catkin_package()
catkin_install_python(PROGRAMS scripts/tutorial_dp_controller.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN "*~" EXCLUDE)After you compile you workspace again with catkin_make, you can run the demo launch file created before.
roslaunch uuv_tutorial_dp_controller start_tutorial_dp_controller_demo.launchThis will start the Gazebo simulator with an instance of the RexROV vehicle with this custom controller being used for positioning.
You can use one the modules from the uuv_control_utils package to send the vehicle some waypoints and see the controller in action. For example, use the default list of waypoints and send them by using
roslaunch uuv_control_utils send_waypoints_file.launch uuv_name:=rexrovThe local planner in uuv_control_interfaces that is used per default by the DPControllerBase class will receive the waypoints and apply a linear interpolation with polynomial blends to generate a path to be followed by the vehicle.
You can now use this files as a template for your implementation.