Abstract— This paper presents a control architecture foran underwater vehicle-manipulator system (UVMS) to enablesimultaneous tracking of end-effector configuration and contactforce during floating-base manipulation. The main feature ofthe architecture is its combination of a task-priority (TP) kine-matic control algorithm with a custom force control strategy,based on impedance (admittance) control. The TP algorithmused in the work includes recent treatment of equality andinequality tasks as well as original concepts to handle operationin singular configurations of the system. In the force controlpart the impedance concept is extended to allow for directcontrol over the value of exerted force and torque. Additionalfeed-forward signal is used to ensure stable contact. Theperformance of the control architecture is demonstrated byexperiments in a test tank, with GIRONA500 I-AUV performingpipe inspection.

I. INTRODUCTION

Underwater intervention at sea remains an extremelycostly and timely operation, with the use of large and heavyremotely operated vehicles (ROV) and special dynamic po-sitioning (DP) vessels. In the last three decades, researchin autonomous underwater robots and robotic interventionis slowly gaining speed. It all started with the pioneeringworks of OTTER, ODIN [6], UNION [18] and AMADEUS[12] but the first field demonstrations arrived during theALIVE project [9], where an autonomous underwater ve-hicle (AUV) docked to a subsea intervention panel usinghydraulic grasps and performed fixed-base valve turning.Similar experiments were later presented in the TRITON[16] project. First intervention from a floating vehicle wasachieved in the SAUVIM [13] project. An intervention-AUV(I-AUV) autonomously located and hooked an object whilehovering, to recover it from the seabed. First multipurposeobject search and recovery strategy was demonstrated in theTRIDENT [21] project, with experiments both in the watertank and in the harbour environment. The vehicle used inthe trials was GIRONA500, equipped with a 4 DOF armor a 7 DOF manipulator with a 3 finger hand. Next, thePANDORA project has demonstrated autonomous floating-base valve turning on a subsea panel, first using a learning bydemonstration (LbD) paradigm [4] and later utilising morestandard task-priority (TP) kinematic control [7]. Finally, the

*Patryk Cieslak has received funding from the European Union’s Horizon2020 research and innovation programme under the Marie Sklodowska-Curie grant agreement no. 750063.

1Patryk Cieslak is with the Computer Vision and Robotics ResearchInstitute, Scientific and Technological Park of the University of Girona,CIRS lab, 17003 Girona, Spain patryk.cieslak@udg.edu

2Pere Ridao is with the Computer Vision and Robotics Research Institute,Scientific and Technological Park of the University of Girona, CIRS lab,17003 Girona, Spain pere.ridao.rodriguez@gmail.com

MARIS project [23] has shown detection and grasping of apipe on the seabed, with a specially designed 3 finger hand.

Above projects have focused on controlling the positionof the robot end-effector (EE). Although some I-AUVswere equipped with force/torque sensors, they were onlyused to detect contact and aid in triggering actions [11],[23]. Moreover, all experimental trials involved grasping andrecovering objects or manipulating valves and connectors.There is no example of a task which relies on accurate controlof contact forces, like the presented here pipe inspection,highly relevant to the oil and gas industry. However, sometheoretical works with simulated experiments can be foundin the literature. In [8] a unified force control for an I-AUVis shown. The authors use dynamic model of the system andinterleve impedance force control with hybrid position/forcecontrol, to allow tracking of the desired contact force and atthe same time ensure soft behaviour on approach. The twostrategies are switched with fuzzy logic. A similar approachwas also presented in [2]. A different algorithm, based onpassivity concept, is proposed in [15]. The dynamic model ofan I-AUV is not needed by the controller and the kinematicconstraints arising from the contact with environment areexplicitly included when solving for system velocities. Au-thors present robustness of the algorithm to fluid disturbancesand obstacle avoidance capabilities. In [10] a model basedmultiple impedance control is used to manipulate an objectwith a dual-arm I-AUV. Next, a force control algorithm,avoiding dynamic modelling, is shown in [5] for the task ofvalve turning and button pushing, simulated on a model ofa 13 DOF I-AUV. Finally, simulated results of autonomousunderwater pipe inspection, including contact force control,were shown in the context of the DexROV project [22].

In this paper, a control architecture is presented, to enablerobust and safe underwater manipulation from a floating I-AUV, especially in the case where the interaction forces haveto be controlled and the contact should be maintained duringthe whole operation. The paper is organised as follows.Section II presents the general control architecture of ourI-AUV system, while Sections III and IV focus on EEforce and position control parts respectively. These twosections present the main contributions of the paper (inbold). The proposed theory is put into practice in SectionV, which presents preliminary experimental results of a pipeinspection operation, obtained with GIRONA500 I-AUV ina test tank. These results constitute, best to our knowledge,the first documented experimental example of autonomousunderwater floating manipulation with contact force control.The work is concluded in Section VI.

Adaptive Admittance Control in Task-Priority Framework for Contact Force Control in Autonomous Underwater Floating Manipulation*

Patryk Cieslak1 and Pere Ridao2

II. CONTROL ARCHITECTURE

The control architecture of the robot is schematicallypresented in Fig. 1. The trajectory generator outputs desiredvalues of the EE configuration and wrench. The desiredEE configuration is modified by the contact force controlblock, based on the measurements from the force/torque (FT)sensor located in the manipulator wrist. Then, it is passedto the kinematic control layer which implements the TPalgorithm and generates velocities for the system to follow,in all controlled DOF. The velocity control layer is based onproportional-integral-differential (PID) controllers and feed-forward compensation of thruster dynamics. The manipulatorhas its own built-in velocity controllers. The thrust controllayer is open-loop and implements nonlinear relationshipbetween the requested wrench and the commanded thrusterset-points. Moreover, there is a feed-forward wrench signalgenerated by the contact force control.

Kinematic control

Contact force

controlVelocity control

Trajectory generator

hee,d<latexit sha1_base64="Gjl5NdW6oRSSJ0MkGoXbe6OQi/s=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSJ4kJKKoN6KXjxWMLbQhrLZTNqlm03c3Qgl9E948aDi1d/jzX/jts1BWx8MPN6bYWZekAqujet+O0vLK6tr66WN8ubW9s5uZW//QSeZYuixRCSqHVCNgkv0DDcC26lCGgcCW8HwZuK3nlBpnsh7M0rRj2lf8ogzaqzUHvRyxNNw3KtU3Zo7BVkk9YJUoUCzV/nqhgnLYpSGCap1p+6mxs+pMpwJHJe7mcaUsiHtY8dSSWPUfj69d0yOrRKSKFG2pCFT9fdETmOtR3FgO2NqBnrem4j/eZ3MRJd+zmWaGZRstijKBDEJmTxPQq6QGTGyhDLF7a2EDaiizNiIyjaE+vzLi8Q7q13V3LvzauO6SKMEh3AEJ1CHC2jALTTBAwYCnuEV3pxH58V5dz5mrUtOMXMAf+B8/gBbUo+2</latexit><latexit sha1_base64="Gjl5NdW6oRSSJ0MkGoXbe6OQi/s=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSJ4kJKKoN6KXjxWMLbQhrLZTNqlm03c3Qgl9E948aDi1d/jzX/jts1BWx8MPN6bYWZekAqujet+O0vLK6tr66WN8ubW9s5uZW//QSeZYuixRCSqHVCNgkv0DDcC26lCGgcCW8HwZuK3nlBpnsh7M0rRj2lf8ogzaqzUHvRyxNNw3KtU3Zo7BVkk9YJUoUCzV/nqhgnLYpSGCap1p+6mxs+pMpwJHJe7mcaUsiHtY8dSSWPUfj69d0yOrRKSKFG2pCFT9fdETmOtR3FgO2NqBnrem4j/eZ3MRJd+zmWaGZRstijKBDEJmTxPQq6QGTGyhDLF7a2EDaiizNiIyjaE+vzLi8Q7q13V3LvzauO6SKMEh3AEJ1CHC2jALTTBAwYCnuEV3pxH58V5dz5mrUtOMXMAf+B8/gBbUo+2</latexit><latexit sha1_base64="Gjl5NdW6oRSSJ0MkGoXbe6OQi/s=">AAAB7nicbVBNS8NAEJ34WetX1aOXxSJ4kJKKoN6KXjxWMLbQhrLZTNqlm03c3Qgl9E948aDi1d/jzX/jts1BWx8MPN6bYWZekAqujet+O0vLK6tr66WN8ubW9s5uZW//QSeZYuixRCSqHVCNgkv0DDcC26lCGgcCW8HwZuK3nlBpnsh7M0rRj2lf8ogzaqzUHvRyxNNw3KtU3Zo7BVkk9YJUoUCzV/nqhgnLYpSGCap1p+6mxs+pMpwJHJe7mcaUsiHtY8dSSWPUfj69d0yOrRKSKFG2pCFT9fdETmOtR3FgO2NqBnrem4j/eZ3MRJd+zmWaGZRstijKBDEJmTxPQq6QGTGyhDLF7a2EDaiizNiIyjaE+vzLi8Q7q13V3LvzauO6SKMEh3AEJ1CHC2jALTTBAwYCnuEV3pxH58V5dz5mrUtOMXMAf+B8/gBbUo+2</latexit>

⌘ee,d<latexit sha1_base64="c5vxkEjX/lBHrsb+XXh4G/07lS0=">AAAB8XicbVBNS8NAEN3Ur1q/qh69BIvgQUoignorevFYwdhCGspmM2mXbjZhdyKU0J/hxYOKV/+NN/+N2zYHbX0w8Hhvhpl5YSa4Rsf5tiorq2vrG9XN2tb2zu5eff/gUae5YuCxVKSqG1INgkvwkKOAbqaAJqGATji6nfqdJ1Cap/IBxxkECR1IHnNG0Uh+D5D2C4CzaNKvN5ymM4O9TNySNEiJdr/+1YtSlicgkQmqte86GQYFVciZgEmtl2vIKBvRAfiGSpqADorZyRP7xCiRHafKlER7pv6eKGii9TgJTWdCcagXvan4n+fnGF8FBZdZjiDZfFGcCxtTe/q/HXEFDMXYEMoUN7fabEgVZWhSqpkQ3MWXl4l33rxuOvcXjdZNmUaVHJFjckpcckla5I60iUcYSckzeSVvFlov1rv1MW+tWOXMIfkD6/MHnWeRAg==</latexit><latexit sha1_base64="c5vxkEjX/lBHrsb+XXh4G/07lS0=">AAAB8XicbVBNS8NAEN3Ur1q/qh69BIvgQUoignorevFYwdhCGspmM2mXbjZhdyKU0J/hxYOKV/+NN/+N2zYHbX0w8Hhvhpl5YSa4Rsf5tiorq2vrG9XN2tb2zu5eff/gUae5YuCxVKSqG1INgkvwkKOAbqaAJqGATji6nfqdJ1Cap/IBxxkECR1IHnNG0Uh+D5D2C4CzaNKvN5ymM4O9TNySNEiJdr/+1YtSlicgkQmqte86GQYFVciZgEmtl2vIKBvRAfiGSpqADorZyRP7xCiRHafKlER7pv6eKGii9TgJTWdCcagXvan4n+fnGF8FBZdZjiDZfFGcCxtTe/q/HXEFDMXYEMoUN7fabEgVZWhSqpkQ3MWXl4l33rxuOvcXjdZNmUaVHJFjckpcckla5I60iUcYSckzeSVvFlov1rv1MW+tWOXMIfkD6/MHnWeRAg==</latexit><latexit sha1_base64="c5vxkEjX/lBHrsb+XXh4G/07lS0=">AAAB8XicbVBNS8NAEN3Ur1q/qh69BIvgQUoignorevFYwdhCGspmM2mXbjZhdyKU0J/hxYOKV/+NN/+N2zYHbX0w8Hhvhpl5YSa4Rsf5tiorq2vrG9XN2tb2zu5eff/gUae5YuCxVKSqG1INgkvwkKOAbqaAJqGATji6nfqdJ1Cap/IBxxkECR1IHnNG0Uh+D5D2C4CzaNKvN5ymM4O9TNySNEiJdr/+1YtSlicgkQmqte86GQYFVciZgEmtl2vIKBvRAfiGSpqADorZyRP7xCiRHafKlER7pv6eKGii9TgJTWdCcagXvan4n+fnGF8FBZdZjiDZfFGcCxtTe/q/HXEFDMXYEMoUN7fabEgVZWhSqpkQ3MWXl4l33rxuOvcXjdZNmUaVHJFjckpcckla5I60iUcYSckzeSVvFlov1rv1MW+tWOXMIfkD6/MHnWeRAg==</latexit>

⌘ee,d<latexit sha1_base64="E9zeu8EncpNxkKA4uxRrXR7hU88=">AAAB+XicbVBNS8NAEN34WetXqkcvwSJ4kJKIoN6KXjxWMLbQhLDZTNqlm03Y3Sgl5qd48aDi1X/izX/jts1BWx8MPN6bYWZemDEqlW1/G0vLK6tr67WN+ubW9s6u2di7l2kuCLgkZanohVgCoxxcRRWDXiYAJyGDbji6nvjdBxCSpvxOjTPwEzzgNKYEKy0FZsMLsSg8ULgMCoCTqAzMpt2yp7AWiVORJqrQCcwvL0pJngBXhGEp+46dKb/AQlHCoKx7uYQMkxEeQF9TjhOQfjE9vbSOtBJZcSp0cWVN1d8TBU6kHCeh7kywGsp5byL+5/VzFV/4BeVZroCT2aI4Z5ZKrUkOVkQFEMXGmmAiqL7VIkMsMFE6rboOwZl/eZG4p63Lln171mxfVWnU0AE6RMfIQeeojW5QB7mIoEf0jF7Rm/FkvBjvxsesdcmoZvbRHxifP+nkk/g=</latexit><latexit sha1_base64="E9zeu8EncpNxkKA4uxRrXR7hU88=">AAAB+XicbVBNS8NAEN34WetXqkcvwSJ4kJKIoN6KXjxWMLbQhLDZTNqlm03Y3Sgl5qd48aDi1X/izX/jts1BWx8MPN6bYWZemDEqlW1/G0vLK6tr67WN+ubW9s6u2di7l2kuCLgkZanohVgCoxxcRRWDXiYAJyGDbji6nvjdBxCSpvxOjTPwEzzgNKYEKy0FZsMLsSg8ULgMCoCTqAzMpt2yp7AWiVORJqrQCcwvL0pJngBXhGEp+46dKb/AQlHCoKx7uYQMkxEeQF9TjhOQfjE9vbSOtBJZcSp0cWVN1d8TBU6kHCeh7kywGsp5byL+5/VzFV/4BeVZroCT2aI4Z5ZKrUkOVkQFEMXGmmAiqL7VIkMsMFE6rboOwZl/eZG4p63Lln171mxfVWnU0AE6RMfIQeeojW5QB7mIoEf0jF7Rm/FkvBjvxsesdcmoZvbRHxifP+nkk/g=</latexit><latexit sha1_base64="E9zeu8EncpNxkKA4uxRrXR7hU88=">AAAB+XicbVBNS8NAEN34WetXqkcvwSJ4kJKIoN6KXjxWMLbQhLDZTNqlm03Y3Sgl5qd48aDi1X/izX/jts1BWx8MPN6bYWZemDEqlW1/G0vLK6tr67WN+ubW9s6u2di7l2kuCLgkZanohVgCoxxcRRWDXiYAJyGDbji6nvjdBxCSpvxOjTPwEzzgNKYEKy0FZsMLsSg8ULgMCoCTqAzMpt2yp7AWiVORJqrQCcwvL0pJngBXhGEp+46dKb/AQlHCoKx7uYQMkxEeQF9TjhOQfjE9vbSOtBJZcSp0cWVN1d8TBU6kHCeh7kywGsp5byL+5/VzFV/4BeVZroCT2aI4Z5ZKrUkOVkQFEMXGmmAiqL7VIkMsMFE6rboOwZl/eZG4p63Lln171mxfVWnU0AE6RMfIQeeojW5QB7mIoEf0jF7Rm/FkvBjvxsesdcmoZvbRHxifP+nkk/g=</latexit>

⇣<latexit sha1_base64="YWxsPsvuGFfP0nI7mTp+9bUXkeU=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQa+hu8eFDx6h/y5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTjPsskYluh9RwKRT3UaDk7VRzGoeSt8LRzdRvPXJtRKLucZzyIKYDJSLBKFrJ7z5xpL1qza27M5Bl4hWkBgWavepXt5+wLOYKmaTGdDw3xSCnGgWTfFLpZoanlI3ogHcsVTTmJshnx07IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fJMLoMcqHSDLli80VRJgkmZPo56QvNGcqxJZRpYW8lbEg1ZWjzqdgQvMWXl4l/Vr+qu3fntcZ1kUYZjuAYTsGDC2jALTTBBwYCnuEV3hzlvDjvzse8teQUM4fwB87nD1T2jo8=</latexit><latexit sha1_base64="YWxsPsvuGFfP0nI7mTp+9bUXkeU=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQa+hu8eFDx6h/y5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTjPsskYluh9RwKRT3UaDk7VRzGoeSt8LRzdRvPXJtRKLucZzyIKYDJSLBKFrJ7z5xpL1qza27M5Bl4hWkBgWavepXt5+wLOYKmaTGdDw3xSCnGgWTfFLpZoanlI3ogHcsVTTmJshnx07IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fJMLoMcqHSDLli80VRJgkmZPo56QvNGcqxJZRpYW8lbEg1ZWjzqdgQvMWXl4l/Vr+qu3fntcZ1kUYZjuAYTsGDC2jALTTBBwYCnuEV3hzlvDjvzse8teQUM4fwB87nD1T2jo8=</latexit><latexit sha1_base64="YWxsPsvuGFfP0nI7mTp+9bUXkeU=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQa+hu8eFDx6h/y5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTjPsskYluh9RwKRT3UaDk7VRzGoeSt8LRzdRvPXJtRKLucZzyIKYDJSLBKFrJ7z5xpL1qza27M5Bl4hWkBgWavepXt5+wLOYKmaTGdDw3xSCnGgWTfFLpZoanlI3ogHcsVTTmJshnx07IiVX6JEq0LYVkpv6eyGlszDgObWdMcWgWvan4n9fJMLoMcqHSDLli80VRJgkmZPo56QvNGcqxJZRpYW8lbEg1ZWjzqdgQvMWXl4l/Vr+qu3fntcZ1kUYZjuAYTsGDC2jALTTBBwYCnuEV3hzlvDjvzse8teQUM4fwB87nD1T2jo8=</latexit>

q<latexit sha1_base64="B+Oxz1w267pLl0MTEnTJ+q6fxtQ=">AAAB7XicbVBNS8NAFHypX7V+VT16WSyCp5KIoN6KXjxWMLbQhrLZbtqlm03cfRFK6I/w4kHFq//Hm//GbZuDtg4sDDNv2PcmTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTjPsskYluh9RwKRT3UaDk7VRzGoeSt8LRzdRvPXFtRKLucZzyIKYDJSLBKFqp1e0nmD9OetWaW3dnIMvEK0gNCjR71S+bZFnMFTJJjel4bopBTjUKJvmk0s0MTykb0QHvWKpozE2Qz9adkBOr9EmUaPsUkpn6O5HT2JhxHNrJmOLQLHpT8T+vk2F0GeRCpRlyxeYfRZkkmJDp7aQvNGcox5ZQpoXdlbAh1ZShbahiS/AWT14m/ln9qu7endca10UbZTiCYzgFDy6gAbfQBB8YjOAZXuHNSZ0X5935mI+WnCJzCH/gfP4AIF+Pnw==</latexit><latexit sha1_base64="B+Oxz1w267pLl0MTEnTJ+q6fxtQ=">AAAB7XicbVBNS8NAFHypX7V+VT16WSyCp5KIoN6KXjxWMLbQhrLZbtqlm03cfRFK6I/w4kHFq//Hm//GbZuDtg4sDDNv2PcmTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTjPsskYluh9RwKRT3UaDk7VRzGoeSt8LRzdRvPXFtRKLucZzyIKYDJSLBKFqp1e0nmD9OetWaW3dnIMvEK0gNCjR71S+bZFnMFTJJjel4bopBTjUKJvmk0s0MTykb0QHvWKpozE2Qz9adkBOr9EmUaPsUkpn6O5HT2JhxHNrJmOLQLHpT8T+vk2F0GeRCpRlyxeYfRZkkmJDp7aQvNGcox5ZQpoXdlbAh1ZShbahiS/AWT14m/ln9qu7endca10UbZTiCYzgFDy6gAbfQBB8YjOAZXuHNSZ0X5935mI+WnCJzCH/gfP4AIF+Pnw==</latexit><latexit sha1_base64="B+Oxz1w267pLl0MTEnTJ+q6fxtQ=">AAAB7XicbVBNS8NAFHypX7V+VT16WSyCp5KIoN6KXjxWMLbQhrLZbtqlm03cfRFK6I/w4kHFq//Hm//GbZuDtg4sDDNv2PcmTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTjPsskYluh9RwKRT3UaDk7VRzGoeSt8LRzdRvPXFtRKLucZzyIKYDJSLBKFqp1e0nmD9OetWaW3dnIMvEK0gNCjR71S+bZFnMFTJJjel4bopBTjUKJvmk0s0MTykb0QHvWKpozE2Qz9adkBOr9EmUaPsUkpn6O5HT2JhxHNrJmOLQLHpT8T+vk2F0GeRCpRlyxeYfRZkkmJDp7aQvNGcox5ZQpoXdlbAh1ZShbahiS/AWT14m/ln9qu7endca10UbZTiCYzgFDy6gAbfQBB8YjOAZXuHNSZ0X5935mI+WnCJzCH/gfP4AIF+Pnw==</latexit>

hee<latexit sha1_base64="h9WVucuDu60Os1WETtTR5DNUnZM=">AAAB7HicbVBNS8NAEJ34WetX1aOXYBE8lUQE9Vb04rGCaQttKJvtpF272Q27G6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMyLUs608bxvZ2V1bX1js7RV3t7Z3duvHBw2tcwUxYBKLlU7Iho5ExgYZji2U4UkiTi2otHt1G89odJMigczTjFMyECwmFFirNQc9nLESa9S9WreDO4y8QtShQKNXuWr25c0S1AYyonWHd9LTZgTZRjlOCl3M40poSMywI6lgiSow3x27cQ9tUrfjaWyJYw7U39P5CTRepxEtjMhZqgXvan4n9fJTHwV5kykmUFB54vijLtGutPX3T5TSA0fW0KoYvZWlw6JItTYgMo2BH/x5WUSnNeua979RbV+U6RRgmM4gTPw4RLqcAcNCIDCIzzDK7w50nlx3p2PeeuKU8wcwR84nz8y448S</latexit><latexit sha1_base64="h9WVucuDu60Os1WETtTR5DNUnZM=">AAAB7HicbVBNS8NAEJ34WetX1aOXYBE8lUQE9Vb04rGCaQttKJvtpF272Q27G6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMyLUs608bxvZ2V1bX1js7RV3t7Z3duvHBw2tcwUxYBKLlU7Iho5ExgYZji2U4UkiTi2otHt1G89odJMigczTjFMyECwmFFirNQc9nLESa9S9WreDO4y8QtShQKNXuWr25c0S1AYyonWHd9LTZgTZRjlOCl3M40poSMywI6lgiSow3x27cQ9tUrfjaWyJYw7U39P5CTRepxEtjMhZqgXvan4n9fJTHwV5kykmUFB54vijLtGutPX3T5TSA0fW0KoYvZWlw6JItTYgMo2BH/x5WUSnNeua979RbV+U6RRgmM4gTPw4RLqcAcNCIDCIzzDK7w50nlx3p2PeeuKU8wcwR84nz8y448S</latexit><latexit sha1_base64="h9WVucuDu60Os1WETtTR5DNUnZM=">AAAB7HicbVBNS8NAEJ34WetX1aOXYBE8lUQE9Vb04rGCaQttKJvtpF272Q27G6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMyLUs608bxvZ2V1bX1js7RV3t7Z3duvHBw2tcwUxYBKLlU7Iho5ExgYZji2U4UkiTi2otHt1G89odJMigczTjFMyECwmFFirNQc9nLESa9S9WreDO4y8QtShQKNXuWr25c0S1AYyonWHd9LTZgTZRjlOCl3M40poSMywI6lgiSow3x27cQ9tUrfjaWyJYw7U39P5CTRepxEtjMhZqgXvan4n9fJTHwV5kykmUFB54vijLtGutPX3T5TSA0fW0KoYvZWlw6JItTYgMo2BH/x5WUSnNeua979RbV+U6RRgmM4gTPw4RLqcAcNCIDCIzzDK7w50nlx3p2PeeuKU8wcwR84nz8y448S</latexit>

v,!<latexit sha1_base64="4VzY7gSnqCXyEmUOL8+QIa8zhZ8=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBoPgQcJGBPUW9OIxgmsCyRJmJ7PJkHmsM7OBsOQnvHhQ8er3ePNvnCR70MSChqKqm+6uKOHMWN//9gorq2vrG8XN0tb2zu5eef/g0ahUExoQxZVuRdhQziQNLLOcthJNsYg4bUbD26nfHFFtmJIPdpzQUOC+ZDEj2DqpNTrrKEH7uFuu+FV/BrRMajmpQI5Gt/zV6SmSCiot4diYds1PbJhhbRnhdFLqpIYmmAxxn7YdlVhQE2azeyfoxCk9FCvtSlo0U39PZFgYMxaR6xTYDsyiNxX/89qpja/CjMkktVSS+aI45cgqNH0e9ZimxPKxI5ho5m5FZIA1JtZFVHIh1BZfXibBefW66t9fVOo3eRpFOIJjOIUaXEId7qABARDg8Ayv8OY9eS/eu/cxby14+cwh/IH3+QM/Zo+k</latexit><latexit sha1_base64="4VzY7gSnqCXyEmUOL8+QIa8zhZ8=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBoPgQcJGBPUW9OIxgmsCyRJmJ7PJkHmsM7OBsOQnvHhQ8er3ePNvnCR70MSChqKqm+6uKOHMWN//9gorq2vrG8XN0tb2zu5eef/g0ahUExoQxZVuRdhQziQNLLOcthJNsYg4bUbD26nfHFFtmJIPdpzQUOC+ZDEj2DqpNTrrKEH7uFuu+FV/BrRMajmpQI5Gt/zV6SmSCiot4diYds1PbJhhbRnhdFLqpIYmmAxxn7YdlVhQE2azeyfoxCk9FCvtSlo0U39PZFgYMxaR6xTYDsyiNxX/89qpja/CjMkktVSS+aI45cgqNH0e9ZimxPKxI5ho5m5FZIA1JtZFVHIh1BZfXibBefW66t9fVOo3eRpFOIJjOIUaXEId7qABARDg8Ayv8OY9eS/eu/cxby14+cwh/IH3+QM/Zo+k</latexit><latexit sha1_base64="4VzY7gSnqCXyEmUOL8+QIa8zhZ8=">AAAB7nicbVDLSgNBEOyNrxhfUY9eBoPgQcJGBPUW9OIxgmsCyRJmJ7PJkHmsM7OBsOQnvHhQ8er3ePNvnCR70MSChqKqm+6uKOHMWN//9gorq2vrG8XN0tb2zu5eef/g0ahUExoQxZVuRdhQziQNLLOcthJNsYg4bUbD26nfHFFtmJIPdpzQUOC+ZDEj2DqpNTrrKEH7uFuu+FV/BrRMajmpQI5Gt/zV6SmSCiot4diYds1PbJhhbRnhdFLqpIYmmAxxn7YdlVhQE2azeyfoxCk9FCvtSlo0U39PZFgYMxaR6xTYDsyiNxX/89qpja/CjMkktVSS+aI45cgqNH0e9ZimxPKxI5ho5m5FZIA1JtZFVHIh1BZfXibBefW66t9fVOo3eRpFOIJjOIUaXEId7qABARDg8Ayv8OY9eS/eu/cxby14+cwh/IH3+QM/Zo+k</latexit>⌘, q

<latexit sha1_base64="lQCirm5P1pAqw7z4Gpay3cQSO1Q=">AAAB7XicbVBNS8NAEJ34WetX1aOXxSJ4kJKIoN6KXjxWMLbQhrLZbtqlm03cnQgl9Ed48aDi1f/jzX/jts1BWx8MPN6bYWZemEph0HW/naXlldW19dJGeXNre2e3srf/YJJMM+6zRCa6FVLDpVDcR4GSt1LNaRxK3gyHNxO/+cS1EYm6x1HKg5j2lYgEo2ilZocjPSWP3UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKfnjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wugxyodIMuWKzRVEmCSZk8jvpCc0ZypEllGlhbyVsQDVlaBMq2xC8+ZcXiX9Wu6q5d+fV+nWRRgkO4QhOwIMLqMMtNMAHBkN4hld4c1LnxXl3PmatS04xcwB/4Hz+AAeWjuY=</latexit><latexit sha1_base64="lQCirm5P1pAqw7z4Gpay3cQSO1Q=">AAAB7XicbVBNS8NAEJ34WetX1aOXxSJ4kJKIoN6KXjxWMLbQhrLZbtqlm03cnQgl9Ed48aDi1f/jzX/jts1BWx8MPN6bYWZemEph0HW/naXlldW19dJGeXNre2e3srf/YJJMM+6zRCa6FVLDpVDcR4GSt1LNaRxK3gyHNxO/+cS1EYm6x1HKg5j2lYgEo2ilZocjPSWP3UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKfnjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wugxyodIMuWKzRVEmCSZk8jvpCc0ZypEllGlhbyVsQDVlaBMq2xC8+ZcXiX9Wu6q5d+fV+nWRRgkO4QhOwIMLqMMtNMAHBkN4hld4c1LnxXl3PmatS04xcwB/4Hz+AAeWjuY=</latexit><latexit sha1_base64="lQCirm5P1pAqw7z4Gpay3cQSO1Q=">AAAB7XicbVBNS8NAEJ34WetX1aOXxSJ4kJKIoN6KXjxWMLbQhrLZbtqlm03cnQgl9Ed48aDi1f/jzX/jts1BWx8MPN6bYWZemEph0HW/naXlldW19dJGeXNre2e3srf/YJJMM+6zRCa6FVLDpVDcR4GSt1LNaRxK3gyHNxO/+cS1EYm6x1HKg5j2lYgEo2ilZocjPSWP3UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKfnjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wugxyodIMuWKzRVEmCSZk8jvpCc0ZypEllGlhbyVsQDVlaBMq2xC8+ZcXiX9Wu6q5d+fV+nWRRgkO4QhOwIMLqMMtNMAHBkN4hld4c1LnxXl3PmatS04xcwB/4Hz+AAeWjuY=</latexit>

hv<latexit sha1_base64="BTyHYoOUNGPM9oVoVSnyjtcRtvI=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262YTdSaGE/gQvHlS8+o+8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6fTJJpxn2WyES3Qmq4FIr7KFDyVqo5jUPJm+Hwduo3R1wbkahHHKc8iGlfiUgwilZ6GHRH3UrVrbkzkGXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8Uu5khqeUDWmfty1VNOYmyGenTsipVXokSrQthWSm/p7IaWzMOA5tZ0xxYBa9qfif184wugpyodIMuWLzRVEmCSZk+jfpCc0ZyrEllGlhbyVsQDVlaNMp2xC8xZeXiX9eu6659xfV+k2RRgmO4QTOwINLqMMdNMAHBn14hld4c6Tz4rw7H/PWFaeYOYI/cD5/AMkijag=</latexit><latexit sha1_base64="BTyHYoOUNGPM9oVoVSnyjtcRtvI=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262YTdSaGE/gQvHlS8+o+8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6fTJJpxn2WyES3Qmq4FIr7KFDyVqo5jUPJm+Hwduo3R1wbkahHHKc8iGlfiUgwilZ6GHRH3UrVrbkzkGXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8Uu5khqeUDWmfty1VNOYmyGenTsipVXokSrQthWSm/p7IaWzMOA5tZ0xxYBa9qfif184wugpyodIMuWLzRVEmCSZk+jfpCc0ZyrEllGlhbyVsQDVlaNMp2xC8xZeXiX9eu6659xfV+k2RRgmO4QTOwINLqMMdNMAHBn14hld4c6Tz4rw7H/PWFaeYOYI/cD5/AMkijag=</latexit><latexit sha1_base64="BTyHYoOUNGPM9oVoVSnyjtcRtvI=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262YTdSaGE/gQvHlS8+o+8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6fTJJpxn2WyES3Qmq4FIr7KFDyVqo5jUPJm+Hwduo3R1wbkahHHKc8iGlfiUgwilZ6GHRH3UrVrbkzkGXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8Uu5khqeUDWmfty1VNOYmyGenTsipVXokSrQthWSm/p7IaWzMOA5tZ0xxYBa9qfif184wugpyodIMuWLzRVEmCSZk+jfpCc0ZyrEllGlhbyVsQDVlaNMp2xC8xZeXiX9eu6659xfV+k2RRgmO4QTOwINLqMMdNMAHBn14hld4c6Tz4rw7H/PWFaeYOYI/cD5/AMkijag=</latexit>

hff<latexit sha1_base64="d3z4ef/i9HRSJMipBwt/+6UJq1E=">AAAB7HicbVDLSgNBEOz1GeMr6tHLYBA8hV0R1FvQi8cIbhJIljA7mU3GzGOZmRXCkn/w4kHFqx/kzb9xkuxBEwsaiqpuurvilDNjff/bW1ldW9/YLG2Vt3d29/YrB4dNozJNaEgUV7odY0M5kzS0zHLaTjXFIua0FY9up37riWrDlHyw45RGAg8kSxjB1knNYS9PkkmvUvVr/gxomQQFqUKBRq/y1e0rkgkqLeHYmE7gpzbKsbaMcDopdzNDU0xGeEA7jkosqIny2bUTdOqUPkqUdiUtmqm/J3IsjBmL2HUKbIdm0ZuK/3mdzCZXUc5kmlkqyXxRknFkFZq+jvpMU2L52BFMNHO3IjLEGhPrAiq7EILFl5dJeF67rvn3F9X6TZFGCY7hBM4ggEuowx00IAQCj/AMr/DmKe/Fe/c+5q0rXjFzBH/gff4ANeyPFA==</latexit><latexit sha1_base64="d3z4ef/i9HRSJMipBwt/+6UJq1E=">AAAB7HicbVDLSgNBEOz1GeMr6tHLYBA8hV0R1FvQi8cIbhJIljA7mU3GzGOZmRXCkn/w4kHFqx/kzb9xkuxBEwsaiqpuurvilDNjff/bW1ldW9/YLG2Vt3d29/YrB4dNozJNaEgUV7odY0M5kzS0zHLaTjXFIua0FY9up37riWrDlHyw45RGAg8kSxjB1knNYS9PkkmvUvVr/gxomQQFqUKBRq/y1e0rkgkqLeHYmE7gpzbKsbaMcDopdzNDU0xGeEA7jkosqIny2bUTdOqUPkqUdiUtmqm/J3IsjBmL2HUKbIdm0ZuK/3mdzCZXUc5kmlkqyXxRknFkFZq+jvpMU2L52BFMNHO3IjLEGhPrAiq7EILFl5dJeF67rvn3F9X6TZFGCY7hBM4ggEuowx00IAQCj/AMr/DmKe/Fe/c+5q0rXjFzBH/gff4ANeyPFA==</latexit><latexit sha1_base64="d3z4ef/i9HRSJMipBwt/+6UJq1E=">AAAB7HicbVDLSgNBEOz1GeMr6tHLYBA8hV0R1FvQi8cIbhJIljA7mU3GzGOZmRXCkn/w4kHFqx/kzb9xkuxBEwsaiqpuurvilDNjff/bW1ldW9/YLG2Vt3d29/YrB4dNozJNaEgUV7odY0M5kzS0zHLaTjXFIua0FY9up37riWrDlHyw45RGAg8kSxjB1knNYS9PkkmvUvVr/gxomQQFqUKBRq/y1e0rkgkqLeHYmE7gpzbKsbaMcDopdzNDU0xGeEA7jkosqIny2bUTdOqUPkqUdiUtmqm/J3IsjBmL2HUKbIdm0ZuK/3mdzCZXUc5kmlkqyXxRknFkFZq+jvpMU2L52BFMNHO3IjLEGhPrAiq7EILFl5dJeF67rvn3F9X6TZFGCY7hBM4ggEuowx00IAQCj/AMr/DmKe/Fe/c+5q0rXjFzBH/gff4ANeyPFA==</latexit>

Thrust control

U<latexit sha1_base64="RxqQx6G/q7HTV9YPHyXwtGcumcI=">AAAB53icbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMLxhbaUDbbSbt2swm7G6GE/gIvHlS8+pe8+W/ctjlo64OBx3szzMwLU8G1cd1vZ2V1bX1js7RV3t7Z3duvHBw+6CRTDH2WiES1Q6pRcIm+4UZgO1VI41BgKxzdTv3WEyrNE3lvxikGMR1IHnFGjZWafq9SdWvuDGSZeAWpQoFGr/LV7Scsi1EaJqjWHc9NTZBTZTgTOCl3M40pZSM6wI6lksaog3x26IScWqVPokTZkobM1N8TOY21Hseh7YypGepFbyr+53UyE10FOZdpZlCy+aIoE8QkZPo16XOFzIixJZQpbm8lbEgVZcZmU7YheIsvLxP/vHZdc5sX1fpNkUYJjuEEzsCDS6jDHTTABwYIz/AKb86j8+K8Ox/z1hWnmDmCP3A+fwAfZIys</latexit><latexit sha1_base64="RxqQx6G/q7HTV9YPHyXwtGcumcI=">AAAB53icbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMLxhbaUDbbSbt2swm7G6GE/gIvHlS8+pe8+W/ctjlo64OBx3szzMwLU8G1cd1vZ2V1bX1js7RV3t7Z3duvHBw+6CRTDH2WiES1Q6pRcIm+4UZgO1VI41BgKxzdTv3WEyrNE3lvxikGMR1IHnFGjZWafq9SdWvuDGSZeAWpQoFGr/LV7Scsi1EaJqjWHc9NTZBTZTgTOCl3M40pZSM6wI6lksaog3x26IScWqVPokTZkobM1N8TOY21Hseh7YypGepFbyr+53UyE10FOZdpZlCy+aIoE8QkZPo16XOFzIixJZQpbm8lbEgVZcZmU7YheIsvLxP/vHZdc5sX1fpNkUYJjuEEzsCDS6jDHTTABwYIz/AKb86j8+K8Ox/z1hWnmDmCP3A+fwAfZIys</latexit><latexit sha1_base64="RxqQx6G/q7HTV9YPHyXwtGcumcI=">AAAB53icbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMLxhbaUDbbSbt2swm7G6GE/gIvHlS8+pe8+W/ctjlo64OBx3szzMwLU8G1cd1vZ2V1bX1js7RV3t7Z3duvHBw+6CRTDH2WiES1Q6pRcIm+4UZgO1VI41BgKxzdTv3WEyrNE3lvxikGMR1IHnFGjZWafq9SdWvuDGSZeAWpQoFGr/LV7Scsi1EaJqjWHc9NTZBTZTgTOCl3M40pZSM6wI6lksaog3x26IScWqVPokTZkobM1N8TOY21Hseh7YypGepFbyr+53UyE10FOZdpZlCy+aIoE8QkZPo16XOFzIixJZQpbm8lbEgVZcZmU7YheIsvLxP/vHZdc5sX1fpNkUYJjuEEzsCDS6jDHTTABwYIz/AKb86j8+K8Ox/z1hWnmDmCP3A+fwAfZIys</latexit>

Fig. 1. Scheme of the control architecture of GIRONA500 I-AUV. Thesymbols used in the drawing are explained in the text. Work presented inthis paper concerns the yellow coloured blocks.

III. CONTACT FORCE CONTROL

A. Adaptive admittance controlThe contact force control layer is based on impedance

control. In case of industrial manipulators, impedance controlusually generates inputs to an inverse dynamics algorithm.However, in our case, it is separated from the motion control,which is model-free. This control scheme is often calledthe admittance control [19]. It is based on the concept of acompliant frame ηee,d attached to the stiff desired EE frameηee,d by a virtual linear mass-spring-damper system, withdynamics described by

M ¨ηee +KD˙ηee +KS ηee = hee

ηee,d = ηee,d + ηee,(1)

where M , KD, KS are impedance parameters (constant,diagonal matrices), ηee is the pose error between the stiffand the compliant EE frame, where η = [η1 η2]

T=

[x y z | φ θ ψ]T , and hee =

[fTee, τ

Tee

]Tis the EE contact

wrench, measured by the FT sensor installed in the robotwrist. The algorithm allows for a good control over thecharacter of contact between the EE and the environment,securing the robot and the manipulated objects against dam-age and allowing for uncertainty in the estimation of the pose

of both. The most obvious drawback of the algorithm is thelack of direct control over the contact wrench value. To fixit we replace the constant stiffness KS with a variable one,given by

KS(t) = KP

∫(hee,d(t)− hee(t)) dt, (2)

where hee,d is the desired EE wrench and KP is a diagonalgain matrix. All impedance parameters, together with theinitial KS and KP have to be tuned during experimentaltrials.

B. Feedforward force extensionDue to the fact that our actual motion control is based

on kinematic control, its performance is limited by the per-formance of the velocity control layer. Accurately trackingvelocities is especially difficult with the I-AUV floating at alltimes, susceptible to noise, nonlinearities and delays in thenavigation and the thrust control layer. Therefore, althoughin theory it is possible to track the desired force usingonly adaptive admittance, in reality we need an additionalcomponent which ensures that the EE is always pushing onthe inspected surface, stabilising the system at the same time.One has to realise, that in the steady-state the contact forceis always generated by the vehicle’s thrusters. Even if themanipulator motion is the source of initial force impulseit is eventually necessary to counteract this impulse withthe vehicle thrust. There is only a short transient periodwhen the vehicle inertia and drag helps the manipulator togenerate force. This observation led to the idea of generatingfeedforward force and torque components, passed directly tothe vehicle thrust control layer. The robot is modelled as arigid body that is in contact with the environment at the EEframe {E} origin and the contact force is equal to the desiredforce vector. Gravity and buoyancy was not included in themodel, i.e., non-zero vehicle pitch and roll angles do notgenerate contact force. The wrench that has to be generatedby the thrusters is then simply given by

hff =

[NRBfee,d

BrBE ×(NRBfee,d

)] , (3)

where NRB is a rotation matrix from the world frame {N}to the vehicle body frame {B}, aligned with robot centre ofgravity (CG), fee,d is the desired contact force and BrBEis a vector from the origin of frame {B} to the origin ofthe EE frame {E}, defined in frame {B}. Which elementsof the feedforward torque can be generated depends onthe number and configuration of the vehicle’s thrusters. Inreality, the thrust control is far from perfect and there isalways a misalignment between EE frame and the surfacenormal vector, therefore only a fraction of (3) is used, justto maintain contact.

IV. KINEMATIC CONTROLThe objective of the kinematic control is to find a vector

of desired quasi-velocities ζ which will drive the EE at adesired velocity xE to achieve a certain goal:

xE = J(q)ζ, (4)

where xE =[vTE ωTE

]T, J(q) constitutes the Jacobian matrix

with q =[ηT qT

]T, where q is a vector of positions of

manipulator joints, and ζ =[νT qT

]T. A solution to (4) is

obtained by computing the inverse of the Jacobian matrix.In case of a redundant system, like an I-AUV, the Jacobianmatrix is non-square so the Moore-Penrose pseudo inversehas to be used [14]:

ζ = J†(q)xE +(I − J†(q)J(q)

)y, (5)

where y is an arbitrary vector and(I − J†(q)J(q)

)= P is

called a null-space projector.

A. Task-priority redundancy resolutionAccording to (5), in case of a redundant system, it is

possible to generate velocities in the null space of thesolution, which will not affect the main task, by definingthe y vector. In general, let us consider a system with ncontrollable DOF and define a task variable x1 ∈ Rm1

related to its corresponding quasi-velocities vector by

x1 = J(q)1ζ, (6)

where J(q)1 ∈ Rm1×n is the Jacobian of the task.Solving for ζ gives

ζ = J†1(q)x1 +(I − J†1(q)J1(q)

)y. (7)

Let us also define a secondary task variable x2 ∈ Rm2 , withits accompanying Jacobian J2(q) ∈ Rm2×n related by

x2 = J(q)2ζ. (8)

We are now looking for the y vector which will ensure com-pletion of the secondary task without affecting the primaryone. Substituting (7) in (8) gives

x2 = J2(q)J†1(q)x1 + J2(q)(I − J†1(q)J1(q)

)y. (9)

Minimising the secondary task error we get

y = J†2(q)(x2 − J2(q)J†1(q)x1 +(I − J†2(q)J2(q)

)z

J2(q) = J2(q)(I − J†1(q)J1(q)

)(10)

Finally, substituting (10) in (7) yields

ζ = J†1(q)x1 + J†2(q)(x2 − J2(q)J†1(q)x1

)+

+(I − J†2(q)J2(q)

)z

(11)

which constitutes the quasi velocity vector that the systemshould follow to achieve the primary and the secondary task,leaving room for additional tasks in the z vector.

It is possible to derive a recursive formula for an arbitrarynumber of N tasks, which is given by [3]

ζi = ζi−1 + J†i (q) (xi − Ji(q)ζi−1) , i = 1 . . . N, (12)

where

Ji(q) = Ji(q)Pi−1, Pi = Pi−1 − J†i (q)Ji(q). (13)

The algorithm has to be initialised with

ζ0 = 0n, P0 = In×n. (14)

B. Task definition

A task σi is defined as a generic m-dimensional variabledepending on the generalised coordinates q [1]:

σi = σi(q) ∈ Rmi (15)

It is connected with the task velocity vector xi by

xi = σi +Kiσi, (16)

where Ki is a diagonal gain matrix and σi = σi,d − σi.Looking at (16) we see two components: a feed-forward partσi, which may be used, e.g., in case of tracking of a movingobject, and a feedback part Kiσi which drives the system tothe goal σi,d.

C. Inequality tasks

The TP algorithm in its pure form has one serious draw-back. It would be natural to put safety tasks, e.g., manipulatorjoint limits, at the top of the hierarchy, above the tasksresponsible for reaching the goal. However, in practice, itresults in over-constraining the system so the goal cannotbe achieved. To solve this problem, the inequality tasks (orset tasks) were introduced [20]. To fulfil an inequality tasksome variables have to be kept in a specified range ratherthan approach a specific value. As a result, these kind oftasks require a definition of a binary activation functionai(q) ∈ {0, 1}. Then, the task hierarchy is continuouslyupdated, depending on the values of the activation functions.Care has to be taken when switching the tasks though. Forexample, a joint limit task may be inactive according to thecurrent joint value but the desired joint velocity computedby the TP algorithm will lead to the violation of the limitbefore the next update. Therefore, the quasi-velocities shouldbe integrated based on the controller sampling time and ifthe violation is detected, then the joint limit task should beactivated and the TP algorithm recomputed.

D. Task hierarchy

1) Task 1: Manipulator joint limits (inequality): Ensuringthat the control system generates velocities compliant withthe manipulator joint limits is a typical safety task put ontop of the task hierarchy. The task is defined for each jointj as follows (written for upper limit qj,1):

σi = qj,1 − δ − qj , σi = 0, Ji(q) = Jq,j , (17)

where Jq,j is a single-entry row matrix, equal 1 for theentry corresponding to the joint j. As an inequality task itis characterised by the following activation function:

ai =

{1, ai = 0 ∧ qj ≥ qj,1 − α0, ai = 1 ∧ qj ≤ qj,1 − δ , (18)

where α and δ are the activation and deactivation marginsrespectively. It is required that δ > α to avoid chatter.

2) Task 2: Nominal value (vehicle yaw, equality): Keepinga specific vehicle yaw is often desired to ensure goodvisibility of the target. Moreover, it automatically results inhaving the manipulator in front of the vehicle, where theEE is well visible and the manipulator is not generatingexcessive asymmetrical drag. Finally, when contact with theenvironment is required, keeping the EE close to the vehiclelongitudinal axis produces the lowest roll torque, which isoften only passively compensated for. The task variables aredefined as follows:

σi = ψd − ψ, σi = ψd, Ji(q) = Jψ, (19)

where Jψ is a single-entry row matrix, equal 1 for the entrycorresponding to ψ.

3) Task 3: End-effector configuration (equality): Finally,the most typical task used to follow the mission plan istracking the desired EE position and orientation. The taskvariables are defined as follows:

σi = ηee,d − ηee =

[η1ee,d − η1ee

λeeεd − λdεee + S(εd)εee

]σi = xE,d, Ji(q) = J(q),

(20)

where the orientation error was computed in quaternionsense and S(·) is a cross-product matrix operator [1]. Thisrepresentation does not suffer from gimbal-lock problems.

E. Operation in singular configurationsDepending on the kinematic chain of the I-AUV and the

task to be performed, it may happen that the robot has towork in a singular configuration where two or more axesof the robot DOF align. This situation can be detected bymonitoring the measure of manipulability (MOM)

m =√

det(JJT ). (21)

A special task can be defined for highly redundant systemsto keep MOM above a predefined threshold. However, itis sometimes not possible to avoid working in singularconfigurations, like in case of our 8 DOF I-AUV during pipeinspection. Without a special treatment of this situation therobot may get stuck around singular configurations becauseTP generates high quasi-velocities in the aligned joints,frequently changing directions. To lower the resulting quasi-velocities the weighted damped least-squares (DLS) solutionto computing the Jacobian inverse is used [7]. However, itdoes not ensure that the system will not get stuck, so wepropose an additional algorithm to selectively lock DOF justbefore the main (goal) task in the hierarchy, which is com-monly the EE configuration task. Locking means injectingadditional nominal tasks into the hierarchy which define thedesired position of the selected DOF equal to the positionat the time of locking. This redundancy reduction algorithmengages only in the vicinity of singular configurations thanksto the continuous monitoring of the main task Jacobianmatrix J(q) ∈ Rm×n and works according to Algorithm 1,where ε ≈ 0.95, Sort() is a sorting function which orderselements of A in an ascending manner, InjectTask() injectstasks to the TP hierarchy H, before the main task, and Cost()

Algorithm 1 Automatic redundancy reduction1: for k 1,m do2: A ∅3: for j ← 1, n do4: if J [k, j] > ε then5: c COST(j)6: A A ∪ {(c, j)}7: end if8: end for9: if |A| > 1 then

10: SORT(A)11: for j 1, |A| − 1 do12: INJECTTASK(Aj ,H)13: end for14: end if15: end for

is a cost function letting us express the preference of lockingspecific DOF. One logical choice is to construct a functionwhich will prefer locking manipulator DOF, and the onesthat are closest to the limits, i.e., with least available rangeof motion, which can be formulated as

c(j) =

{∞ vehicle DOF− |(qj−(qj,1+qj,0)/2)|

(qj,1−qj,0)/2otherwise , (22)

where qj,0 and qj,1 denote lower and upper limits of DOF jrespectively. As a side effect this criterion effectively ensurescompliancy with manipulator limits in most situations, sothat manipulator joint limits task engages much less often.

V. EXPERIMENT

A. Experimental setupThe developed control algorithm was implemented on-

board the GIRONA500 I-AUV and tested in a pipe in-spection scenario, in the CIRS test tank. GIRONA500 isa compact-sized underwater robot designed and developedat the University of Girona [17], which can be adaptedto different tasks by changing its payload and number ofthrusters. Here the vehicle was used in a 4 DOF configuration(yaw, surge, sway, heave) and equipped with an electric ECA5E Micro manipulator. The vehicle is passively stable inpitch and roll and the manipulator has 4 DOF actuated byscrew drives, which make it powerful but slow. Moreover, thedrives exhibit high friction forces resulting in velocity dead-band. The vehicle is equipped with two vertical thrusterswhich enable generating a torque in pitch, used in hff . Thekinematic structure of the system is presented in Fig. 2.

An aluminium structure was built to support a 2 m sectionof a 500 mm PVC pipe. The pipe was placed high enoughabove the bottom to ensure correct operation of the Dopplervelocity log (DVL), being the most important navigationsensor. Arrays of four markers (ARUCO) were placed onboth ends of the pipe to facilitate its stable detection withan on-board Point Grey Bumblebee 2 camera. The test rig ispresented in Fig. 3. The vehicle was equipped with a strain

⌘<latexit sha1_base64="sZD7pdzOvH5+7/IlAYHJOBxoVS8=">AAAB6nicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQS+he8eFDx6i/y5r9x0+ag1QcDj/dmmJkXplIYdN0vp7Kyura+Ud2sbW3v7O7V9w8eTJJpxn2WyER3Q2q4FIr7KFDybqo5jUPJO+HkpvA7j1wbkah7nKY8iOlIiUgwioXU50gH9YbbdOcgf4lXkgaUaA/qn/1hwrKYK2SSGtPz3BSDnGoUTPJZrZ8ZnlI2oSPes1TRmJsgn986IydWGZIo0bYUkrn6cyKnsTHTOLSdMcWxWfYK8T+vl2F0GeRCpRlyxRaLokwSTEjxOBkKzRnKqSWUaWFvJWxMNWVo46nZELzll/8S/6x51fTuzhut6zKNKhzBMZyCBxfQgltogw8MxvAEL/DqxM6z8+a8L1orTjlzCL/gfHwDdhiODA==</latexit><latexit sha1_base64="sZD7pdzOvH5+7/IlAYHJOBxoVS8=">AAAB6nicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQS+he8eFDx6i/y5r9x0+ag1QcDj/dmmJkXplIYdN0vp7Kyura+Ud2sbW3v7O7V9w8eTJJpxn2WyER3Q2q4FIr7KFDybqo5jUPJO+HkpvA7j1wbkah7nKY8iOlIiUgwioXU50gH9YbbdOcgf4lXkgaUaA/qn/1hwrKYK2SSGtPz3BSDnGoUTPJZrZ8ZnlI2oSPes1TRmJsgn986IydWGZIo0bYUkrn6cyKnsTHTOLSdMcWxWfYK8T+vl2F0GeRCpRlyxRaLokwSTEjxOBkKzRnKqSWUaWFvJWxMNWVo46nZELzll/8S/6x51fTuzhut6zKNKhzBMZyCBxfQgltogw8MxvAEL/DqxM6z8+a8L1orTjlzCL/gfHwDdhiODA==</latexit><latexit sha1_base64="sZD7pdzOvH5+7/IlAYHJOBxoVS8=">AAAB6nicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQS+he8eFDx6i/y5r9x0+ag1QcDj/dmmJkXplIYdN0vp7Kyura+Ud2sbW3v7O7V9w8eTJJpxn2WyER3Q2q4FIr7KFDybqo5jUPJO+HkpvA7j1wbkah7nKY8iOlIiUgwioXU50gH9YbbdOcgf4lXkgaUaA/qn/1hwrKYK2SSGtPz3BSDnGoUTPJZrZ8ZnlI2oSPes1TRmJsgn986IydWGZIo0bYUkrn6cyKnsTHTOLSdMcWxWfYK8T+vl2F0GeRCpRlyxRaLokwSTEjxOBkKzRnKqSWUaWFvJWxMNWVo46nZELzll/8S/6x51fTuzhut6zKNKhzBMZyCBxfQgltogw8MxvAEL/DqxM6z8+a8L1orTjlzCL/gfHwDdhiODA==</latexit><latexit sha1_base64="sZD7pdzOvH5+7/IlAYHJOBxoVS8=">AAAB6nicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FLx4rGFtoQ9lsN+3SzSbsToQS+he8eFDx6i/y5r9x0+ag1QcDj/dmmJkXplIYdN0vp7Kyura+Ud2sbW3v7O7V9w8eTJJpxn2WyER3Q2q4FIr7KFDybqo5jUPJO+HkpvA7j1wbkah7nKY8iOlIiUgwioXU50gH9YbbdOcgf4lXkgaUaA/qn/1hwrKYK2SSGtPz3BSDnGoUTPJZrZ8ZnlI2oSPes1TRmJsgn986IydWGZIo0bYUkrn6cyKnsTHTOLSdMcWxWfYK8T+vl2F0GeRCpRlyxRaLokwSTEjxOBkKzRnKqSWUaWFvJWxMNWVo46nZELzll/8S/6x51fTuzhut6zKNKhzBMZyCBxfQgltogw8MxvAEL/DqxM6z8+a8L1orTjlzCL/gfHwDdhiODA==</latexit>

⌘ee<latexit sha1_base64="aYXecA19Vvm/Lbg/koLhaJAiDPI=">AAAB73icbVBNS8NAEN3Ur1q/qh69LBbBU0lEUG9FLx4rGFtpQ9lsJ+3S3STsToQS+iu8eFDx6t/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTHHyeyES3Q2ZAihh8FCihnWpgKpTQCkc3U7/1BNqIJL7HcQqBYoNYRIIztNJjF5D1coBJr1pz6+4MdJl4BamRAs1e9avbT3imIEYumTEdz00xyJlGwSVMKt3MQMr4iA2gY2nMFJggnx08oSdW6dMo0bZipDP190TOlDFjFdpOxXBoFr2p+J/XyTC6DHIRpxlCzOeLokxSTOj0e9oXGjjKsSWMa2FvpXzINONoM6rYELzFl5eJf1a/qnt357XGdZFGmRyRY3JKPHJBGuSWNIlPOFHkmbySN0c7L8678zFvLTnFzCH5A+fzB3LikF8=</latexit><latexit sha1_base64="aYXecA19Vvm/Lbg/koLhaJAiDPI=">AAAB73icbVBNS8NAEN3Ur1q/qh69LBbBU0lEUG9FLx4rGFtpQ9lsJ+3S3STsToQS+iu8eFDx6t/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTHHyeyES3Q2ZAihh8FCihnWpgKpTQCkc3U7/1BNqIJL7HcQqBYoNYRIIztNJjF5D1coBJr1pz6+4MdJl4BamRAs1e9avbT3imIEYumTEdz00xyJlGwSVMKt3MQMr4iA2gY2nMFJggnx08oSdW6dMo0bZipDP190TOlDFjFdpOxXBoFr2p+J/XyTC6DHIRpxlCzOeLokxSTOj0e9oXGjjKsSWMa2FvpXzINONoM6rYELzFl5eJf1a/qnt357XGdZFGmRyRY3JKPHJBGuSWNIlPOFHkmbySN0c7L8678zFvLTnFzCH5A+fzB3LikF8=</latexit><latexit sha1_base64="aYXecA19Vvm/Lbg/koLhaJAiDPI=">AAAB73icbVBNS8NAEN3Ur1q/qh69LBbBU0lEUG9FLx4rGFtpQ9lsJ+3S3STsToQS+iu8eFDx6t/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTHHyeyES3Q2ZAihh8FCihnWpgKpTQCkc3U7/1BNqIJL7HcQqBYoNYRIIztNJjF5D1coBJr1pz6+4MdJl4BamRAs1e9avbT3imIEYumTEdz00xyJlGwSVMKt3MQMr4iA2gY2nMFJggnx08oSdW6dMo0bZipDP190TOlDFjFdpOxXBoFr2p+J/XyTC6DHIRpxlCzOeLokxSTOj0e9oXGjjKsSWMa2FvpXzINONoM6rYELzFl5eJf1a/qnt357XGdZFGmRyRY3JKPHJBGuSWNIlPOFHkmbySN0c7L8678zFvLTnFzCH5A+fzB3LikF8=</latexit><latexit sha1_base64="aYXecA19Vvm/Lbg/koLhaJAiDPI=">AAAB73icbVBNS8NAEN3Ur1q/qh69LBbBU0lEUG9FLx4rGFtpQ9lsJ+3S3STsToQS+iu8eFDx6t/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmlldW19o7xZ2dre2d2r7h88mCTTHHyeyES3Q2ZAihh8FCihnWpgKpTQCkc3U7/1BNqIJL7HcQqBYoNYRIIztNJjF5D1coBJr1pz6+4MdJl4BamRAs1e9avbT3imIEYumTEdz00xyJlGwSVMKt3MQMr4iA2gY2nMFJggnx08oSdW6dMo0bZipDP190TOlDFjFdpOxXBoFr2p+J/XyTC6DHIRpxlCzOeLokxSTOj0e9oXGjjKsSWMa2FvpXzINONoM6rYELzFl5eJf1a/qnt357XGdZFGmRyRY3JKPHJBGuSWNIlPOFHkmbySN0c7L8678zFvLTnFzCH5A+fzB3LikF8=</latexit>

{N}<latexit sha1_base64="GXn0Q4q+WwfMoextbh4U/RRZP+E=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FL56kgrGFJpTNdtsu3WzC7kQoob/BiwcVr/4hb/4bt20O2vpg4PHeDDPzolQKg6777ZRWVtfWN8qbla3tnd296v7Bo0kyzbjPEpnodkQNl0JxHwVK3k41p3EkeSsa3Uz91hPXRiTqAccpD2M6UKIvGEUr+UF+F0y61Zpbd2cgy8QrSA0KNLvVr6CXsCzmCpmkxnQ8N8UwpxoFk3xSCTLDU8pGdMA7lioacxPms2Mn5MQqPdJPtC2FZKb+nshpbMw4jmxnTHFoFr2p+J/XybB/GeZCpRlyxeaL+pkkmJDp56QnNGcox5ZQpoW9lbAh1ZShzadiQ/AWX14m/ln9qu7dn9ca10UaZTiCYzgFDy6gAbfQBB8YCHiGV3hzlPPivDsf89aSU8wcwh84nz85z45+</latexit><latexit sha1_base64="GXn0Q4q+WwfMoextbh4U/RRZP+E=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FL56kgrGFJpTNdtsu3WzC7kQoob/BiwcVr/4hb/4bt20O2vpg4PHeDDPzolQKg6777ZRWVtfWN8qbla3tnd296v7Bo0kyzbjPEpnodkQNl0JxHwVK3k41p3EkeSsa3Uz91hPXRiTqAccpD2M6UKIvGEUr+UF+F0y61Zpbd2cgy8QrSA0KNLvVr6CXsCzmCpmkxnQ8N8UwpxoFk3xSCTLDU8pGdMA7lioacxPms2Mn5MQqPdJPtC2FZKb+nshpbMw4jmxnTHFoFr2p+J/XybB/GeZCpRlyxeaL+pkkmJDp56QnNGcox5ZQpoW9lbAh1ZShzadiQ/AWX14m/ln9qu7dn9ca10UaZTiCYzgFDy6gAbfQBB8YCHiGV3hzlPPivDsf89aSU8wcwh84nz85z45+</latexit><latexit sha1_base64="GXn0Q4q+WwfMoextbh4U/RRZP+E=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FL56kgrGFJpTNdtsu3WzC7kQoob/BiwcVr/4hb/4bt20O2vpg4PHeDDPzolQKg6777ZRWVtfWN8qbla3tnd296v7Bo0kyzbjPEpnodkQNl0JxHwVK3k41p3EkeSsa3Uz91hPXRiTqAccpD2M6UKIvGEUr+UF+F0y61Zpbd2cgy8QrSA0KNLvVr6CXsCzmCpmkxnQ8N8UwpxoFk3xSCTLDU8pGdMA7lioacxPms2Mn5MQqPdJPtC2FZKb+nshpbMw4jmxnTHFoFr2p+J/XybB/GeZCpRlyxeaL+pkkmJDp56QnNGcox5ZQpoW9lbAh1ZShzadiQ/AWX14m/ln9qu7dn9ca10UaZTiCYzgFDy6gAbfQBB8YCHiGV3hzlPPivDsf89aSU8wcwh84nz85z45+</latexit><latexit sha1_base64="GXn0Q4q+WwfMoextbh4U/RRZP+E=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FL56kgrGFJpTNdtsu3WzC7kQoob/BiwcVr/4hb/4bt20O2vpg4PHeDDPzolQKg6777ZRWVtfWN8qbla3tnd296v7Bo0kyzbjPEpnodkQNl0JxHwVK3k41p3EkeSsa3Uz91hPXRiTqAccpD2M6UKIvGEUr+UF+F0y61Zpbd2cgy8QrSA0KNLvVr6CXsCzmCpmkxnQ8N8UwpxoFk3xSCTLDU8pGdMA7lioacxPms2Mn5MQqPdJPtC2FZKb+nshpbMw4jmxnTHFoFr2p+J/XybB/GeZCpRlyxeaL+pkkmJDp56QnNGcox5ZQpoW9lbAh1ZShzadiQ/AWX14m/ln9qu7dn9ca10UaZTiCYzgFDy6gAbfQBB8YCHiGV3hzlPPivDsf89aSU8wcwh84nz85z45+</latexit>

{B}<latexit sha1_base64="RgO0EtmOhH8jbLjkCT7+1xHKl/g=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG+lXjxWMLbQhLLZTtulm03Y3Qgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelAqujet+O6W19Y3NrfJ2ZWd3b/+genj0qJNMMfRZIhLViahGwSX6hhuBnVQhjSOB7Wh8O/PbT6g0T+SDmaQYxnQo+YAzaqzkB3kzmPaqNbfuzkFWiVeQGhRo9apfQT9hWYzSMEG17npuasKcKsOZwGklyDSmlI3pELuWShqjDvP5sVNyZpU+GSTKljRkrv6eyGms9SSObGdMzUgvezPxP6+bmcF1mHOZZgYlWywaZIKYhMw+J32ukBkxsYQyxe2thI2ooszYfCo2BG/55VXiX9Rv6t79Za3RLNIowwmcwjl4cAUNuIMW+MCAwzO8wpsjnRfn3flYtJacYuYY/sD5/AEnk45y</latexit><latexit sha1_base64="RgO0EtmOhH8jbLjkCT7+1xHKl/g=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG+lXjxWMLbQhLLZTtulm03Y3Qgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelAqujet+O6W19Y3NrfJ2ZWd3b/+genj0qJNMMfRZIhLViahGwSX6hhuBnVQhjSOB7Wh8O/PbT6g0T+SDmaQYxnQo+YAzaqzkB3kzmPaqNbfuzkFWiVeQGhRo9apfQT9hWYzSMEG17npuasKcKsOZwGklyDSmlI3pELuWShqjDvP5sVNyZpU+GSTKljRkrv6eyGms9SSObGdMzUgvezPxP6+bmcF1mHOZZgYlWywaZIKYhMw+J32ukBkxsYQyxe2thI2ooszYfCo2BG/55VXiX9Rv6t79Za3RLNIowwmcwjl4cAUNuIMW+MCAwzO8wpsjnRfn3flYtJacYuYY/sD5/AEnk45y</latexit><latexit sha1_base64="RgO0EtmOhH8jbLjkCT7+1xHKl/g=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG+lXjxWMLbQhLLZTtulm03Y3Qgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelAqujet+O6W19Y3NrfJ2ZWd3b/+genj0qJNMMfRZIhLViahGwSX6hhuBnVQhjSOB7Wh8O/PbT6g0T+SDmaQYxnQo+YAzaqzkB3kzmPaqNbfuzkFWiVeQGhRo9apfQT9hWYzSMEG17npuasKcKsOZwGklyDSmlI3pELuWShqjDvP5sVNyZpU+GSTKljRkrv6eyGms9SSObGdMzUgvezPxP6+bmcF1mHOZZgYlWywaZIKYhMw+J32ukBkxsYQyxe2thI2ooszYfCo2BG/55VXiX9Rv6t79Za3RLNIowwmcwjl4cAUNuIMW+MCAwzO8wpsjnRfn3flYtJacYuYY/sD5/AEnk45y</latexit><latexit sha1_base64="RgO0EtmOhH8jbLjkCT7+1xHKl/g=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG+lXjxWMLbQhLLZTtulm03Y3Qgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelAqujet+O6W19Y3NrfJ2ZWd3b/+genj0qJNMMfRZIhLViahGwSX6hhuBnVQhjSOB7Wh8O/PbT6g0T+SDmaQYxnQo+YAzaqzkB3kzmPaqNbfuzkFWiVeQGhRo9apfQT9hWYzSMEG17npuasKcKsOZwGklyDSmlI3pELuWShqjDvP5sVNyZpU+GSTKljRkrv6eyGms9SSObGdMzUgvezPxP6+bmcF1mHOZZgYlWywaZIKYhMw+J32ukBkxsYQyxe2thI2ooszYfCo2BG/55VXiX9Rv6t79Za3RLNIowwmcwjl4cAUNuIMW+MCAwzO8wpsjnRfn3flYtJacYuYY/sD5/AEnk45y</latexit>

{E}<latexit sha1_base64="DT1hjb0C0xjY8Mx6EE05U+EkmPg=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FETxWMLbQhLLZbtulm03YnQgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelEph0HW/ndLK6tr6RnmzsrW9s7tX3T94NEmmGfdZIhPdjqjhUijuo0DJ26nmNI4kb0Wjm6nfeuLaiEQ94DjlYUwHSvQFo2glP8hvg0m3WnPr7gxkmXgFqUGBZrf6FfQSlsVcIZPUmI7nphjmVKNgkk8qQWZ4StmIDnjHUkVjbsJ8duyEnFilR/qJtqWQzNTfEzmNjRnHke2MKQ7NojcV//M6GfYvw1yoNEOu2HxRP5MEEzL9nPSE5gzl2BLKtLC3EjakmjK0+VRsCN7iy8vEP6tf1b3781rjukijDEdwDKfgwQU04A6a4AMDAc/wCm+Ocl6cd+dj3lpyiplD+APn8wcsIo51</latexit><latexit sha1_base64="DT1hjb0C0xjY8Mx6EE05U+EkmPg=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FETxWMLbQhLLZbtulm03YnQgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelEph0HW/ndLK6tr6RnmzsrW9s7tX3T94NEmmGfdZIhPdjqjhUijuo0DJ26nmNI4kb0Wjm6nfeuLaiEQ94DjlYUwHSvQFo2glP8hvg0m3WnPr7gxkmXgFqUGBZrf6FfQSlsVcIZPUmI7nphjmVKNgkk8qQWZ4StmIDnjHUkVjbsJ8duyEnFilR/qJtqWQzNTfEzmNjRnHke2MKQ7NojcV//M6GfYvw1yoNEOu2HxRP5MEEzL9nPSE5gzl2BLKtLC3EjakmjK0+VRsCN7iy8vEP6tf1b3781rjukijDEdwDKfgwQU04A6a4AMDAc/wCm+Ocl6cd+dj3lpyiplD+APn8wcsIo51</latexit><latexit sha1_base64="DT1hjb0C0xjY8Mx6EE05U+EkmPg=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FETxWMLbQhLLZbtulm03YnQgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelEph0HW/ndLK6tr6RnmzsrW9s7tX3T94NEmmGfdZIhPdjqjhUijuo0DJ26nmNI4kb0Wjm6nfeuLaiEQ94DjlYUwHSvQFo2glP8hvg0m3WnPr7gxkmXgFqUGBZrf6FfQSlsVcIZPUmI7nphjmVKNgkk8qQWZ4StmIDnjHUkVjbsJ8duyEnFilR/qJtqWQzNTfEzmNjRnHke2MKQ7NojcV//M6GfYvw1yoNEOu2HxRP5MEEzL9nPSE5gzl2BLKtLC3EjakmjK0+VRsCN7iy8vEP6tf1b3781rjukijDEdwDKfgwQU04A6a4AMDAc/wCm+Ocl6cd+dj3lpyiplD+APn8wcsIo51</latexit><latexit sha1_base64="DT1hjb0C0xjY8Mx6EE05U+EkmPg=">AAAB63icbVBNS8NAEJ3Ur1q/qh69LBbBU0lEUG9FETxWMLbQhLLZbtulm03YnQgl9Dd48aDi1T/kzX/jts1BWx8MPN6bYWZelEph0HW/ndLK6tr6RnmzsrW9s7tX3T94NEmmGfdZIhPdjqjhUijuo0DJ26nmNI4kb0Wjm6nfeuLaiEQ94DjlYUwHSvQFo2glP8hvg0m3WnPr7gxkmXgFqUGBZrf6FfQSlsVcIZPUmI7nphjmVKNgkk8qQWZ4StmIDnjHUkVjbsJ8duyEnFilR/qJtqWQzNTfEzmNjRnHke2MKQ7NojcV//M6GfYvw1yoNEOu2HxRP5MEEzL9nPSE5gzl2BLKtLC3EjakmjK0+VRsCN7iy8vEP6tf1b3781rjukijDEdwDKfgwQU04A6a4AMDAc/wCm+Ocl6cd+dj3lpyiplD+APn8wcsIo51</latexit>

q1<latexit sha1_base64="b4gb5EJr9NHcCvRbk5lKH3Ttjbs=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262cTdiVBCf4IXDype/Ufe/Ddu2xy09cHA470ZZuaFqRQGXffbWVpeWV1bL22UN7e2d3Yre/sPJsk04z5LZKJbITVcCsV9FCh5K9WcxqHkzXB4PfGbT1wbkah7HKU8iGlfiUgwila6e+x63UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKenjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wughyodIMuWKzRVEmCSZk8jfpCc0ZypEllGlhbyVsQDVlaNMp2xC8+ZcXiX9au6x5t2fV+lWRRgkO4QhOwINzqMMNNMAHBn14hld4c6Tz4rw7H7PWJaeYOYA/cD5/AG7SjW0=</latexit><latexit sha1_base64="b4gb5EJr9NHcCvRbk5lKH3Ttjbs=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262cTdiVBCf4IXDype/Ufe/Ddu2xy09cHA470ZZuaFqRQGXffbWVpeWV1bL22UN7e2d3Yre/sPJsk04z5LZKJbITVcCsV9FCh5K9WcxqHkzXB4PfGbT1wbkah7HKU8iGlfiUgwila6e+x63UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKenjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wughyodIMuWKzRVEmCSZk8jfpCc0ZypEllGlhbyVsQDVlaNMp2xC8+ZcXiX9au6x5t2fV+lWRRgkO4QhOwINzqMMNNMAHBn14hld4c6Tz4rw7H7PWJaeYOYA/cD5/AG7SjW0=</latexit><latexit sha1_base64="b4gb5EJr9NHcCvRbk5lKH3Ttjbs=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262cTdiVBCf4IXDype/Ufe/Ddu2xy09cHA470ZZuaFqRQGXffbWVpeWV1bL22UN7e2d3Yre/sPJsk04z5LZKJbITVcCsV9FCh5K9WcxqHkzXB4PfGbT1wbkah7HKU8iGlfiUgwila6e+x63UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKenjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wughyodIMuWKzRVEmCSZk8jfpCc0ZypEllGlhbyVsQDVlaNMp2xC8+ZcXiX9au6x5t2fV+lWRRgkO4QhOwINzqMMNNMAHBn14hld4c6Tz4rw7H7PWJaeYOYA/cD5/AG7SjW0=</latexit><latexit sha1_base64="b4gb5EJr9NHcCvRbk5lKH3Ttjbs=">AAAB6XicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWNFYwttKJvtpl262cTdiVBCf4IXDype/Ufe/Ddu2xy09cHA470ZZuaFqRQGXffbWVpeWV1bL22UN7e2d3Yre/sPJsk04z5LZKJbITVcCsV9FCh5K9WcxqHkzXB4PfGbT1wbkah7HKU8iGlfiUgwila6e+x63UrVrblTkEXiFaQKBRrdylenl7As5gqZpMa0PTfFIKcaBZN8XO5khqeUDWmfty1VNOYmyKenjsmxVXokSrQthWSq/p7IaWzMKA5tZ0xxYOa9ifif184wughyodIMuWKzRVEmCSZk8jfpCc0ZypEllGlhbyVsQDVlaNMp2xC8+ZcXiX9au6x5t2fV+lWRRgkO4QhOwINzqMMNNMAHBn14hld4c6Tz4rw7H7PWJaeYOYA/cD5/AG7SjW0=</latexit>

q2<latexit sha1_base64="tnqTiybmLxsc2nClGzYLJse+jIU=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mKoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4lfq15WvduzSv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3BVjW4=</latexit><latexit sha1_base64="tnqTiybmLxsc2nClGzYLJse+jIU=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mKoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4lfq15WvduzSv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3BVjW4=</latexit><latexit sha1_base64="tnqTiybmLxsc2nClGzYLJse+jIU=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mKoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4lfq15WvduzSv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3BVjW4=</latexit><latexit sha1_base64="tnqTiybmLxsc2nClGzYLJse+jIU=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mKoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4lfq15WvduzSv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3BVjW4=</latexit>

q3<latexit sha1_base64="UtMRPzltGak80VvSSY0xuKjPe+A=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lUUG9FLx4rGltoQ9lsp+3SzSbuboQS+hO8eFDx6j/y5r9x2+agrQ8GHu/NMDMvTATXxnW/ncLS8srqWnG9tLG5tb1T3t170HGqGPosFrFqhlSj4BJ9w43AZqKQRqHARji8nviNJ1Sax/LejBIMItqXvMcZNVa6e+ycdsoVt+pOQRaJl5MK5Kh3yl/tbszSCKVhgmrd8tzEBBlVhjOB41I71ZhQNqR9bFkqaYQ6yKanjsmRVbqkFytb0pCp+nsio5HWoyi0nRE1Az3vTcT/vFZqehdBxmWSGpRstqiXCmJiMvmbdLlCZsTIEsoUt7cSNqCKMmPTKdkQvPmXF4l/Ur2serdnldpVnkYRDuAQjsGDc6jBDdTBBwZ9eIZXeHOE8+K8Ox+z1oKTz+zDHzifP3HYjW8=</latexit><latexit sha1_base64="UtMRPzltGak80VvSSY0xuKjPe+A=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lUUG9FLx4rGltoQ9lsp+3SzSbuboQS+hO8eFDx6j/y5r9x2+agrQ8GHu/NMDMvTATXxnW/ncLS8srqWnG9tLG5tb1T3t170HGqGPosFrFqhlSj4BJ9w43AZqKQRqHARji8nviNJ1Sax/LejBIMItqXvMcZNVa6e+ycdsoVt+pOQRaJl5MK5Kh3yl/tbszSCKVhgmrd8tzEBBlVhjOB41I71ZhQNqR9bFkqaYQ6yKanjsmRVbqkFytb0pCp+nsio5HWoyi0nRE1Az3vTcT/vFZqehdBxmWSGpRstqiXCmJiMvmbdLlCZsTIEsoUt7cSNqCKMmPTKdkQvPmXF4l/Ur2serdnldpVnkYRDuAQjsGDc6jBDdTBBwZ9eIZXeHOE8+K8Ox+z1oKTz+zDHzifP3HYjW8=</latexit><latexit sha1_base64="UtMRPzltGak80VvSSY0xuKjPe+A=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lUUG9FLx4rGltoQ9lsp+3SzSbuboQS+hO8eFDx6j/y5r9x2+agrQ8GHu/NMDMvTATXxnW/ncLS8srqWnG9tLG5tb1T3t170HGqGPosFrFqhlSj4BJ9w43AZqKQRqHARji8nviNJ1Sax/LejBIMItqXvMcZNVa6e+ycdsoVt+pOQRaJl5MK5Kh3yl/tbszSCKVhgmrd8tzEBBlVhjOB41I71ZhQNqR9bFkqaYQ6yKanjsmRVbqkFytb0pCp+nsio5HWoyi0nRE1Az3vTcT/vFZqehdBxmWSGpRstqiXCmJiMvmbdLlCZsTIEsoUt7cSNqCKMmPTKdkQvPmXF4l/Ur2serdnldpVnkYRDuAQjsGDc6jBDdTBBwZ9eIZXeHOE8+K8Ox+z1oKTz+zDHzifP3HYjW8=</latexit><latexit sha1_base64="UtMRPzltGak80VvSSY0xuKjPe+A=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lUUG9FLx4rGltoQ9lsp+3SzSbuboQS+hO8eFDx6j/y5r9x2+agrQ8GHu/NMDMvTATXxnW/ncLS8srqWnG9tLG5tb1T3t170HGqGPosFrFqhlSj4BJ9w43AZqKQRqHARji8nviNJ1Sax/LejBIMItqXvMcZNVa6e+ycdsoVt+pOQRaJl5MK5Kh3yl/tbszSCKVhgmrd8tzEBBlVhjOB41I71ZhQNqR9bFkqaYQ6yKanjsmRVbqkFytb0pCp+nsio5HWoyi0nRE1Az3vTcT/vFZqehdBxmWSGpRstqiXCmJiMvmbdLlCZsTIEsoUt7cSNqCKMmPTKdkQvPmXF4l/Ur2serdnldpVnkYRDuAQjsGDc6jBDdTBBwZ9eIZXeHOE8+K8Ox+z1oKTz+zDHzifP3HYjW8=</latexit> q4

<latexit sha1_base64="8dX89Ve0/J3kXLb3lJPhBP+sRyI=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mkoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4l/Vr2sere1Sv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3NbjXA=</latexit><latexit sha1_base64="8dX89Ve0/J3kXLb3lJPhBP+sRyI=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mkoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4l/Vr2sere1Sv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3NbjXA=</latexit><latexit sha1_base64="8dX89Ve0/J3kXLb3lJPhBP+sRyI=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mkoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4l/Vr2sere1Sv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3NbjXA=</latexit><latexit sha1_base64="8dX89Ve0/J3kXLb3lJPhBP+sRyI=">AAAB6XicbVBNS8NAEJ3Ur1q/qh69LBbBU0mkoN6KXjxWNLbQhrLZTtulm03c3Qgl9Cd48aDi1X/kzX/jts1BWx8MPN6bYWZemAiujet+O4WV1bX1jeJmaWt7Z3evvH/woONUMfRZLGLVCqlGwSX6hhuBrUQhjUKBzXB0PfWbT6g0j+W9GScYRHQgeZ8zaqx099itdcsVt+rOQJaJl5MK5Gh0y1+dXszSCKVhgmrd9tzEBBlVhjOBk1In1ZhQNqIDbFsqaYQ6yGanTsiJVXqkHytb0pCZ+nsio5HW4yi0nRE1Q73oTcX/vHZq+hdBxmWSGpRsvqifCmJiMv2b9LhCZsTYEsoUt7cSNqSKMmPTKdkQvMWXl4l/Vr2sere1Sv0qT6MIR3AMp+DBOdThBhrgA4MBPMMrvDnCeXHenY95a8HJZw7hD5zPH3NbjXA=</latexit>

'<latexit sha1_base64="fFWBIbp62QE489epHA88wiMyomE=">AAAB7XicbVBNSwMxEJ3Ur1q/qh69BIvgqeyKoN6KXjxWcG2hXUo2zbah2WxIsoWy9Ed48aDi1f/jzX9j2u5BWx8MPN6bYWZepAQ31vO+UWltfWNzq7xd2dnd2z+oHh49mTTTlAU0FaluR8QwwSULLLeCtZVmJIkEa0Wju5nfGjNteCof7USxMCEDyWNOiXVSqzsmWg15r1rz6t4ceJX4BalBgWav+tXtpzRLmLRUEGM6vqdsmBNtORVsWulmhilCR2TAOo5KkjAT5vNzp/jMKX0cp9qVtHiu/p7ISWLMJIlcZ0Ls0Cx7M/E/r5PZ+DrMuVSZZZIuFsWZwDbFs99xn2tGrZg4Qqjm7lZMh0QTal1CFReCv/zyKgku6jd1/+Gy1rgt0ijDCZzCOfhwBQ24hyYEQGEEz/AKb0ihF/SOPhatJVTMHMMfoM8f5y2Peg==</latexit><latexit sha1_base64="fFWBIbp62QE489epHA88wiMyomE=">AAAB7XicbVBNSwMxEJ3Ur1q/qh69BIvgqeyKoN6KXjxWcG2hXUo2zbah2WxIsoWy9Ed48aDi1f/jzX9j2u5BWx8MPN6bYWZepAQ31vO+UWltfWNzq7xd2dnd2z+oHh49mTTTlAU0FaluR8QwwSULLLeCtZVmJIkEa0Wju5nfGjNteCof7USxMCEDyWNOiXVSqzsmWg15r1rz6t4ceJX4BalBgWav+tXtpzRLmLRUEGM6vqdsmBNtORVsWulmhilCR2TAOo5KkjAT5vNzp/jMKX0cp9qVtHiu/p7ISWLMJIlcZ0Ls0Cx7M/E/r5PZ+DrMuVSZZZIuFsWZwDbFs99xn2tGrZg4Qqjm7lZMh0QTal1CFReCv/zyKgku6jd1/+Gy1rgt0ijDCZzCOfhwBQ24hyYEQGEEz/AKb0ihF/SOPhatJVTMHMMfoM8f5y2Peg==</latexit><latexit sha1_base64="fFWBIbp62QE489epHA88wiMyomE=">AAAB7XicbVBNSwMxEJ3Ur1q/qh69BIvgqeyKoN6KXjxWcG2hXUo2zbah2WxIsoWy9Ed48aDi1f/jzX9j2u5BWx8MPN6bYWZepAQ31vO+UWltfWNzq7xd2dnd2z+oHh49mTTTlAU0FaluR8QwwSULLLeCtZVmJIkEa0Wju5nfGjNteCof7USxMCEDyWNOiXVSqzsmWg15r1rz6t4ceJX4BalBgWav+tXtpzRLmLRUEGM6vqdsmBNtORVsWulmhilCR2TAOo5KkjAT5vNzp/jMKX0cp9qVtHiu/p7ISWLMJIlcZ0Ls0Cx7M/E/r5PZ+DrMuVSZZZIuFsWZwDbFs99xn2tGrZg4Qqjm7lZMh0QTal1CFReCv/zyKgku6jd1/+Gy1rgt0ijDCZzCOfhwBQ24hyYEQGEEz/AKb0ihF/SOPhatJVTMHMMfoM8f5y2Peg==</latexit><latexit sha1_base64="fFWBIbp62QE489epHA88wiMyomE=">AAAB7XicbVBNSwMxEJ3Ur1q/qh69BIvgqeyKoN6KXjxWcG2hXUo2zbah2WxIsoWy9Ed48aDi1f/jzX9j2u5BWx8MPN6bYWZepAQ31vO+UWltfWNzq7xd2dnd2z+oHh49mTTTlAU0FaluR8QwwSULLLeCtZVmJIkEa0Wju5nfGjNteCof7USxMCEDyWNOiXVSqzsmWg15r1rz6t4ceJX4BalBgWav+tXtpzRLmLRUEGM6vqdsmBNtORVsWulmhilCR2TAOo5KkjAT5vNzp/jMKX0cp9qVtHiu/p7ISWLMJIlcZ0Ls0Cx7M/E/r5PZ+DrMuVSZZZIuFsWZwDbFs99xn2tGrZg4Qqjm7lZMh0QTal1CFReCv/zyKgku6jd1/+Gy1rgt0ijDCZzCOfhwBQ24hyYEQGEEz/AKb0ihF/SOPhatJVTMHMMfoM8f5y2Peg==</latexit>

✓<latexit sha1_base64="hzzKqhwXj16uGJuoMOKCNod5B/c=">AAAB7HicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMF0xbaUDbbTbt2swm7E6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6bJsk04z5LZKLbITVcCsV9FCh5O9WcxqHkrXB0O/VbT1wbkagHHKc8iOlAiUgwilZqdnHIkfYqVbfmzkCWiVeQKhRo9Cpf3X7CspgrZJIa0/HcFIOcahRM8km5mxmeUjaiA96xVNGYmyCfXTshp1bpkyjRthSSmfp7IqexMeM4tJ0xxaFZ9Kbif14nw+gqyIVKM+SKzRdFmSSYkOnrpC80ZyjHllCmhb2VsCHVlKENqGxD8BZfXib+ee265t1fVOs3RRolOIYTOAMPLqEOd9AAHxg8wjO8wpuTOC/Ou/Mxb11xipkj+APn8wcQXo78</latexit><latexit sha1_base64="hzzKqhwXj16uGJuoMOKCNod5B/c=">AAAB7HicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMF0xbaUDbbTbt2swm7E6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6bJsk04z5LZKLbITVcCsV9FCh5O9WcxqHkrXB0O/VbT1wbkagHHKc8iOlAiUgwilZqdnHIkfYqVbfmzkCWiVeQKhRo9Cpf3X7CspgrZJIa0/HcFIOcahRM8km5mxmeUjaiA96xVNGYmyCfXTshp1bpkyjRthSSmfp7IqexMeM4tJ0xxaFZ9Kbif14nw+gqyIVKM+SKzRdFmSSYkOnrpC80ZyjHllCmhb2VsCHVlKENqGxD8BZfXib+ee265t1fVOs3RRolOIYTOAMPLqEOd9AAHxg8wjO8wpuTOC/Ou/Mxb11xipkj+APn8wcQXo78</latexit><latexit sha1_base64="hzzKqhwXj16uGJuoMOKCNod5B/c=">AAAB7HicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMF0xbaUDbbTbt2swm7E6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6bJsk04z5LZKLbITVcCsV9FCh5O9WcxqHkrXB0O/VbT1wbkagHHKc8iOlAiUgwilZqdnHIkfYqVbfmzkCWiVeQKhRo9Cpf3X7CspgrZJIa0/HcFIOcahRM8km5mxmeUjaiA96xVNGYmyCfXTshp1bpkyjRthSSmfp7IqexMeM4tJ0xxaFZ9Kbif14nw+gqyIVKM+SKzRdFmSSYkOnrpC80ZyjHllCmhb2VsCHVlKENqGxD8BZfXib+ee265t1fVOs3RRolOIYTOAMPLqEOd9AAHxg8wjO8wpuTOC/Ou/Mxb11xipkj+APn8wcQXo78</latexit><latexit sha1_base64="hzzKqhwXj16uGJuoMOKCNod5B/c=">AAAB7HicbVBNS8NAEJ34WetX1aOXxSJ4KokI6q3oxWMF0xbaUDbbTbt2swm7E6GE/gcvHlS8+oO8+W/ctjlo64OBx3szzMwLUykMuu63s7K6tr6xWdoqb+/s7u1XDg6bJsk04z5LZKLbITVcCsV9FCh5O9WcxqHkrXB0O/VbT1wbkagHHKc8iOlAiUgwilZqdnHIkfYqVbfmzkCWiVeQKhRo9Cpf3X7CspgrZJIa0/HcFIOcahRM8km5mxmeUjaiA96xVNGYmyCfXTshp1bpkyjRthSSmfp7IqexMeM4tJ0xxaFZ9Kbif14nw+gqyIVKM+SKzRdFmSSYkOnrpC80ZyjHllCmhb2VsCHVlKENqGxD8BZfXib+ee265t1fVOs3RRolOIYTOAMPLqEOd9AAHxg8wjO8wpuTOC/Ou/Mxb11xipkj+APn8wcQXo78</latexit>

<latexit sha1_base64="fYJX9pkPa3xD4PNOJcgdBDiVkBg=">AAAB6nicbVBNSwMxEJ2tX7V+VT16CRbBU9kVQb0VvXis4NpCu5Rsmm1Dk2xIskJZ+he8eFDx6i/y5r8x2+5Bqw8GHu/NMDMvVpwZ6/tfXmVldW19o7pZ29re2d2r7x88mDTThIYk5anuxthQziQNLbOcdpWmWMScduLJTeF3Hqk2LJX3dqpoJPBIsoQRbAuprwwb1Bt+058D/SVBSRpQoj2of/aHKckElZZwbEwv8JWNcqwtI5zOav3MUIXJBI9oz1GJBTVRPr91hk6cMkRJql1Ji+bqz4kcC2OmInadAtuxWfYK8T+vl9nkMsqZVJmlkiwWJRlHNkXF42jINCWWTx3BRDN3KyJjrDGxLp6aCyFYfvkvCc+aV83g7rzRui7TqMIRHMMpBHABLbiFNoRAYAxP8AKvnvCevTfvfdFa8cqZQ/gF7+MbkWOOHg==</latexit><latexit sha1_base64="fYJX9pkPa3xD4PNOJcgdBDiVkBg=">AAAB6nicbVBNSwMxEJ2tX7V+VT16CRbBU9kVQb0VvXis4NpCu5Rsmm1Dk2xIskJZ+he8eFDx6i/y5r8x2+5Bqw8GHu/NMDMvVpwZ6/tfXmVldW19o7pZ29re2d2r7x88mDTThIYk5anuxthQziQNLbOcdpWmWMScduLJTeF3Hqk2LJX3dqpoJPBIsoQRbAuprwwb1Bt+058D/SVBSRpQoj2of/aHKckElZZwbEwv8JWNcqwtI5zOav3MUIXJBI9oz1GJBTVRPr91hk6cMkRJql1Ji+bqz4kcC2OmInadAtuxWfYK8T+vl9nkMsqZVJmlkiwWJRlHNkXF42jINCWWTx3BRDN3KyJjrDGxLp6aCyFYfvkvCc+aV83g7rzRui7TqMIRHMMpBHABLbiFNoRAYAxP8AKvnvCevTfvfdFa8cqZQ/gF7+MbkWOOHg==</latexit><latexit sha1_base64="fYJX9pkPa3xD4PNOJcgdBDiVkBg=">AAAB6nicbVBNSwMxEJ2tX7V+VT16CRbBU9kVQb0VvXis4NpCu5Rsmm1Dk2xIskJZ+he8eFDx6i/y5r8x2+5Bqw8GHu/NMDMvVpwZ6/tfXmVldW19o7pZ29re2d2r7x88mDTThIYk5anuxthQziQNLbOcdpWmWMScduLJTeF3Hqk2LJX3dqpoJPBIsoQRbAuprwwb1Bt+058D/SVBSRpQoj2of/aHKckElZZwbEwv8JWNcqwtI5zOav3MUIXJBI9oz1GJBTVRPr91hk6cMkRJql1Ji+bqz4kcC2OmInadAtuxWfYK8T+vl9nkMsqZVJmlkiwWJRlHNkXF42jINCWWTx3BRDN3KyJjrDGxLp6aCyFYfvkvCc+aV83g7rzRui7TqMIRHMMpBHABLbiFNoRAYAxP8AKvnvCevTfvfdFa8cqZQ/gF7+MbkWOOHg==</latexit><latexit sha1_base64="fYJX9pkPa3xD4PNOJcgdBDiVkBg=">AAAB6nicbVBNSwMxEJ2tX7V+VT16CRbBU9kVQb0VvXis4NpCu5Rsmm1Dk2xIskJZ+he8eFDx6i/y5r8x2+5Bqw8GHu/NMDMvVpwZ6/tfXmVldW19o7pZ29re2d2r7x88mDTThIYk5anuxthQziQNLbOcdpWmWMScduLJTeF3Hqk2LJX3dqpoJPBIsoQRbAuprwwb1Bt+058D/SVBSRpQoj2of/aHKckElZZwbEwv8JWNcqwtI5zOav3MUIXJBI9oz1GJBTVRPr91hk6cMkRJql1Ji+bqz4kcC2OmInadAtuxWfYK8T+vl9nkMsqZVJmlkiwWJRlHNkXF42jINCWWTx3BRDN3KyJjrDGxLp6aCyFYfvkvCc+aV83g7rzRui7TqMIRHMMpBHABLbiFNoRAYAxP8AKvnvCevTfvfdFa8cqZQ/gF7+MbkWOOHg==</latexit>

Fig. 2. Kinematic structure of GIRONA500 I-AUV.

Fig. 3. GIRONA500 I-AUV performing pipe inspection in the test tank.Also shown is the end-effector equipped with a force/torque sensor.

gauge 6-axis force/torque sensor in the manipulator wrist anda simple end-effector with a cylindrical, flat-headed, rigid tip.

B. Path generationDesired EE path is computed based on the detected

pipe axis, central point and a vertical axis aligned withmarker surface. The user specifies a pice-wise-linear path incylindrical coordinates (lp, ϕp), where lp is the translationalong the pipe axis from the central point and ϕp is anangle between the vertical axis and a line parallel to thepipe radius, passing through a desired point on pipe surface.Desired EE orientation is computed based on surface normalat each point of the generated path.

C. Experimental resultsThe results of the pipe inspection experiment are presented

in Fig. 4-8. The I-AUV was commanded to autonomouslyfollow a rectangular path in cylindrical coordinates, includingmotions around and along the pipe, see Fig. 4. Due to pipedetection errors the path was generated assuming a pipediameter which was significantly smaller than the real one,hence ensuring that the contact will always occur. Moreover,

North [m]

Dept

h [m

]

East [m]

3.2

-2

3

-1.5

2.8

2.6

-1

0.80.60.4

2.4

0.20-0.2-0.4-0.6-0.8

Fig. 4. Path of end-effector during pipe inspection. Shown are the stiffdesired path (red), computed based on pipe detection with a pipe diameterset to 350 mm, and the actual path (blue), generated by the contact withthe real pipe of diameter 500 mm. The two pipes do not coincide becausethere is a detection error, mainly in the distance between robot and pipe.

0 100 200 300 400 500 600 700Time [s]

-5

0

5

10

15

20Fo

rce [N

]

Fig. 5. Tracking of the desired contact force, along z-axis of {E}. Redline denotes the desired force and blue is the measured force.

0 100 200 300 400 500 600 700Time [s]

0

50

100

150

200

Stiff

ness

[N/m

]

Fig. 6. Value of the variable stiffness in the impedance model. In this caseall diagonal entries of KS were set to the same value.

the area around the top of the pipe was selected on purpose,to show how the automatic redundancy reduction kicks inwhen multiple system axes align, see Fiq. 7. Contact betweenthe tip of the end-effector and the surface of the pipe waskept properly while following the path, as shown in Fig. 5.The impact behaviour can be observed just before the desiredforce is set. At this stage the admittance model uses defaultparameters. When the system stabilised, the desired forcewas set to 8 N and the automatic stiffness adjustment gotengaged, see Fig. 6. Looking at Fig. 8 it can be noticed that

0 100 200 300 400 500 600 700Time [s]

0

0.5

1

1.5

2Va

lue [1

]

Fig. 7. Functioning of the automatic redundancy reduction algorithm. Blueline is the value of MOM and red line is the number of locked DOF.

0 100 200 300 400 500 600 700Time [s]

-1

-0.5

0

0.5

1

1.5

Angl

e [ra

d]

q 1q2q3q4

Fig. 8. Compliancy with the manipulator joint limits (inequality task).Continuous lines denote joint positions, dashed lines represent limits.

the joint movements were not smooth. This effect occurs dueto the fact that the path is followed point by point with someresolution, the joint limits get activated and the manipulatorexhibits velocity dead-band. The overall performance of thesystem is severely limited by the used manipulator and apoor control of vehicle velocity in heave.

VI. CONCLUSIONS

The paper presents a control system for an I-AUV toenable contact force control during intervention missionswhich require it. The control ideas are based on admittancecontrol combined with a modern task-priority framework.The admittance control was modified to allow direct controlover the contact force value by introducing variable, self-adjusting stiffness matrix. Moreover, a feed-forward forceis generated by the algorithm to maintain contact in caseof velocity controller instability during collision. There isalso an original idea of a redundancy reduction algorithmdescribed. It enabled correct system operation in singularconfigurations, often encountered when using our 8 DOF I-AUV. The theory is proven by the results of a successfulexperimental trial representing a pipe inspection operation.

ACKNOWLEDGMENT

This work is part of a project titled ”Force/position controlsystem to enable compliant manipulation from a floating I-AUV”, which received funding from the European Union’sHorizon 2020 research and innovation programme, under theMarie Sklodowska-Curie grant agreement no. 750063.

REFERENCES

[1] G Antonelli. Underwater Robots, volume 96 of Springer Tracts inAdvanced Robotics. Springer, Cham, December 2013.

[2] M R Arshad, S M Suboh, I A Rahman, and M N Mahyuddin. FuzzyLogic Approach for Hybrid Position/Force Control On UnderwaterManipulator. In Proceedings of Malaysia-Japan International Sympo-sium On Advanced Technology, November 2007.

[3] P Baerlocher and R Boulic. Task-priority formulations for thekinematic control of highly redundant articulated structures. InProceedings of IEEE International Conference on Intelligent Robotsand Systems, 1998.

[4] A Carrera, N Palomeras, N Hurtos, P Kormushev, and M Carreras.Learning by demonstration applied to underwater intervention. In Pro-ceedings of the Seventeenth International Conference of the CatalanAssociation of Artificial Intelligence (CCIA), 2014.

[5] E Cataldi and G Antonelli. Basic interaction operations for anunderwater vehicle-manipulator system. In Proceedings of 17thInternational Conference on Advanced Robotics, October 2015.

[6] S K Choi, G Y Takashige, and J Yuh. Experimental study on anunderwater robotic vehicle: ODIN. In Proceedings of IEEE Symposiumon Autonomous Underwater Vehicle Technology, December 1994.

[7] P Cieslak, P Ridao, and M Giergiel. Autonomous underwater panel op-eration by GIRONA500 UVMS: A practical approach to autonomousunderwater manipulation. In Proceedings of IEEE InternationalConference on Robotics and Automation, 2015.

[8] Yong Cui and N Sarkar. A unified force control approach toautonomous underwater manipulation. In Proceedings of IEEE In-ternational Conference on Robotics and Automation, 2000.

[9] J C Evans, P Redmond, C Plakas, K Hamilton, and D Lane. Au-tonomous docking for Intervention-AUVs using sonar and video-basedreal-time 3D pose estimation. In Proceedings of OCEANS 2003, 2003.

[10] H Farivarnejad and S Ali A Moosavian. Multiple Impedance Controlfor object manipulation by a dual arm underwater vehicle–manipulatorsystem. Ocean Engineering, 89(C):82–98, October 2014.

[11] N Jamali, P Kormushev, A C Vinas, M Carreras, and D G Caldwell.Underwater robot-object contact perception using machine learning onforce/torque sensor feedback. In Proceedings of IEEE InternationalConference on Robotics and Automation, January 2015.

[12] D M Lane, JBC Davies, and G Casalino. AMADEUS: AdvancedMAnipulation for DEep Underwater Sampling. IEEE Robotics &Automation Magazine, 4(4):34–45, December 1997.

[13] G Marani and J Yuh. Introduction to Autonomous Manipulation, CaseStudy with an Underwater Robot, SAUVIM, volume 102. Springer,Berlin, April 2014.

[14] Y Nakamura. Advanced robotics: redundancy and optimization, 1990.[15] E Olguin-Diaz and G Arechavaleta. A passivity-based model-

free force–motion control of underwater vehicle-manipulator systems.IEEE Transactions on Robotics, December 2013.

[16] N Palomeras, P Ridao, D Ribas, and G Vallicrosa. Autonomous I-AUVDocking for Fixed-base Manipulation. IFAC Proceedings Volumes,47(3):12160–12165, January 2014.

[17] D Ribas, P Ridao, Lluıs Magı, N Palomeras, and M Carreras. TheGirona 500, a multipurpose autonomous underwater vehicle. InProceedings of OCEANS 2011, April 2011.

[18] V Rigaud, E Coste-Maniere, and M J Aldon. UNION: underwaterintelligent operation and navigation. IEEE Robotics & AutomationMagazine, 5(1):25–35, March 1998.

[19] B Siciliano, L Sciavicco, L Villani, and G Oriolo. Robotics. Modelling,Planning and Control. Springer Science & Business, London, 2009.

[20] E Simetti and G Casalino. A Novel Practical Technique to IntegrateInequality Control Objectives and Task Transitions in Priority BasedControl. Journal of Intelligent & Robotic Systems, December 2016.

[21] E Simetti, G Casalino, S Torelli, A Sperind, and A Turetta. FloatingUnderwater Manipulation: Developed Control Methodology and Ex-perimental Validation within the TRIDENT Project. Journal of FieldRobotics, 31(3):364–385, 2014.

[22] E Simetti, S Galeano, and G Casalino. Underwater Vehicle Manipula-tor Systems: Control Methodologies for Inspection and MaintenanceTasks. In Proceedings of OCEANS 2016, May 2016.

[23] E Simetti, F Wanderlingh, S Torelli, M Bibuli, A Odetti, G Bruzzone,D L Rizzini, J Aleotti, G Palli, L Moriello, and U Scarcia. AutonomousUnderwater Intervention: Experimental Results of the MARIS Project.IEEE Journal of Oceanic Engineering, pages 1–20, September 2017.