IEEE Robotics & Automation Magazine - September 2017 - 44

[η2,υ2]

υ1

Sensor
Fusion

AR
K -1
Tool Kit
η0,ee

Transponder

Camera

r
Single
Beacon

I
Visual
Servoing

Lη1

Lη

υ
Velocity
Controller

υd

Thrusters
r/min

Navigation Filter
η
Pose
Controller

Joints
Pos. Ctrl

Joints
Vel. Ctl

Depth
Sensor

DVL

TCM

Sensing and
Actuation

AHRS

Navigation

Encoders

Camera

Guidance and
Control

Joint
Sensing and
Position
Actuation
Control

Motors

ηd

η
q

Joint State
Synch

qkd

Trajectory
Planning

ηeed ... ηeed
1

Grasp
Analysis

Achieved
Figure 2. The GIRONA500 I-AUV software architecture. AR: augmented reality; TCM: tilt current meter.

into thruster set points. On its top, a pose controller handles
AUV pose (h) regulation and tracking. This works by sending
velocity set points (v d) to the velocity controller and reading
the pose feedback (h) from the EKF component to achieve a
desired pose (h d).
Arm Software Architecture
Sensing and Actuation
The sensing and actuation layer provides access to the motor
drivers, the joint encoders, and a camera used to estimate the
EE pose through visual servoing.
Joint Position
Although the arm position in configuration space is instrumented with joint encoders, the arm may miss pulses during
operation, which can lead to inaccurate calibration. To deal
with this problem, the EE is labeled using an augmented reality (AR) code [8], whose pose can be easily estimated with a
camera. Using the inverse kinematics, it is then possible to
estimate the arm configuration (Q) used to calibrate the position read from the encoders.
Joint Control
A cascade joint position and velocity controller is used to control each arm joint. Hence, it is possible to control the joint
velocity and/or position. It is also possible to control the EE
44

IEEE ROBOTICS & AUTOMATION MAGAZINE

SEPTEMBER 2017

velocity (ho 0,ee) in Cartesian space by means of the arm pseudoinverse Jacobian (J @ (q)) that translates the Cartesian velocity into the joint velocity (qvo).
Coordination
This coordination layer is devoted to the joint control of the
AUV and the arm. For this work, we implemented the coordination layer using MoveIt! As shown in Figure 2, the layer
integrates three modules: a trajectory planning, joint-state
synchronizer, and grasp analysis component. The trajectory
planner computes collision-safe AUV-arm trajectories in
the 8-DoF configuration space to achieve a desired EE
pose, providing control set points to the AUV and robotarm uncoupled controllers. In addition, the joint-state synchronizer component ensures that both the AUV and the
arm have reached their own set points before scheduling
the next one. Finally, the grasp analysis is included for grasp
reachability analysis.
MoveIt!-Based I-AUV Implementation
MoveIt! [3], a software framework providing a wide functionality that covers several features of mobile manipulation, encapsulates a forward/inverse kinematics solver, planning techniques,
and collision-detection methods through a plug-in-based
mechanism. Widely used third-party libraries are already integrated, including the Kinematics and Dynamics Library (KDL),
the Open Motion Planning Library (OMPL) [9], and the Fast

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2017