Vision-based manipulation with the humanoid robot Romeo

Transcription

1 Vision-based manipulation with the humanoid robot Romeo Giovanni Claudio, Fabien Spindler, François Chaumette To ite this version: Giovanni Claudio, Fabien Spindler, François Chaumette. Vision-based manipulation with the humanoid robot Romeo. IEEE-RAS International Conferene on Humanoid Robotis, Humanoids 216, Nov 216, Canun, Mexio. <hal > HAL Id: hal Submitted on 21 Ot 216 HAL is a multi-disiplinary open aess arhive for the deposit and dissemination of sientifi researh douments, whether they are published or not. The douments may ome from teahing and researh institutions in Frane or abroad, or from publi or private researh enters. L arhive ouverte pluridisiplinaire HAL, est destinée au dépôt et à la diffusion de douments sientifiques de niveau reherhe, publiés ou non, émanant des établissements d enseignement et de reherhe français ou étrangers, des laboratoires publis ou privés.

2 Vision-based manipulation with the humanoid robot Romeo Giovanni Claudio, Fabien Spindler and François Chaumette 1 Abstrat The aim of this paper is to show how visual servoing an help a humanoid robot to realize manipulation tasks with one and two hands, in onjuntion with a gaze ontrol. In addition, the use of vision in a losed-loop ontrol sheme allows to aomplish these tasks with high repeatability without an aurate alibration of the robot kinemati model, the intrinsi and extrinsi parameters of the amera. The first appliation shown is grasping: one arm is ontrolled in order to reah the desired pose for a suessful grasp along with a gaze ontrol that keeps hand and objet in the amera field of view. This approah is extended to the grasping of ylindrial objets, ignoring in the ontrol the orientation along the revolution axis, that ould ause an unneessary motion of the arm. Moreover, we show how to ontrol both arms with a master/slave approah for a two-handed manipulation without any fore ontrol. The robot is then able to solve a ball-in-maze game in augmented reality manipulating a tray with two hands, just using vision. A seondary task to avoid the joint limits using a large projetion operator is added to improve the reliability of the system. Finally, the framework is implemented and evaluated on the humanoid robot Romeo using the ViSP library. The soure ode of the libraries, demos and examples are ompletely aessible on GitHub, allowing an easy adaptation of the visual servoing framework to other robots. I. INTRODUCTION Humanoid robots are designed to emulate aspets of human form and behavior. Despite industrial manipulators and mobile robots, humanoid robots take advantage of human environments and equipments avoiding the need to alter surroundings and objets. These robots usually share similar kinematis to humans, as well as similar sensing. About 55 million years ago, something triggered a great explosion of different life forms on earth. Aording to Andrew Parker s Light-Swith Theory [1], the reason for sudden diversifiation of animals was due to the development of the first form of the eye, whih greatly inreased the predator-prey importane of natural seletion. Indeed, one of the most important sensory pereptual-ognitive systems of our brain is vision. In fat, aording to some studies, 5% of our brain is involved in visual proessing and 7% of all our sensory reeptors are in our eyes [2]. With this in mind, it goes without saying why vision has beome essential in robotis. Thanks to vision, the robots, before onsidered blind mahines, aquire a new sense, improving their dynami interation with the environment and giving them the ability to perform daily human tasks. Humanoid robots, despite industrial robots, usually have a low repeatability due to the disrepany between model and 1 G. Claudio, F. Spindler and F. Chaumette are with Inria at IRISA in Rennes, Frane. (giovanni.laudio@inria.fr, fabien.spindler@inria.fr, franois.haumette@inria.fr) This work was supported by Equipex Robotex and Oseo Romeo 2 projet. real robot, beause of inaurate manufaturing proesses and errors in the measurement of the joint positions. One way to solve this issue is to adopt the visual servoing approah, that allows to define a losed-loop sheme using information oming from a vision sensor. An important task that humanoids should aomplish is grasping [3][4]. During the past deades, several works have addressed this hallenge by proposing sensor-based ontrol methods [5][6][7]. In [8] the robot HRP-2 grasps a simple ball while walking thanks to visual servoing. Using the robot ARMAR-III, a hybrid approah, whih ombines visual estimations with orientations omputed through the kinemati model, is used to ontrol the movement of a humanoid arm in [9]. The authors of [1] present a framework for visual servo ontrol of the REEM robot upper body for a box grasping. In [11] a vision/fore oupling approah is used to guide the robot hand towards the grasp position and perform a task manipulation taking into aount external fores. Finally, in [12] a method for fast visual grasping of unknown objets with a multi-fingered roboti hand is desribed. In the last deades, with the advent of humanoid robots and bi-manual industrial robots, the interest regarding dual arm manipulation has inreased. The most ommon approah adopted is the hybrid fore/position ontrol and impedane ontrol [13]. In [14] the target objet and both hands are traked alternately, and a ombined open-losed-loop ontroller that uses a feedbak from the fore/torque sensors is used for positioning the hands with respet to the target. In absene of fore sensors, other approahes have to be investigated. In this paper, we show a solution for dual arm manipulation based solely on vision. Aside from grasping a ylindrial objet, where we propose a lever set of visual features allowing an adequate behavior, this paper does not present any partiular new methodologial ontributions. In fat, the aim of this work is to offer to the ommunity a omplete and fully explained appliation and integration of visual servoing approahes to ontrol a humanoid robot, as well as the omplete orresponding C++ ode. We also show how visual servoing is robust to modeling and alibration errors by intentionally adding errors in the intrinsi and extrinsi amera parameters. Moreover, the framework is appliable to any humanoid robot to ahieve tasks suh as manipulation with one arm, dual arm oordination and gaze ontrol. Here it is tested on the Romeo robot from SoftBank Robotis. The paper is organized as follows: in Setion II basi notions of visual servoing are realled. In Setion III the following visual servoing shemes are presented: an Image- Based Visual Servoing (IBVS) for gaze ontrol, a Pose-Based

3 Visual Servoing (PBVS) for the grasping of a generi objet, a speial PBVS for the grasping of a ylindrial objet and finally a master/slave approah for two-handed manipulation using only vision. Setion IV shows the implementation and the results obtained on Romeo and Setion V the software involved. Finally, Setion VI features a summary and ideas for future works. II. VISUAL SERVOING Visual servo ontrol refers to the use of information oming from vision sensors and extrated using omputer vision algorithms to ontrol the motion of a dynami system, suh as roboti arms, mobile robots or virtual ameras. The aim of a vision-based ontrol sheme is to minimize an error e(t) built from the differene between an atual feature vetor and the orresponding desired feature vetor [15]: e (t) = s(t) s (1) With a vision sensor providing 2D measurements, potential visual features are numerous. An IBVS uses diretly 2D data information as features (for instane oordinates of image points, moments, lines and areas), while a PBVS uses 3D parameters extrated from the image measurements. In addition, it is possible to ombine 2D and 3D visual features (Hybrid Visual Servoing) to take advantage of eah approah while avoiding their respetive drawbaks. Considering a robot, a amera ould be attahed to its endeffetor (eye-in-hand visual servoing) or it an be external observing the sene (eye-to-hand visual servoing). A visual servoing sheme an be defined to ontrol in veloity the joints of a robot in order to redue the error expressed in (1), omputed using the information oming from the amera: where: q = λĵ+ e e Ĵ+ e s t + P λg (2) Ĵ+ e is an estimation of the Moore-Penrose pseudoinverse of the task Jaobian, a ombination of the interation matrix and the artiular Jaobian of the robot. The form of J e depends on the type of visual servoing task adopted (see Setion III). is an estimation of the features veloity due to the generally unknown target motion. P λ is the large projetor operator proposed in [16] that allows to perform a seondary task even when the main task is full rank. It is defined as: s t P λ = λ ( e ) P e + ( 1 λ ( e ) ) P e (3) with P e = (I n J + e J e ) the lassial projetion operator and P e the new projetion operator that imposes the exponential derease of the norm of the error, instead of eah term of the error vetor: 1 P e = I n e J e J ee J eee J e (4) To ensure the onvergene of the system, sine that (4) is singular when e =, a swithing strategy is defined Fig. 1. Romeo frames definition: {t} is the fixed base frame plaed in the torso, {} is the amera frame on the eye of the robot, {w} is the wrist frame in the last joint of the hain of the arm and {h} is the frame plaed on the hand target. using a sigmoid funtion λ ( e ), in order to swith from P e to P e when the norm of the error is reahing zero. g is a vetor that defines the seondary task, it an be designed for instane to avoid joint limits, obstales, olusions or singularities. In this paper, we designed the seondary task for avoiding the joint limits as desribed in [17] to make the veloity ontroller more reliable. III. VISUAL SERVOING APPLIED TO A HUMANOID ROBOT Visual servoing is nowadays well established for the ontrol of manipulator and mobile robots. An humanoid robot an be seen as a tree struture with a base link and several terminal links. Several branhes an be defined starting from the base to eah terminal link suh as hands, feet, and eyes (see Figure 1). Eah branh an be onsidered as a serial robot and therefore it is possible to ontrol it with the same tehnique. The analysis inreases in omplexity with the addition of more branhes, to perform different tasks or in oordination. In this setion we present some visual servoing shemes for a humanoid robot, to aomplish tasks suh as traking a target with the gaze, grasp an objet and use its hands together. The methodologial approahes for grasping an objet (Setion III-A) and the gaze ontrol (Setion III-D) stem from the work presented in [1] on the robot REEM while the sheme for grasping a ylindrial objet (Setion III-B), for dual arm manipulation (Setion III-C) and the appliations (Setion IV) are presented here for the first time. A. Visual servoing for grasping Humanoid robots need to perform everyday tasks similar to those performed by humans. One of the most important is the manipulation of objets. We will define here a ontrol sheme that allows the robot to grasp a known objet. We onsider an eye-to-hand PBVS to ontrol the arm joints using a amera in the robot s head. During the ontrol, beause of

4 Fig. 2. Frames definition for grasping an objet. The frame {o} is attahed to the objet, {h} to the marker on the hand and {h } represents the desired pose of the hand. The transformation matrix o Mh is onstant. At the end of the PBVS the frames {h} and {h } will be oinident. inauray in the kinemati model of the robot, we need to ompute the atual pose of the hand at eah iteration, using vision. Moreover, the pose of the objet to grasp (see Figure 2) is also required at eah frame. The 6D pose of the hand Mh and of the objet Mo an be estimated for instane using visual markers plaed on them, or using a Model-Based Traker (MBT) if the model is known. A goal frame Mh is also required to ompute the servo. The pose Mh is equal to the pose of the objet to grasp multiplied by a onstant transformation o Mh, that an be easily learned by plaing the hand at the desired position with respet to the objet and using the following equation: 1 o (5) Mh = h M Mo During the visual servoing we want to minimize the error between the pose {h} and {h }: h 1 Mh = o M 1 Mh h Mo (6) From h Mh we an extrat the error vetor omposed by the error in translation and in orientation using the angleaxis representation: (7) eh = h th, h θuh and the orresponding interation matrix as shown in [15]: h Rh 3 Le = 3 Lθu (8) 2 sinθ θ [u] Lθu = I3 2 [u] + 1 sin 2θ 2 Finally, the joint veloity vetor is omputed from (2) setting s t equal to zero: q h = λj+ h eh + Pλ g with Jh = Le h Vw w Jw (qh ) Fig. 3. Frames definition for grasping of a ylindrial objet. {yl} is the frame attahed to the objet, {h} to the marker on the hand and {yl} represents the atual pose that we are ontrolling. The transformation matrix hm yl is onstant. At the end of the PBVS the frames {h} and {h } have to be oinident exept for the rotation around the vertial axis, that is not relevant for the grasping, being the objet symmetrial. This sheme produes, under ideal ondition, a pure straight line of the hand in the Cartesian spae. B. Visual servoing for ylindrial objets In Setion III-A we have seen how to deal with the grasping of a generi known objet. Here we propose a new tehnique to improve the grasping for ylindrial objets based on the fat that the rotation around the revolution axis of the objet is not relevant. In fat using a lassi PBVS with all 6 Degrees Of Freedom (DOF) onstrained, a small rotation of the objet ould ause a large motion of the arm, augmenting the risk of reahing joint limits. For this reason, we modify the PBVS sheme proposed in Setion III-A. Let Myl be the transformation between the atual frame of the ylindrial objet with respet to the amera frame (as shown in Figure 3). To define the error vetor and the interation matrix, the transformation yl Myl = h M 1 yl Myl is used, where Myl = Mh Myl. The transformation matrix Mh refers to the hand target with respet to the amera and it is obtained by vision. The transformation h Myl is a onstant matrix from the hand target to the objet after having plaed the hand manually at the desired pose for a suessful grasping (learned pose omputed by vision). The features vetor s has to be designed to ignore the rotation around the revolution axis of the objet (illustrated as the blue axis zyl of the frame {yl } in Figure 3). The following feature vetor is onsidered: yl tyl s = yl (11) zyl zd (9) where h Vw is the onstant twist transformation matrix to express the Jaobian w Jw (whih refers to the wrist joint) with respet to the hand frame {h}: h Rw h t w h Rw h Vw = (1) h Rw 3 where yl zyl is the third olumn vetor of the rotational matrix yl Ryl. Sine we want the revolution axis zyl of the ylinder and the axis related to the atual pose of the hand zyl to be oinident, we set zd = [ 1 ].Therefore, due to the ross produt involving zd in (11), the last element of the feature vetor s is always equal to zero. Finally, we an ompute the error vetor e = s with s = and the interation matrix Le :

5 Fig. 4. Frames definition for a dual arm manipulation visual servoing sheme. {o} is the frame attahed to the objet, {h l } and {h r} to the hand markers and {o } represents the desired pose of the objet. L e = [ yl ] R yl 3 3 [ yl z yl ] [z d] (12) We an notie that the rank of L e is equal to 5 sine its last row is omposed of six zero elements (due to the last omponent of e, that is always equal to zero). This is oherent with the fat that, for this task, only 5DOF an be ontrolled, beause of the ylinder symmetry. For this ase then, the task Jaobian expressed in (9) has the following form: J h = L e yl V w w J w (q h ) Thanks to this sheme, when the visual servoing has onverged, the axis z yl will be aligned to z yl so that the rotation error around z yl will be ignored as expeted (see Figure 3). C. Visual servoing for dual arm ontrol In this setion, we onsider the two-handed manipulation problem, where two arms reate a losed kinemati hain holding a rigid body objet with fixed grasp handles. For dual arm manipulation, ommon solutions are hybrid fore/position ontrol and impedane ontrol [18][19], but if the arms are not equipped with any fore sensors, as in our ase, an alternative solution is needed. The aim here is to ontrol both arms using only vision information, in order to apply any translation and rotation to the grabbed objet, just knowing its pose with respet to the amera. We onsider the robot grabbing a tray as in Figure 4. The matries M hl and M hr are the transformations from the amera to the left and right hand targets while M o is the matrix expressing the pose of the objet with respet to the amera. 1) Initialization: In this phase, the onstant transformations between eah hand target and the objet are omputed: h l M o = M 1 h l M o h r M o = M 1 h r M o (13) 2) Dual arm visual servoing: Even if eah arm has 7 DOF, the mobility of the grasped objet, when distant from joint limits and singularities, is equal to 6. A master/slave approah is used, where the left arm is the leader and the right arm is the follower. A PBVS is defined to ontrol diretly the pose of the objet {o}, onsidered now as part of the hain of the robot, instead of the hand pose {h} as in Setion III-A. The joint veloities an be omputed, setting s t equal to zero, with the following ontrol sheme: q hl = λj + o l e o + P λ g with J ol = L o w e V l wl J wl (q hl ) (14) In this ase o V wl is the onstant twist transformation matrix used to express the Jaobian of the left arm J wl in the objet frame {o}, and is omputed from the transformation matrix o M wl = ( w l h M l hl M o ) 1. L e is the lassial interation matrix for PBVS (8) and e o = ( ) o t o, o θu o is the error between the desired and atual pose of the objet. Assuming that the two arms are not hanging their relative pose with the objet and themselves, we an impose the following kinemati onstraint: v o = J or q hr = J ol q hl (15) Solving (15) for q hr, the joint veloities to apply to the follower arm are given by: q hr = J + o r J ol q hl (16) This sheme allows moving the objet by oordinating both hands from the urrent position M o to the desired one M o following, in the ideal ase, a 3D straight line trajetory. D. Gaze ontrol Gaze ontrol is a fundamental behavior for a humanoid robot beause it allows keeping the interested target(s) in the field of view, overoming the issue of a narrow field of view of the amera. For the gaze ontrol several joints of the humanoid robot an be involved, suh as the joints of trunk, nek, head and eyes. An eye-in-hand IBVS is implemented, where the task onsists of traking an image point x in the amera frame. For this task we hoose as feature the vetor s = (x, y), ontaining the oordinates of the image point x. The image point ould be the enter of an objet to grasp, a hand, a fae or even the midpoint of some of them. The error will be in this ase e x = (s s ), with s = (x, y ), the vetor ontaining the oordinates of the desired image point. The joints veloities q g to minimize the error an be omputed using the following equation: q g = λĵ+ g e x Ĵ+ g The task Jaobian J g is equal to: s t + P λg (17) J g = L x V e e J e (q g ) (18) with V e the twist transformation matrix to express the Jaobian e J e (whih refers to the last joint of the hain,

6 the eye) with respet to the amera frame {}. Lx is the lassial interation matrix related to the oordinates of an image point x = (x, y): 1 x xy (1 + x2 ) y Z Z (19) Lx = xy x Z1 Zx 1 + y 2 where Z is the depth of the observed point. If the traked objet is known, the depth value an be omputed from its vision-based pose, otherwise a onstant positive oarse value is suffiient for this purpose. The term s t is added to take into onsideration perturbations to the system. If the motion of the target objet is diffiult to estimate, this term an be set equal to zero aepting that a traking error will be observed. Otherwise, when the target on the hand is traked, a feed-forward term an be added to remove the traking error: we an set s w t = Lx Vw Jw (qh ) q h as already done in [1]. Fig. 5. Calibration errors in the kinemati model. The blue frame represents the sensed WristPith frame omputed using the robot model and the enoder sensor. The RGB frames represent the sensed target pose and the one omputed by vision. IV. I MPLEMENTATION ON ROMEO Romeo, a humanoid robot developed by SoftBank Robotis, is intended to be in the future a genuine personal assistant and ompanion. Romeo is 1,4 meters tall and it weights 4 kg. It is equipped with 37 motors, four ameras, four mirophones, speakers, a tatile sensor on the head and IMUs. Two mobile ameras are plaed on the eyes while other two are fixed in the forehead. In order to overome the large alibration inauray of the kinemati model and the sensor errors (see Figure 5), the 6D pose of the hand is estimated using a known dimension target and ViSP pose estimation algorithm. For the experiments, two kinds of targets were used alternatively: a QR-Code and a target omposed by four blobs (one red and the others blak). Both targets are automatially deteted and a oarse estimation of their fixed pose w Mh with respet to the last joint of the arm is suffiient. Due to the low resolution of the images, the detetion and traking resulted more robust using the target with blobs. During all the experiments we used the left eye amera. In fat, when dealing with objets of known dimensions, only one amera is needed, avoiding any alibration to know the relative pose between two ameras. The robot Romeo is onneted via Ethernet able to a remote omputer, where all the libraries are installed and where the main algorithm is running. The images are aquired from the robot amera with QVGA (32x24) resolution at a frame-rate of approximately 15Hz. The general ontrol sheme, whih runs at the frequeny of the aquisition rate of the amera, an be summarized as follow: 1) Get a new image from the robot. 2) Compute the atual visual features vetor s. 3) Get the atual state of the robot q (joint positions). 4) Get the robot Jaobians (from torso to arm and eye). 5) Compute e, Le and the task Jaobian. 6) Compute the ontrol law to get the joint veloities q. 7) Add the seondary task veloities. 8) Send a new ommand to the robot (all the joint veloities are applied at the same time). Fig. 6. Romeo detets a box and the table, grasp it and it delivers it to a human. In this setion we desribe how the visual servoing shemes presented in Setion III have been implemented and validated on Romeo (see aompanying video): Romeo grasps a box on a table and finally delivers it to a human [2]. Romeo grasps a an and puts it bak on a table [21]. Romeo manipulates with its hands a tray in order to solve a ball-in-maze game in augmented reality [22]. A. Grasping an objet using vision 1) Grasping a box: The implementation is mainly omposed by two veloity ontrollers: an arm and a head-eye gaze ontroller. Eah of Romeo s arm is omposed by seven DOF: two in the shoulder, two in the elbow and three in the wrist. Sine the hand is not equipped with any fore sensor, only vision is used to determine when the hand is near enough to the objet to perform a suessful grasping. Sine the box has known dimensions, the MBT available in ViSP is used to ompute its pose. Here the steps of the demonstration are desribed: a) Romeo detets the box using olor information and traks it with its gaze (Figure 6.A). For this task, six joints are ontrolled: two in the nek, two in the head and two in the eye. This is a typial eye-in-hand IBVS, by having a 2D image point as feature: the enter of the box (see Setion III-D). For the detetion by olor, OpenCV is used. b) One the box is on the table, Romeo estimates the box pose to initialize automatially the MBT (Figure 6.B).

7 Here the ViSP objet loalization algorithm uses key points to detet and estimate the objet pose using its known model and texture. ) Gaze ontrol: An IBVS is implemented to see the box on the right part of the image. This is neessary to see both the hand and the box in the narrow field of view of the amera. d) Open-loop motion of the arm to get lose to the box using the robot odometry. This is not a repeatable motion and a losed-loop is neessary to grasp the box suessfully. e) The QR-Code on the hand is automatially deteted and its pose is omputed using the four image points of the orners and its dimension. f) Grasping phase steps (Figure 6.C): IBVS (see Setion III-D) ontrolling the gaze to keep both the hand (QR-Code) and the box in the field of view of the eye s amera. The visual feature used is the image midpoint of the QR-Code and the box in the image. PBVS (see Setion III-A) to move the hand from the atual pose (extrated estimating the pose of the QR- Code) to the desired one (the grasping pose omputed from the atual pose of the box). Note that the box an be plaed at any reahable loation and that the arm will adjust reatively its pose if the box is moved during the grasping proess. g) One the desired pose is reahed, Romeo loses its hand and grasps the box. h) Romeo raises the head looking for a human. One a fae is deteted an IBVS starts to trak it. i) Finally, Romeo delivers the box to the human moving the arm toward him (Figure 6.D). 2) Grasping a an: This ase is similar to the previous task, exept that the objet to grasp has a ylindrial shape. For this reason the approah presented in Setion III-B is used. Thanks to this visual servoing sheme, the hand moves, following a straight line in the Cartesian spae, to the nearest onvenient pose for a suessful grasping. If a rotation around the vertial axis is applied to the an, the final desired pose of the hand will not hange (as shown in the video [21] and in Figure 9.C). This allows to improve the robustness of the grasping. Also in this ase, the ViSP MBT is used to trak the an. In the demonstration, Romeo grasps the an, raises it and finally puts it bak on the table. Figure 7 shows the task error and the joint veloities relative to the PBVS, whih moves the arm from its atual position to the desired one. As expeted, the omponent r z is always zero. The experiment was repeated while adding intentionally an error in the intrinsi and extrinsi parameters in order to show that, thanks to visual servoing, the system is robust against this kind of perturbations (see Figure 8). B. Dual arm manipulation - Ball-in-Maze game To demonstrate that it is possible to perform a dual arm manipulation only with vision, without any fore ontrol, we developed an augmented reality demonstration using the ontrol sheme explained in Setion III-C. t, θu Error (m,rad) Task Error t x t y t z θu x θu y θu z Veloities (rad/s) Joint veloities ShoulderPith WristPith Fig. 7. Task error and joint veloities of Romeo s left arm while grasping a an using a PBVS (see Setion IV-A.2). t, θu Error (m,rad) Task Error t x t y t z θu x θu y θu z Veloities (rad/s) Joint veloities ShoulderPith WristPith Fig. 8. Task error and joint veloities of Romeo s left arm while grasping a an using PBVS and adding some bias. The bias added to the estimated extrinsi parameters were: 3m in eah translation axis and 2 degrees in rotation. The bias added to the amera intrinsi parameters were: +4 pixels in the foal lengths and +1 pixels in the prinipal point. Fig. 9. Romeo grasps a an ignoring the orientation along the revolution axis in the ontrol. Fig. 1. Romeo solves a ball-in-maze game in augmented reality using two hands. Only vision is used to ontrol both arms.

8 Veloities (rad/s) t, θu Error (m,rad) Task Error t x t y t z θu x θu y θu z Joints Veloities Leader Arm ShoulderPith WristPith Veloities (rad/s) Joints Veloities Follower Arm ShoulderPith WristPith Fig. 11. Plots related to the initial Master/Slave PBVS experiment shown in Setion IV-B, whih moves the tray at a desired position before starting the ball-in-maze game. Veloities (rad/s) t, θu Error (m,rad) Task Error t x t y t z θu x θu y θu z Joint Veloities Leader Arm ShoulderPith WristPith Veloities (rad/s) Joint Veloities Follower Arm ShoulderPith WristPith Fig. 12. Same Master/Slave PBVS experiment shown in Figure 11 while adding some bias in the intrinsi parameter: +3 pixels on the foal lengths and +15 pixel on the prinipal point. As we an see, the system is robust against this kind of perturbations In this ase we onsider a oordinated manipulation where the two arms (14 joints in total) have to hold and manipulate the same objet. The motion of the arms has to be well synhronized otherwise the objet ould break, fall or fail to reah the desired pose. The two arms are holding a tray from two handles as in Figure 1. A known piture is plaed on the tray, whih is automatially deteted and then traked using the template traker in ViSP, that omputes its 6D pose with respet to the amera. First of all, the transformation matries from the hand targets to the objet have to be omputed using (13). Sine the field of view of the amera is not large enough to see in the same images the two hand targets and the objet, it is neessary to ompute before the transformation of the right hand, then move the head, and finally ompute the transformation for the left hand. While the head is moving, the piture is ontinuously traked, even if only a portion of it is visible in the image (see video [22]). One the alibration phase is ompleted, we need to trak only the piture. The tray is moved to a onvenient position (parallel to the ground) in order to start the game (see Figure 11 and 12). Then a virtual maze is added in augmented reality on the top of the tray and its pose is diretly linked with the pose of the piture. The aim of the game is to roll the virtual ball from its atual position to the end of the maze. First, Romeo heks where the ball is in the maze using the simulation information and then, it rotates the tray in order to roll the ball to the next orridor until the end of the maze, by taking advantage of its walls and ontrolling both arms simultaneously. V. SOFTWARE A tutorial and the soure ode to reprodue the grasping demos (box and an) an be found in GitHub 1, as well as for the dual arm manipulation demo 2. The main software used to develop this framework are the SoftBank Robotis SDK C++, ViSP [23], ViSPNaoqi, Panda3D [24], Metapod [25] and OpenCV [26]. ViSP (standing for Visual Servoing Platform) is a modular ross platform library that allows prototyping and developing appliations using visual traking and visual servoing tehnis to ontrol the robots. It provides a set of visual features that an be traked using real-time image proessing and omputer vision algorithms. ViSPNaoqi is a bridge between ViSP and the SoftBank Robotis SDK C++, the library that ontains all the tools to manage the robot. ViSPNaoqi is used mainly to grab images from Romeo, estimate the intrinsi and extrinsi amera parameters, ontrol the robot in veloity and get the atual artiular Jaobians. The library is available online 3 and it an also be used with other robots ompatible with NAOqi (like Nao and Pepper). Panda3D is a game engine, a framework for 3D rendering and game development for Python and C++ programs. It is 1 Romeo-Grasping-Demo

9 used for the augmented reality demonstration, to simulate the ball rolling on the maze, and for handling the ollisions with the floor and the wall. Two threads are reated for the ballin-maze demonstration: the first one is related to the image proessing and ontrol of Romeo, while the seond one takes are of the ball-in-maze simulation in augmented reality. In order to ompute the kinemati model of the robot, the software Metapod is used. Metapod (for META- Programming Optimized Dynamis library) provides robot dynamis algorithms making use of R. Featherstone s Spatial Algebra [27] to desribe fores, motions and inertias. To reate the model, the URDF desription of the robot is neessary. OpenCV (Open Soure Computer Vision) is a library aimed to develop real-time omputer vision algorithm. Here, it is used for the fae reognition, olor detetion, olor segmentation, and indiretly by ViSP. VI. CONCLUSIONS In this paper, several visual servoing shemes have been presented: an IBVS for gaze ontrol, a PBVS for grasping of a generi objet, a speial PBVS for grasping of a ylindrial objet, and finally a master/slave approah for two-handed manipulation using only vision. These shemes lead to the implementation of real appliations, ahieved on the humanoid robot Romeo: box grasping, an grasping, and a twohanded manipulation for holding a tray, in order to solve a ball-in-maze game in augmented reality. The implementation on Romeo demonstrates that these tasks an be exeuted with high reliability and robustness despite an impreise kinemati model, bakslash of the joints, small resolution and low frame rate of the images oming from Romeo s amera. In addition, to improve the robustness, if one of the traked targets is lost, Romeo re-detets it automatially and initializes the traker with the new estimated pose. Moreover, the soure ode is entirely available to the ommunity in order to simplify the implementation of our work on other humanoid robots. This work gives room for numerous extensions and improvements. The next step is to adopt a MBT to trak and estimate the 6D pose of the hand, allowing to get rid of the additional visual targets. The algorithms (or part of them) ould be run diretly on Romeo s CPUs in order to overome the bottlenek of ommuniation between a remote omputer and the robot. Moreover, this work ould be joined with a whole-body ontrol framework, in order to take into aount other important tasks suh as the overall stability of the robot. When the robot will have a stable walk, the aim is to ontrol its gait and the diretion of its head using visual information. When walking, the images aquired by the ameras will probably be very unstable beause of the steps of the robot. Traking algorithms have to be redefined or modified to take into aount the large displaements of objets in the images even if the gaze ontrol should diminish this effet. REFERENCES [1] J. Lupovith and G. A. Williams, In the blink of an eye: How vision sparked the big bang of evolution, Arhives of Ophthalmology, vol. 124(1):142, 26. [2] E. N. Marieb and K. Hoehn, Human anatomy & physiology. Pearson Eduation, 27. [3] K. B. Shimoga, Robot grasp synthesis algorithms: A survey, The Int. Journal of Robotis Researh, vol. 15(3):23-266, [4] A. Bihi and V. Kumar, Roboti grasping and ontat: a review, in IEEE ICRA, vol. 1(1): , 2. [5] R. Horaud, F. Dornaika, and B. Espiau, Visually guided objet grasping, IEEE ICRA, vol. 14(4): , [6] D. Kragić, A. T. Miller, and P. K. Allen, Real-time traking meets online grasp planning, in IEEE ICRA, vol. 3(1): , 21. [7] H. Fujimoto, L.-C. Zhu, and K. Abdel-Malek, Image-based visual servoing for grasping unknown objets, in IECON/IEEE Industrial Eletronis Soiety, vol. 2(1): , 2. [8] N. Mansard, O. Stasse, F. Chaumette, and K. Yokoi, Visually-guided grasping while walking on a humanoid robot, in IEEE ICRA, 27, pp [9] N. Vahrenkamp, S. Wieland, P. Azad, D. Gonzalez, T. Asfour, and R. Dillmann, Visual servoing for humanoid grasping and manipulation tasks, in 8th IEEE-RAS Int. Conferene Humanoid Robots, 28, pp [1] D. Agravante, J. Pages, and F. Chaumette, Visual servoing for the REEM humanoid robot s upper body, in IEEE ICRA, 213, pp [11] M. Prats, P. Martinet, A. P. Del Pobil, and S. Lee, Vision fore ontrol in task-oriented grasping and manipulation, in IEEE/RSJ IROS, 27, pp [12] V. Lippiello, F. Ruggiero, B. Siiliano, and L. Villani, Visual grasp planning for unknown objets using a multifingered roboti hand, IEEE/ASME Transations on Mehatronis, vol. 18(3):15-159, 213. [13] C. Smith, Y. Karayiannidis, L. Nalpantidis, X. Gratal, P. Qi, D. V. Dimarogonas, and D. Kragi, Dual arm manipulation a survey, Robotis and Autonomous systems, vol. 6(1): , 212. [14] N. Vahrenkamp, C. Böge, K. Welke, T. Asfour, J. Walter, and R. Dillmann, Visual servoing for dual arm motions on a humanoid robot, in on 9th IEEE/RAS Int. Conferene Humanoid Robots, 29, pp [15] F. Chaumette and S. Huthinson, Visual servoing and visual traking. In Handbook of Robotis, B. Siiliano, O. Khatib (eds.), Chap. 24, pp , Springer, 28. [16] M. Marey and F. Chaumette, A new large projetion operator for the redundany framework, in IEEE ICRA, 21, pp [17], New strategies for avoiding robot joint limits: Appliation to visual servoing using a large projetion operator, in IEEE/RSJ IROS, 21, pp [18] F. Caavale, P. Chiahio, A. Marino, and L. Villani, Six- DOF impedane ontrol of dual-arm ooperative manipulators, IEEE/ASME Trans. on Mehatronis, vol. 13(5): , 28. [19] R. Bonitz and T. Hsia, Internal fore-based impedane ontrol for ooperating manipulators, IEEE ICRA, vol. 12(1):78-89, [2] Video, Romeo humanoid robot grasping demonstration, [21], Romeo grasping a an by visual servoing, [22], Two-handed manipuation: Romeo solving a ball-in-maze game. [23] E. Marhand, F. Spindler, and F. Chaumette, ViSP for visual servoing: a generi software platform with a wide lass of robot ontrol skills, Robotis & Automation Magazine, IEEE, vol. 12(4):4-52, 25. [24] M. Goslin and M. R. Mine, The Panda3D graphis engine, Computer, vol. 37(1): , 24. [25] M. Naveau, J. Carpentier, S. Barthelemy, O. Stasse, and P. Soueres, METAPOD - Template META-programming applied to dynamis: CoP-CoM trajetories filtering, in 14th IEEE-RAS Int. Conferene on Humanoid Robots (Humanoids), 214, pp [26] G. Bradski and A. Kaehler, Learning OpenCV: Computer vision with the OpenCV library. O Reilly Media, In., 28. [27] R. Featherstone, Rigid body dynamis algorithms. Springer, 214.