From simulation to the field:
Learning to swim with the AQUArobot
Juan Camilo Gamboa Higuera,David Meger and Gregory Dudek
School of Computer ScienceCentre for Intelligent Machines
McGill University

Work in our lab
Adaptive systems for autonomous scientific data collection

Meger et al, 3D Trajectory Synthesis and Control
for a Legged Swimming Robot, IROS 2014

Shkurti et al, Underwater Multi-Robot Convoying
using Visual Tracking by Detection, IROS 2017

Outline of this talk1. The Aqua Robota.
b.

Hardware Overview
Software Overview

2. The aqua_description and aqua_gazebo packagesa.
b.

Hardware emulation
Hydrodynamics simulation

3. Architecture for motor control learninga.
b.

Implementation of Model-Based RL algorithms (kusanagi)
The RL glue code (aqua_rl/kusanagi_ros)

1. The AQUA robot
Portable Underwater Autonomous Vehicle
Weight: 15Kgs
13cm

60cm

Based on RHex walking platform• Developed at McGill University• Commercialized by Independent Robotics

24cm

Who is using the AQUA robot?
Mobile Robotics Laboratory @ McGill(Greg Dudek and Dave Meger)
VGR Lab @ York U(Michael Jenkin)

Autonomous Field Robotics Lab @ U South Carolina(Yiannis Rekleitis)Interactive Robotics Laboratory @ U Minnesota(Junaed Sattar)

Vision and Robotics Lab @ CINVESTAV(Luz Abril Torres Mendez)

WARPLab @ Woods Hole(Yogesh Girdhar)

AQUA robot hardware
Aqua Robot
Internal sensors(IMU/Depth/Encoders)

Motor controller
Ethernet to Fiber
Optic converter
1211-1

Control CPU (RT)Vision CPU

Cameras

WiFi

Fiber Optic toEthernet
converter
1210-1

Subconn
Ethernet Gigabit
SwitchPC

External payload(USBL/GPS)Software Overview (Control only)
Control Stack (RT)
RoboDevel• Low-level Motor Ccontrol• Power Management• Internal Sensors

IMU driver

Vision Stack

External PC

Hardware interface (ROS)

Controller Learning (ROS)• aquahw• aquacore

Navigation (ROS)• aquaautopilot• aqua_depth• aqua_positioning

User interface (ROS)• aquajoy

1 khz loop

• Task Marshalling(aqua_marshall)• Feedback Controllers andDataset generations(kusanagi_ros)

General Purpose RL• kusanagi• rllabHardware Interface

aquacore specifies common messages for aqua state and control
aquahw UDP interface exposing robot state and commands through ROS

Navigationaquaautopilot implements trajectory tracking via waypoints

Meger et al, 3D Trajectory Synthesis and Control for a Legged Swimming Robot, IROS 2014aqua_positioning and aqua_depth transform sensor data to ROS
convention

User interface
aquajoy is the joypad interface to aquaautopilot
Sets waypoints and swimming modes• Flat swim
• Constant depth• 3D pose control

Meger et al, 3D Trajectory Synthesis and Control for a Legged Swimming Robot, IROS 2014

Shkurti et al, Underwater Multi-Robot Convoying using Visual Tracking by Detection, IROS 2017

2. aqua_description and aqua_gazebo

Software Overview
Development PCGazebo
aqua_hardware_emulator• Low-level Motor Control• Internal Sensors

imu_sensor_plugin

Emulated interface (ROS)• gazebo_ros• aqua_description

Navigation (ROS)• aquaautopilot• aqua_depth• aqua_positioning

1 khz loop

• Task Marshalling(aqua_marshall)• Feedback Controllers andDataset generations(kusanagi_ros)

General Purpose RL

hydrodynamics_plugin
flippers_plugin

Controller Learning (ROS)

User Interface (ROS)• aquajoy• kusanagi

Emulating the AQUA hardware
aqua_hardware_emulator provides the same interface as aquahw, running as a
Gazebo plugin

imu_sensor_plugin comes from gazebo_ros_plugins
hydrodynamics_plugin simulates additional underwater forces
flippers_plugin implements a PID controller and simulates propulsive forces foreach leg

hydrodynamics_plugin
Do regular rigid body dynamics simulation on gazebo (e.g. with ODE)

Add hydrodynamic effects and hydrostatic buoyancy as applied forces(addRelativeForce, addRelativeTorque):

Added mass

Buoyancy
Compute volume V for each link and apply force at center of buoyancy, of eachlink, opposite to gravity

Added Mass and DragPrecompute drag tensor D and added mass tensor
using a simplified meshgeometry (see Weissman and Pinkall (2013) for details)

47k vertices

500 vertices

Added mass modelled as external force

flippers_plugin
Hydrodynamics alone do not model propulsion due to turbulence
We use the empirical model of Plamondon and Nahon, 2013, for each leg

Is this simulator accurate?

No• It gives a reasonable how the system might behave• We may improve it with models learned from real-world data

3. Architecture for motor control learningAdaptive systems realized via learning
Design is never finalized and depends on interactions with the environment(i.e. trial and error)

Experiments replace design effort

For a given a task, the agent must find a controller that realizes it

The problem with trial and error…

An alternative to robot experiments
Learn basics on a low
cost simulator

Adjust skills on thetarget platformROS and robot learning
ROS provides a flexible way of developing
robot software
Reinforcement Learning (RL) is a powerfulparadigm for solving robotics tasks
We have generic tools for developing and testing RL
algorithms, on idealized environments (e.g. OpenAIGym)

Controller Learning (ROS)• Task Marshalling(aqua_marshall)• Feedback Controllers andDataset generations(kusanagi_ros)

General Purpose RL• kusanagi

How can we glue all of this together?

kusanagi library overview
Algorithms• PILCO• MC-PILCO• PDDP• Policy Adjustments
Controllers• Linear Controllers• Radial Basis Functions• Neural Networks

Utilities• Experience datasets• Applying and evaluatingcontrollers• PlottingRegression• Gaussian Process Regression• Sparse Gaussian ProcessRegression• Bayesian Neural Networks

Coming soon at: https://github.com/juancamilog/kusanagi

aqua_marshall and kusanagi_ros
Pipelining of multiple learning tasks

Sequential execution of learned
controllers on the robot

Parallel learning of models andcontrollers on off-site server

Building learning datasets/kusanagi_ros/state/kusanagi_ros/command

Aggregate data from multiple sources (ROS topics), into
robot-agnostic sensor and control streams

topics_to_rl_streams yaml configuration• Specifying state stream (published as /kusanagi_ros/state)experience_state_topics:- topic_name: /aqua/state
type: {package: aquacore, name: StateMsg}
filter: ["RollAngle","PitchAngle","YawAngle", "Depth"]- topic_name: /aqua/imu_data
type: {package: sensor_msgs, name: Imu}
filter: ["angular_velocity.x","angular_velocity.y","angular_velocity.z"]• Specifying action stream (published as /kusanagi_ros/command)experience_command_topics:- topic_name: /aqua/periodic_leg_command
type: {package: aquacore, name: PeriodicLegCommand}
filter: ["amplitudes[2]","amplitudes[5]","leg_offsets[2]","leg_offsets[5]"]default_values: { frequencies: 2.5 }Motor Control Learning on AQUAController Learning (ROS)• Task Marshalling :aqua_marshall• Feedback Policies andDataset generation:kusanagi_ros
General Purpose RL• kusanagi

Meger et al, Learning Legged Swimming Gaits from Experience, ICRA 2015

Meger et al, Learning Legged Swimming Gaits from Experience, ICRA 2015

What’s next?

Is this robotics?

Summary• Gave an overview of ROS packages for motor control on the AQUA robot.
• aquahw, aquaautopilot, aquajoy• Described gazebo plugins for underwater rigid body dynamics simulation• aqua_gazebo• Introduced a generic model-based RL pipeline, and its application to theAQUA robot• aqua_marshall• aqua_rl/kusanagi_ros• kusanagi

https://github.com/mcgillmrl

Thanks!
Some of the contributors:• Anqi Xu• Alex Chatron• Chris Prahacs• David Meger• Florian Shkurti• Joanna Hansen• Juan Camilo Gamboa Higuera• Junaed Sattar• Jimmy Li• Malika Meghjani• Nikolaos Pateromichelakis• Nikhil Kakodkar• Philippe Giguère• Sandeep Manjanna• Travis Manderson• Victor Barbarosh• Yogesh Girdhar

https://github.com/mcgillmrl