5. underwater manipulationsanzp/irs/autrobmanipula-2011-4.pdf · 5/9/2011 2 rauvi dpi2008-06548-c03...

5/9/2011

1

Autonomous Robotic Manipulation (4/4)

[email protected]

Pedro J Sanz

April 2010 Fundamentals of Robotics (UdG) 2

5. Underwater

Manipulation

mailto:[email protected]

5/9/2011

2

RAUVI

DPI2008-06548-C03

Multipurpose

Autonomous

Manipulation Systems

for Underwater

Intervention Missions

TRIDENT

(FP7-ICT-248497)

Project Timeline 200

9 EN

E FE

B

MA

R

AB

R

MA

Y JU

N

JUL

AG

O

SEP

O

CT

NO

V

DEC

2

010

ENE

FEB

M

AR

A

BR

M

AY

JUN

JU

L A

GO

SE

P

OC

T N

OV

D

EC

201

1 EN

E FE

B

MA

R

AB

R

MA

Y JU

N

JUL

AG

O

SEP

O

CT

NO

V

DEC

Final Design Preliminary

Design

Autonomous Teleoperated

I-A

UV

DES

IGN

M

ECH

ATR

ON

ICS

SOFT

WA

RE

EXP

ERIM

ENTS

(O

bje

ct

Rec

ove

ry)

Sea Trials 3D Simulation

Final Architecture

G500

Mechatronics Mechatronics

Integration

7

F ig. 9 Vision module architecture as a ROS node.

but a few invariant features, a more aggressive approachis needed to ext ract and match as many features of thetarget as possible. Solut ions include theext ract ion of fea-tures at different angles of rotat ion of the original frameand the gathering of addit ional features over mult iplenew frames.

6.2 The Vision Module Architecture

The vision module must provide the rest of the sys-tem with higher-level processing capabilit iesas describedabove. To that end, this module is conceived as a ROSnode on independent processing hardware and that ad-vert ises a number of topics [7] to which other ROS nodescan subscribe when needed (see Figure 9).

Our first implementat ion is a monocular two-dimen-sional system that works best when the seabed is rea-sonably flat . Two methods to process three-dimensionalinformat ion are under development , integrat ing st ruc-tural informat ion both from consecut ive imagesand frombinocular cameras. The vision system hardware is basedon two Firewire stereo rigs and a processing unit . Thismodule is connected to other computers on the vehiclethrough an ethernet link. The cameras and the process-ing unit are placed in separate watert ight cases (see fig-ure10). Thissolut ion isvery flexibleand allowsus to testdifferent configurat ions and cameras during the project .

The visual odometer developed here extracts a set offeatures from an image that are relat ively invariant tocont rast , scale, and view point [16–18]. We find that theSURF feature descriptor [19,20] offers the best combi-nat ion of speed, invariance, and configurability. In par-t icular, the same features allow us to calculate mot ionbetween consecut ive images, ident ify overlap at points

Fig. 10 Watert ight cases containing the computer (left ) anda stereo-camera (right ) of the vision system

where the survey t rajectory intersects, and to detect andlocalize theToI. Images areprocessed only onceand thenstored. All further operat ions are performed on the ex-t racted features. Thefeaturedescriptorsof a single imagetypically occupy in the order of 100kB of memory, andthe visual system adopts a variety of heurist ics to loadonly those features into main memory that have a highprobability to match against the next image.

For each feature, a descriptor is calculated from thetwo-dimensional Haar wavelet response in a number ofrectangular regions that surround the feature. A matchwith a feature in another imageor in theToI is confirmedif the Euclidean distance between responses is below acertain threshold, and is also significant ly lower than toany other features in the same image. Mot ion betweenconsecut ive images, as well as pose est imates with regardto intersect ions of the survey trajectory, with regard toan arbit rary frame during stat ion keeping, and with re-gard to the ToI are all est imated from the affine homog-raphy calculated between sets of matching features.

Our affine homography allows only for four degreesof freedom: lateral t ranslat ion, yaw, and scaling. Despitethe fact that the vehicle cannot completely prevent pitchand roll, inclusion of theseaddit ional degrees of freedomsin the calculat ion of thehomography int roducesan unac-ceptable level of numerical instability, in part icular whenmot ion est imates are calculated over a longer series ofimages. We make extensive use of RANSAC (RANdomSAmpleConsensus [21]), both to filter out the largenum-ber of mismatches between features, as well as to preventpoorly localized features from influencing the pose est i-mate.

7 Exper iment al val idat ion: t he Sear ch &R ecover y pr oblem

To experimentally validate the system described abovewe applied it to a real Search & Recovery problem: find-ing and retrieving a flight data recorder. The experi-ments were carried out at the CIRS water tank (Uni-versity of Girona). A digital image of a real sea floor (seeFigure 11) was printed in a 4× 8 m poster and placed atthe bot tom of the water tank, as can be appreciated inFigure 1. A mockup of a black box (of size 13× 15× 40cm) was placed at an unknown posit ion at the floor ofthe water tank. The experiment was divided into two

2

by Remotely Operated Vehicles (ROVs). Manned sub-mersibles have the advantage of placing the operator inthe field of operat ion with direct view to the object be-ing manipulated. Their drawbacks are the reduced t imefor operat ion (typically in the order of a few hours) thehuman presence in a dangerous and host ile environment ,and a very high cost of the associated oceanographic ves-sel. Work class ROVs are current ly the preferred technol-ogy for deep water intervent ion. They can be remotelyoperated for days without problems. Nevertheless, theyst ill need an expensive oceanographic vessel with a heavycrane and automat ic Tether Management System (TMS)and a Dynamic Posit ion system (DP). The cognit ive fa-t igueof theoperator who has to takecareof theumbilicaland the ROV while cooperat ing with the operator of therobot ic arms is remarkable.

For thesereasons, someresearchershaverecent ly start -ed to think about the natural evolut ion of the interven-t ion ROV, the Intervent ion AUV (I-AUV). Without theneed for the TMS and the DP, light I-AUVs could theo-ret ically be operated from cheap vessels of opportunity,considerably reducing the cost of operat ion. Consideringthe fast development of bat tery technology, and remov-ing the operator from the control loop, one can start tothink about intervent ion operat ions that last for severaldays, where a ship is only needed on the first and thelast day for launch and recovery.

But this fascinat ing scenario, where I-AUVs do thework autonomously, comes at the cost of endowing therobot with the intelligence needed to keep the operatorout of the cont rol loop. Although standard AUVs arealso operated without human intervent ion, they are con-st rained to survey operat ions, commonly flying at a safealt itude with respect to the ocean floor while loggingdata. I-AUVs must be operated in the close proximityof the seabed or art ificial st ructures. They have to beable to ident ify the objects to be manipulated and theintervent ion tasks to be undertaken, while safely movingwithin a clut tered work area. While I-AUVs are the nat -ural way of technological progress, they represent an au-thent ic research challenge for the Robot ics community.Moreover, the I-AUVs that have been developed unt ilnow, and which have proven field capabilit ies, are heavyvehicles intended for very deep water intervent ions. E.g.,the SAUVIM [1] and ALIVE [2] vehicles weight 6 and 3.5ton respect ively. It is a fact that science and indust ry areinterested in the design and development of a very lightI-AUV (< 300 kg) that is constrained to shallow waterintervent ions in depths up to 300 m. The construct ion ofan I-AUV that is able to perform intervent ion act ivit iescompletely autonomously, and can be validated exper-imentally in a realist ic scenario with a real prototype,would const itutea technological milestone. This is in factthe aim of the RAUVI project [3].

To foster further research and development of ourproject , we have selected a Search & Recovery (S&R)testbed applicat ion (see Figure 1). A typical S&R mis-

F ig. 2 The GIRONA 500 AUV in a survey configurat ion.

sion is the recovery of a Flight Data Recorder (FDR, alsoknown as black-box) from a crashed airplane. Flight re-corders are typically equipped with a 27-39 KHz pingerthat periodically emits an acoust ic signal that is audibleup to a distance of approximately one kilometer. Theacoust ic beacon will begin to emit when immersed in wa-ter and the ping will last unt il the bat tery is exhausted,around one month later. The t ime limitat ion forces thesearch method to be as efficient as possible. For the ex-periments presented in this paper, we assume the FDRto have already been localized within a small area, andwe focus on the local vision-based search and recovery.

Few technical papers discuss black box recovery withthe aid of an underwater intervent ion vehicle. All exam-ples in the literaturedescribe theuseof ROV vehicles. Tothe best of the authors’ knowledge, an autonomous vehi-cle has never been used for a black box recovery mission,likely due to the high complexity of this task. Only sometheoret ic papers are available that describe prospect ivework [4].

The remainder of this paper is organized as follows.Sect ion 2 presents the evolut ion of the I-AUV conceptunder development and int roduces details of both thevehicle and the robot arm. Sect ion 3 shows an overviewof the global control architecture. Sect ions 4 and 5 de-scribe the user interface and 3D simulat ion module. Sec-t ion 6 int roduces the main characterist ics of the visionsystem under development . Experimental results of anS&R mission are presented in Sect ion 7. Sect ion 8 offersa discussion and conclusive remarks.

2 T he I -A U V developed

2.1 The autonomous underwater vehicle

The GIRONA 500 is a reconfigurable autonomous un-derwater vehicle (AUV) designed for a maximum opera-

3D Simulator HMI

4

F ig. 5 The integrated I-AUV prototype in a water tank.

3 T he Cont r ol A r chi t ect ur e

The I-AUV control architecture is composed of two ini-t ially independent architectures: the underwater vehicleand the manipulator architectures. Both of them havebeen combined into a new schema that allows for reac-t ive and deliberat ive behaviors on both subsystems. Re-act ive act ions areperformed in the low-level cont rol layerthat communicates with the real or simulated I-AUV viaan abst ract ion interface. On the other hand, the wholemission is supervised at a high-level by a Mission Con-t rol System (MCS), implemented using the Petri net for-malism. Visual percept ion services are provided by thevision module described in Sect ion 6. To integrate theheterogeneous comput ing hardware and software of allsystem components, to allow for easy integrat ion of ad-dit ional mission specific components, and to record allsensor input in a suitable playback format for simula-t ion purposes, we use the ROS Robot Operat ing System[6][7]. Vehicle cont rol, the manipulator, and the visionsystem are implemented as independent ROS nodes thatare executed on their own independent hardware unitsand that communicate through ROS messages over anonboard ethernet network. The general architecture isillust rated in Figure 6. For further detail see [8].

4 T he U ser I nt er face

The RAUVI project proposes a two-stage strategy[3]:during the first stage, the I-AUV is programmed at thesurface and receives a plan for surveying a given Regionof Interest (RoI). During the survey it collects data fromcameras and other sensors. At the end of this first stage,the I-AUV returns to the surface (or to an underwaterdocking stat ion) where thedata is ret rieved and an imagemosaic of the seabed is reconst ructed [9]. The Target ofInterest (ToI) is then ident ified on the mosaic and the in-tervent ion act ion is specified by means of a user interface

F ig. 6 An overview of the RAUVI software architecture.Communicat ions through the network are implemented viaROS messages.

described later in this sect ion. Then, during the secondstage, the I-AUV navigates again to the RoI, localizesthe target and executes the intervent ion mission in anautonomous manner.

The Graphical User Interface (GUI) is used to specifyboth the survey path and the intervent ion task. The for-mer is done by loading a geo-referenced map of the areaand indicat ing a set of waypoints (possibly using prede-fined grid-shaped trajectories). Thewaypointsaresent tothe vehicle control system that guides the robot throughthem. Figure 7a shows an example of a grid-shaped t ra-jectory superposed on a generated mosaic obtained dur-ing the experiments described later in this paper. Oncethe mosaic has been built , the user first looks for thetarget of interest on it . After select ing the target , the in-tervent ion task is indicated by choosing between differentpre-programmed act ions such as grasping, hooking, etc.

The user interface contains built -in image process-ing and grasp planning algorithms that automate thetask specificat ion process when possible. If automat icmethods fail, the user can always specify the task pa-rameters manually. For the experiments described herewe consider a hooking task, which we define as enclosingthe target of interest in a bounding box, and select ingthe point and the direct ion where to at tach the hook, asshown in Figure 7b.

Timeline

12/04/2011 DPI, Madrid 4

5/9/2011

3

R a u v i /Reconfigurable

Autonomous Underwater

Vehicles for Intervention

12/04/2011 DPI, Madrid 5

I-AUV Architecture

R a u v i /Reconfigurable

Autonomous Underwater

Vehicles for Intervention

12 ABR 2011 Jornadas DPI 6

I-AUV Integration (UdG, March 2011)

5/9/2011

4

MAMSUIT

MAIN RESULTS ABOUT MANIPULATION

1. A complete arm-hand system is now ready to use for

working standalone or integrated over the developed AUV

“GIRONA 500”.

IEEE Trans. on Mechatronics (2011)

2. Autonomous Manipulation Methodology Adapted to

Underwater Scenarios

Autonomous Robots (2; 2010)

Robotic and Autonomous Systems (2011 )

3. HMI & 3D Simulator

Intelligent Service Robotics (2011)

April 12, 2011 DPI, Madrid 7

MAMSUIT

JUL 2010 2009

Hydraulic Arm

(Robotnik)

Light-Weight

ARM 5 E (CSIP)

JAN 2010

Meeting

(CSIP, UK)

NOV 2009

ARM 5 E

(CSIP)

Comparative

Study


MECHATRONICS

5/9/2011

5

MAMSUIT

D-H PARAMETERS


MECHATRONICS

MAMSUIT


Manipulation SOFTWARE

5/9/2011

6

MAMSUIT

HMI & 3D Simulator

(1)

SU

RV

EY

(2) INTERVENTION

(1) (2)


MAMSUIT

Pedro J Sanz

WP7

Multisensory Based

Manipulation

Architecture

GIRONA 2011 1st Year Review Meeting

Marine Robot and Dexterous Manipulatin for Enabling Multipurpose Intevention Missions

IRS Lab http://www.irs.uji.es/

http://www.irs.uji.es/

http://www.irs.uji.es/

5/9/2011

7

Intervention Scenarios 2

01

1

ENE

FEB

M

AR

A

BR

M

AY

JUN

JU

L A

GO

SE

P

OC

T N

OV

D

EC

20

12

EN

E FE

B

MA

R

AB

R

MA

Y JU

N

JUL

AG

O

SEP

O

CT

NO

V

DEC

2

01

3

ENE

FEB

18

FIXED-BASE MANIPULATION

M2 M3

I-AUV READY

24 34

M5

FREE-FLOATING MANIPULATION

“Object recovery from a fixed base manipulator”

“Object recovery from the free floating I-AUV developed system”

ESC

ENA

RIO

S

• Isolated objects • Target perturbations • Perturbations on the base • Water tank conditions

“Object recovery from the available

I-AUV (station-keeping)”??

Annex 1 (p 57) Annex 1 (p 57)

• Overlapped objects • Control panel operations • Avoiding obstacles • Water tank conditions

• Overlapped objects • Control panel operations • Avoiding obstacles • Seabed conditions

Level of complexity +

WP6: Hand+Arm Mechatronics System

and Control UNIBO

WP7: Multisensory Based Manipulation

Architecture UJI

WP4: Visual/Acoustic Image Processing

UIB

WP5: Floating Manipulation UNIGE-ISME

WP1: Navigation and Mapping

UdG

WP8: Dissemination, Education and Training UdG

WP9: Project Coordination and Management UJI

WP3: Vehicles Intelligent Control

Architecture HWU

WP’s Relationships

UdG, SPAIN 14 May 5th, 2011

WP2: Single and Multiple Vehicles

Control IST

5/9/2011

8

MAMSUIT

Increasing the performance, focused on the physical interaction problem

First experiments, in controlled conditions (e.g. recovery a specific object has been demonstrated

Long Term Objective Related with

Grasping / manipulation in realistic conditions conditions (i.e. overlapping, bad visibility, etc.)

WP’7

MAMSUIT WP7 - Multisensory Based Manipulation Architecture

The Aim

May 5th, 2011 1stAPR, Girona 16

A new methodology for

multipurpose manipulation was

successfully proved and now

we want to adapt it within

underwater robotics scenario

WP’7

5/9/2011

9


T7.1 Sensor Integration [UJI 8]. Months 1 to 12 (12 months)

Multisensory-based framework for the specification and

robust control of physical interaction tasks, where the

grasp and the task are jointly considered on the basis of

the Task Frame Formalism (TFF) [Bruyninckx & De

Schutter, 96] and the Knowledge-based approach to

grasping [Stansfield, 91]

The Physical Interaction

Framework Methodology [Prats et al., 2010]



Our previous work…

5/9/2011

10



May 5th, 2011 19 The physical interaction frames

Moving to underwater…



Under-constrained

grasps

• Do not fix all the 6 DOF for the

grasp

• Use them for secondary tasks •Keep the hand in the camera view

•Avoid occluding the object

•Control center of gravity

May 5th, 2011 20

5/9/2011

11



May 5th, 2011 21

The grasp is based on the knowledge-

based approach to grasping [Stansfield,

91]

Let 𝒫 = 𝑚0, 𝑚1, … , 𝑚𝑛 represent a hand preshape

(either prehensile or non-prehensile), where 𝑚𝑖 is the

desired value for each of the n DOF's of the hand.

The grasp is then defined as: 𝒢 = 𝒫, 𝐻, 𝐺, 𝐌𝐺∗

𝐻 , 𝐒𝐶

𝐒𝐶 is a a 6 x 6 diagonal selection matrix



May 5th, 2011 22

The physical interaction

frames for a grasping

action

S𝑐 = diag 1, 1, 1, 1, 0, 1

5/9/2011

12



May 5th, 2011 23

The consideration of under-constrained

grasps presents several advantages in

the context of underwater manipulation

Real-time low-level grasp synthesis controllers

will be able to exploit this free DOF in order to

achieve a suitable configuration of the whole

vehicle-arm kinematic chain.



May 5th, 2011 24

Task Specification

The task requires performing compliant motion,

following a set of velocity-force references defined in

the task frame, according to the TFF. It is defined as

follows:

𝒯 = T, v𝑇∗ , f𝑇

∗, S𝑓

5/9/2011

13

MAMSUIT

ROS interfaces (rxgraph output)


WP7 - Multisensory Based Manipulation Architecture

T7.2 Specification of interfaces [UJI 11] [UNIBO 1] [UNIGE-ISME 1]. Months 7 to 18 (11

months)


Milestone

no. Milestone name Delivery date Comments

Means of verification

2 Object

recovery from

a fixed base

manipulator

18 Experimental results using

a fixed industrial robot

arm, but simulating the

underwater conditions

Del. no. Deliverable name Dissemi-

nation

level

Delivery date

(proj.month)

D7.1 Technical report on the

methodology aspects and

requirements on the Multisensory

and knowledge-based approach

architecture for grasping and

dexterous manipulation

PU 18

1stAPR, Girona 26 May 5th, 2011

WP’7 Milestone2

5/9/2011

14

MAMSUIT

The Roadmap Realism

Complexity 3D Simulator

Fixed manipulator

Manipulator inside the water under disturbances


WP’7 Milestone2

MAMSUIT

Initial concept


WP’7 Milestone2

5/9/2011

15

MAMSUIT

Initial concept

Current concept


WP’7 Milestone2

MAMSUIT

Simulation experiments on grasping under vehicle

disturbances

Visual Tracking and control for manipulation

WP’7 Milestone2

The hand is controlled in order to keep a given relative pose with respect to

the object

5/9/2011

16

MAMSUIT

Simulated I-AUV

Underwater simulation

Virtual camera sensor

Virtual joint position sensors

Two arm models: 5DOF and

7 DOF

ROS interfaces:

Set/Get vehicle pose

Set/Get arm joints

Get camera image

May 5th, 2011 31

WP’7 Milestone2

MAMSUIT

1stAPR, Girona 32

WP’7 Milestone2

Un

de

rw

ate

r sim

ulatio

n

5/9/2011

17

MAMSUIT

M2 – Object recovery

from a fixed-base

manipulator

1st Successful Experiments on Tracking,

Visual Servoing and arm control for

grasping

• Vision sensors

• Force/torque sensor

• Tactile sensors


WP’7 Milestone2

MAMSUIT

Object recovery from a fixed base manipulator (M2)

CALIBRATION APPROACHING GRASPING

WP’7 Milestone2

5/9/2011

18

MAMSUIT

Autonomous hooking sequence of

a flight data recorder prototype in

water tank conditions, with the

arm mounted on an aluminium

structure and under manual

disturbances

WP’7 Milestone2

MAMSUIT

1. M. Prats, P.J. Sanz and A.P del Pobil. “A framework for compliant physical interaction: the grasp

meets the task”. Journal of Autonomous Robots (Special Issue on autonomous mobile

manipulation), 28(1), pp. 89-111, 2010.

2. M. Prats, P.J. Sanz and A.P. del Pobil. “Reliable non-prehensile door opening through the

combination of vision, tactile and force feedback”. Journal of Autonomous Robots, 29(2), pp.

201-218, August 2010.

3. M. Prats, J.C. García, R. Marin and P.J. Sanz. “Autonomous Grasping in Underwater

Environments: A Case Study on the Object Recovery Problem”. Special Issue "Autonomous

Grasping"- Robotic and Autonomous Systems Journal. (Submitted, January 2011).

4. M. Prats, D. Ribas, N. Palomeras, J. C. García, V. Nannen, J. J. Fernández, J. P. Beltrán, R.

Campos, P. Ridao, P. J. Sanz, G. Oliver, M. Carreras, N. Gracias, R. Marín, A. Ortiz.

“Reconfigurable AUV for Intervention Missions: A Case Study on Underwater Object Recovery”.

Journal of Intelligent Service Robotics, Sp. Issue on Marine Robotic Systems. (Submitted,

March 2011).

5. J. J. Fernández, M. Prats, P. J. Sanz, J. C. García, R. Marín, and Mike Robinson. “A New

Underwater Robot Arm for Shallow Water Intervention Missions”. IEEE/ASME Trans. on

Mechatronics, Focused Section on Marine Mechatronic Systems. (Submitted, April 2011)

EURON “10th G. Giralt PhD Award”: PhD Thesis of M. Prats

WP’7 Dissemination

D:/Trabajo/MICINN/DPI/DPI2008-06548-C03-01/Seguimiento-DPI-ABR2011/Presenta/CSIP with disturbance.mp4

5. underwater manipulationsanzp/irs/autrobmanipula-2011-4.pdf · 5/9/2011 2 rauvi dpi2008-06548-c03...

Documents