Skip to content
This repository has been archived by the owner on Feb 6, 2023. It is now read-only.

Setup up of thruster manager and simple velocity controller with joystick teleoperation for a new vehicle

Jump to bottom
Musa Morena Marcusso Manhães edited this page Nov 29, 2016 · 11 revisions

Assuming you have followed this tutorial for the creation of a thruster actuated vehicle model, you can now setup a simple velocity controller to control the vehicle with a joystick in the simulation. Be sure to have the installed the joy and joy-teleop packages for this. For ROS Indigo, install the packages as follows

sudo apt-get install ros-indigo-joy ros-indigo-joy-teleop

and for ROS Kinetic

sudo apt-get install ros-kinetic-joy ros-kinetic-joy-teleop

Creating a package for configuration and launch files

Again for this example you can create a new catkin package for your controller configuration files. This can be done as follows:

cd ~/catkin_ws/src
catkin_create_pkg uuv_control_example
mkdir -p config/rov_example launch

For this example, you can copy the following lines into the file ~/catkin_ws/src/uuv_control_example/CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(uuv_control_example)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED)

catkin_package()

install(DIRECTORY launch config
        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
        PATTERN "*~" EXCLUDE)

to have your folders properly installed.

Configuring your thruster manager

The thruster manager node will use the settings in the configuration file to search for the transformations between the vehicle's base_link and each thruster to generate the thruster allocation matrix. This can be done every time you use the thruster manager to command the thrusters, but you can also store the thruster allocation matrix for a faster initialization of the controllers.

Once it is configured, the thruster manager will subscribe to a ROS topic /<vehicle_name>/thruster_manager/input to receive messages of the type geometry_msgs/Wrench and then will compute the individual thrust efforts to each thruster unit. You can also use the thruster manager for your specific control applications by publishing this wrench message.

First, create a new configuration file for your thruster manager:

cd ~/catkin_ws/src/uuv_control_example/config/rov_example
touch thruster_manager.yaml

The file thruster_manager.yaml should look like this

thruster_manager:
  tf_prefix: rov_example
  base_link: base_link
  thruster_topic_prefix: thrusters/
  thruster_topic_suffix: /input
  thruster_frame_base: thruster_
  max_thrust: 100.0
  timeout: -1
  update_rate: 50
  conversion_fcn: custom
  conversion_fcn_params:
    input: [0, 1, 2, 3]
    output: [0, 1, 2, 3]

If you have different models for each individual thruster, you can assign a model for each thruster as follows:

thruster_manager:
  tf_prefix: rov_example
  base_link: base_link
  thruster_topic_prefix: thrusters/
  thruster_topic_suffix: /input
  thruster_frame_base: thruster_
  max_thrust: 100.0
  timeout: -1
  update_rate: 50
  conversion_fcn: 
    - custom
    - custom
    - custom
  conversion_fcn_params:
    - input: [0, 1, 2, 3]       
      output: [0, 1, 2, 3]       
    - input: [0, 1, 2, 3]       
      output: [0, 1, 2, 3]       
    - input: [0, 1, 2, 3]       
      output: [0, 1, 2, 3]

The most important items in the file above are the max_thrust (the absolute maximum for the thrust force at each thruster) and the conversion_fcn and conversion_fcn_params parameters.

As explained in this tutorial, the thruster units have to be configured with a dynamic model and a conversion curve to generate the thrust force. If you used the following setup for your thruster's conversion curve

<conversion>
  <type>Basic</type>
  <rotorConstant>0.0</rotorConstant>
</conversion>

then your thruster manager configuration file should have the following items

  conversion_fcn: proportional
  conversion_fcn_params:
    gain: <value of rotorConstant>

If you used the linear interpolation option

<conversion>
  <type>LinearInterp</type>
  <inputValues>0 1 2 3</inputValues>
  <outputValues>0 1 2 3</outputValues>
</conversion>

then set the conversion curve in thruster_manager.yaml as

  conversion_fcn: custom
  conversion_fcn_params:
    input: <values of inputValues>
    output: <values of outputValues>

Be aware to write the values in the vectors input and output between [] and separated by commas.

Generating the thruster allocation matrix

To make it easier to call the thruster manager for your application, you can create the following launch file to start the thruster manager for your vehicle

cd ~/catkin_ws/src/uuv_control_example
touch launch/thruster_manager.launch

and setup the file as

<launch>
  <arg name="model_name" default="rov_example"/>
  <arg name="uuv_name" default="$(arg model_name)"/>
  <arg name="base_link" default="base_link" />
  <arg name="timeout" default="-1" />
  <arg name="reset_tam" default="false"/>
  <arg name="output_dir" default="$(find uuv_control_example)/config/$(arg model_name)"/>
  <arg name="config_file" default="$(find uuv_control_example)/config/$(arg model_name)/thruster_manager.yaml"/>
  <arg name="tam_file" default="$(find uuv_control_example)/config/$(arg model_name)/TAM.yaml"/>

  <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
    <arg name="model_name" value="$(arg model_name)"/>
    <arg name="uuv_name" value="$(arg uuv_name)"/>
    <arg name="base_link" value="$(arg base_link)"/>
    <arg name="timeout" value="$(arg timeout)"/>
    <arg name="reset_tam" value="$(arg reset_tam)"/>
    <arg name="output_dir" value="$(arg output_dir)"/>
    <arg name="config_file" value="$(arg config_file)"/>
    <arg name="tam_file" value="$(arg tam_file)"/>
   </include>
</launch>

If started as

roslaunch uuv_control_example thruster_manager.launch

this node will try to first find the TAM.yaml with the thruster allocation matrix, and if it fails, it will calculate the matrix using the information in tf.

If the reset_tam flag is set to true as follows

roslaunch uuv_control_example thruster_manager.launch reset_tam:=true

the thruster allocator node will create (or overwrite) the thruster allocation matrix file in the folder given by output_dir.

To do this, you have to start the simulation with the vehicle and the thruster allocation matrix at least once using the reset_tam:=true flag. Here is an example of the sequence of commands to use

roslaunch uuv_descriptions ocean_waves.launch

and then

roslaunch uuv_descriptions_example rov_example.launch

and finally

roslaunch uuv_control_example thruster_manager.launch reset_tam:=true

Or setup a launch file uuv_control_example/launch/reset_tam.launch as follows

<launch>
  <!-- Start new world -->
  <include file="$(find uuv_descriptions)/launch/ocean_waves.launch"/>

  <!-- Start your vehicle -->
  <include file="$(find uuv_descriptions_example)/models/rov_example/launch/upload_rov_example.launch"/>

  <!-- Reset the TAM matrix -->
  <include file="$(find uuv_control_example)/launch/thruster_manager.launch">
    <arg name="reset_tam" value="true"/>
  </include>
</launch>

and just run

roslaunch uuv_control_example reset_tam.launch

You can see that the TAM was generated successfully if you see an output similar to this in the terminal (the following output was created with the rexrov model)

output_dir= /home/my_home/catkin_ws/src/uuv_simulator/uuv_control/uuv_thruster_manager/config/rexrov
ThrusterManager: updating thruster poses
transform: /rexrov/base_link -> /rexrov/thruster_0
Thruster #0 - proportional - thrusters/0/input
transform: /rexrov/base_link -> /rexrov/thruster_1
Thruster #1 - proportional - thrusters/1/input
transform: /rexrov/base_link -> /rexrov/thruster_2
Thruster #2 - proportional - thrusters/2/input
transform: /rexrov/base_link -> /rexrov/thruster_3
Thruster #3 - proportional - thrusters/3/input
transform: /rexrov/base_link -> /rexrov/thruster_4
Thruster #4 - proportional - thrusters/4/input
transform: /rexrov/base_link -> /rexrov/thruster_5
Thruster #5 - proportional - thrusters/5/input
transform: /rexrov/base_link -> /rexrov/thruster_6
Thruster #6 - proportional - thrusters/6/input
transform: /rexrov/base_link -> /rexrov/thruster_7
Thruster #7 - proportional - thrusters/7/input
transform: /rexrov/base_link -> /rexrov/thruster_8
could not get transform from: /rexrov/base_link
to: /rexrov/thruster_8
[<uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ad2e7fd0>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ac06c410>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ad2e7e50>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ac06c250>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ac06cad0>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ac06ccd0>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ac06cd90>, <uuv_thrusters.models.thruster_proportional.ThrusterProportional object at 0x7fa1ac06cf10>]
TAM=
[[ 0.15974269  0.15974269 -0.15974243 -0.15974243  0.70710686  0.70710686
  -0.70710654 -0.70710654]
 [-0.21361012  0.21361012 -0.21360977  0.21360977  0.7071067  -0.7071067
   0.70710702 -0.70710702]
 [ 0.9637702   0.9637702   0.96377032  0.96377032  0.          0.          0.
   0.        ]
 [ 0.43523203 -0.43523203  0.43523188 -0.43523188 -0.09121676  0.09121676
  -0.09121681  0.09121681]
 [ 0.9430935   0.9430935  -0.94309347 -0.94309347  0.09121679  0.09121679
  -0.09121674 -0.09121674]
 [ 0.13688862 -0.13688862 -0.1368884   0.1368884  -0.64879876  0.64879876
   0.64879873 -0.64879873]]
ThrusterManager: ready
ThrusterManager: ready

You can see that your TAM.yaml looks like something similar to this

tam:
- [0.15974268848446743, 0.15974268848446743, -0.1597424266785477, -0.1597424266785477,
  0.7071068613706476, 0.7071068613706476, -0.7071065406341925, -0.7071065406341925]
- [-0.2136101154609486, 0.2136101154609486, -0.21360976537047976, 0.21360976537047976,
  0.7071067010024383, -0.7071067010024383, 0.7071070217388206, -0.7071070217388206]
- [0.9637701967007046, 0.9637701967007046, 0.9637703176884092, 0.9637703176884092,
  0.0, 0.0, 0.0, 0.0]
- [0.4352320257020549, -0.4352320257020549, 0.4352318810229865, -0.4352318810229865,
  -0.09121676442931455, 0.09121676442931455, -0.09121680580430785, 0.09121680580430785]
- [0.9430934973994073, 0.9430934973994073, -0.9430934667380182, -0.9430934667380182,
  0.09121678511681354, 0.09121678511681354, -0.09121674374181084, -0.09121674374181084]
- [0.13688862492470316, -0.13688862492470316, -0.1368884005748274, 0.1368884005748274,
  -0.6487987634902757, 0.6487987634902757, 0.6487987335687417, -0.6487987335687417]

Setup a velocity PID controller

The uuv_simulator package has an implementation of cascaded PIDs to quickly test the vehicle in the simulation. To setup the controller in the uuv_control_cascaded_pid package for your new model, create the configuration files as follows

cd ~/catkin_ws/src/uuv_control_example
touch config/rov_example/inertial.yaml
touch config/rov_example/vel_pid_control.yaml

The inertial.yaml must contain the following items

pid:
  mass: 0
  inertial:
    ixx: 0
    ixy: 0
    ixz: 0
    iyy: 0
    iyz: 0
    izz: 0

and you can use the information you used in the rigid-body description of your vehicle in uuv_descriptions_example in the file rov_example_base.xacro.

In the file vel_pid_control.yaml, setup the parameters of your PID controller following this template:

velocity_control/linear_d: 0.0
velocity_control/linear_i: 0.0
velocity_control/linear_p: 0.0
velocity_control/linear_sat: 0.0

velocity_control/angular_d: 0.0
velocity_control/angular_i: 0.0
velocity_control/angular_p: 0.0
velocity_control/angular_sat: 0.0

Now you can start the PID velocity control node in uuv_control_cascaded_pid with your vehicle. To do this more easily, you can create a launch file

cd ~/catkin_ws/src/uuv_control_example
touch launch/joy_velocity.launch

and copy the following lines into it

<launch>
  <arg name="model_name" default="rov_example"/>
  <arg name="uuv_name" default="$(arg model_name)"/>
  <arg name="joy_id" default="0"/>

  <include file="$(find uuv_control_example)/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)">
    <rosparam file="$(find uuv_control_example)/config/$(arg model_name)/inertial.yaml" command="load"/>
    <rosparam file="$(find uuv_control_example)/config/$(arg model_name)/vel_pid_control.yaml" command="load"/>

    <node pkg="uuv_control_cascaded_pid" type="AccelerationControl.py" name="acceleration_control"
          output="screen">
      <param name="tf_prefix" type="string" value="$(arg uuv_name)/" />
    </node>

    <node pkg="uuv_control_cascaded_pid" type="VelocityControl.py" name="velocity_control"
        output="screen">
      <remap from="odom" to="/$(arg uuv_name)/pose_gt"/>
      <remap from="cmd_accel" to="/$(arg uuv_name)/cmd_accel"/>
    </node>
  </group>

  <include file="$(find uuv_vehicle_teleop)/launch/uuv_teleop.launch">
    <arg name="uuv_name" value="$(arg uuv_name)"/>
    <arg name="joy_id" value="$(arg joy_id)"/>
  </include>
</launch>

Now you can start the vehicle using the controller and joystick as follows:

roslaunch uuv_descriptions ocean_waves.launch

and then

roslaunch uuv_descriptions_example rov_example.launch

and finally

roslaunch uuv_control_example joy_velocity.launch joy_id:=0

Check the RexROV example to see how to find the ID of your joystick and control mapping.

The joystick teleoperation is setup as default for the XBox 360 controller. Check this link for more information on the controller mapping.

If you have a XBox 360 controller, keep the LB button pressed while controlling your vehicle. This is done so that it is possible to switch and also control the manipulator using the same controller.

You can create also a launch file to help you start this scenario:

cd ~/catkin_ws/src/uuv_control_example
touch launch/start_joy_control.launch

Copy the following in the start_joy_control.launch file.

<launch>
  <arg name="joy_id" default="0"/>
  <!-- Start new world -->
  <include file="$(find uuv_descriptions)/launch/ocean_waves.launch"/>

  <!-- Start your vehicle -->
  <include file="$(find uuv_descriptions_example)/models/rov_example/launch/upload_rov_example.launch"/>

  <!-- Start the controller and teleop nodes -->
  <include file="$(find uuv_control_example)/launch/joy_velocity.launch">
    <arg name="joy_id" value="$(arg joy_id)"/>
  </include>
</launch>

Now you can start the whole scenario by using

roslaunch uuv_control_example start_joy_control.launch joy_id:=0
Clone this wiki locally