Karim Bouyarmane

Antonio Paolillo

Don Pierre Gergondet

Joven Agravante

Benjamin Navarro

Stéphane Caron

Adrien Escande

Mitsuharu Morisawa

Mehdi Benallegue

Keywords: Task-space QP control, Concurrent vSLAM and 3D object tracking, humanoid loco-manipulations Commande QP dans l

Recently, the application elds of humanoids are gradually shifting from laboratory research prototypes, entainterrment, social assistant agent to large-scale manufacturing industries such as aeronautics, shipyards, and building. Indeed humanoid robots can assist and help in well-identied tedious non-added values tasks depending on the industrial context.

The challenge is to enable manufacturing humanoid robots to operate in their environment autonomously while performing tasks with, at least, human speed while being in close physical contact with other workers.

However, achieving human-like full-body motion for humanoid robots is still complex and requires additional research eorts; especially when it involves both manipulation and locomotion: the loco-manipulation. Manufacturing operations require robots capable of moving in large and changing workspaces and to manage dierent types of tasks; thus adaptability is paramount. This form of autonomy can embed the capacity to locate itself, to detect and manipulate various objects of interest to accomplish the task.

The purpose of this thesis is the sophistication of task-space control methods, in particular those formulated as quadratic optimization (QP). We show the benet of integrating QP controllers as a single problem for handling multi-robot (MQP) interactions (where any object is considered as an additional robot). We emphasized on the closed-loop force control where the MQP can switch in-between dierent modes enabling the robots to rely on force sensors in physical contact/interaction. Then, to integrate vision tasks, we developed a 3D model-based articulated object estimator relying on an RGB sensor that can also be formulated as a QP in the acceleration space. This estimator serves as input to the MQP (both QPs share the same object model) to achieve the desired manipulation task. We show that our algorithm is able to track articulated objects, cope with missing / occluded features and messy environments, where our humanoid robot HRP-4 manipulated them in dierent experiments.

In our previous works, we used a classic RGB-D camera that has already demonstrated its strength with numerous algorithms and applications. However in terms of vision sensors, the hardware is constantly evolving and bringing-up new capabilities. As for many reasons we only have a unique camera embedded in our humanoid robots, its choice is important. We conducted a benchmark study on dierent types of cameras to evaluate their performance in terms of reliability and accuracy for vSLAM under dierent perturbations in indoor contexts.

It helped us choose a camera knowing that it will concurrently be used for object tracking.

To use the full capability of this camera, we developed a 3D model-based object tracking using its new modality: the wide-angle depth image. Its use in conjunction with existing vS-LAM and MQP allows it to have a framework to autonomously perform loco-manipulation of large-scale and heavy objects, such as bobbins, to achieve industrial use-cases. We have carried out important and challenging experimental works to validate the proposed framework with our latest humanoid robot HRP-5P and its predecessor HRP-2Kai.

List of Figures

2.2

Predicted forces (green) in planned contact state {Contact 1, Contact 2, Contact 3} versus sensor forces (yellow) in real contact state {Contact 1, Contact 2} (the predicted forces are resultant at the sensor locations of the point forces in dashed lines computed at the vertices of the contact prints). In this situation the QP controller assumes that the robot is in the planned contact state and therefore predicts forces that do not correspond to the actual force repartition, since the hand contact (Contact 3) is not yet established. . . . .

2.3

Block diagram for force control with the QP. The error between the target force and the sensed force is converted into velocity with diagonal matrix gain K I . This velocity is low-pass ltered into a reference velocity for the end-eector QP tracking task. The target force comes either from the force output of the QP (switch 2 up) or from a external user command (switch 2 down). That external user command can also alternatively be incorporated inside the QP (switch 1 closed) in order to inuence the force output of the QP when using the latter as a target force (i.e. with switch 2 up). . . . . . .

2.4

Simplied representations of the dierent switch positions in the block diagram of Fig. The eect of the gains in diag(K I ) on the force time response and fast interactivity of the right hand of the HRP-4 robot to human interactive manipulation.

The same behavior (dierent gains for feet) are observed in the both hands and feet that are force-control to zero-force reference (F d = 0 in all components). The admittance gains are changed on-line three times during the manipulation from its initial (high damped) value to double and double again from the values. The higher the admittance gain, the faster the response and the more interactive the robot is to human guidance. . . . . . . . . . . . . . 19

2.6

Base experiment for comparing the dierent proposed QP force control paradigms.

Each hand is controlled with a dierent paradigm. . . . . . . . . . . . . . . . 20

2.7

Simultaneous multi-unilateral-contact force control of the humanoid robot HRP-4 in the setting of 2.9

Comparison between the two methods that account for a desired force command F d , in two additional instances of the experiment in Fig. 2.6. In the rst method, the robot follows the user command but reaches torque limits (over-torque errors appeared on the robot during both executions). The second method (right hand), the QP autonomously saturates the exaggerated force command to keep the robot within the torque limit constraint. The second method is thus the safest for the robot. . Hough-guided used in this work. The edge is not disturb by the moving part. [START_REF] Dalibard | Manipulation of documented objects by a walking humanoid robot[END_REF] 3.4

Moving-edge used in [START_REF] Comport | Kinematic sets for real-time robust articulated object tracking[END_REF]. The edge remains attached to the moving part. . . The rst one is demonstrated by Glory, a company in Saitama, Japan, using torso humanoid robots on xed based. The provider of such bi-arm, torso, head robots is Kawada Robotics, the manufacturer of the HRP humanoid family. In 2019, we successfully demonstrated a humanoid robot HRP-4 from Kawada company for aircraft manufacturing. The demonstrator consists of showing the possibility for a humanoid robot to use advanced walking and multi-contact technology, to move inside an airplane manufacturing shop-oor and perform a bracket assembly inside the fuselage of an A350 at Airbus Saint-Nazaire site [START_REF] Kheddar | Humanoid robots in aircraft manufacturing: The airbus use cases[END_REF]. In this project, the robot had to walk through the A350 mock-up that included stairs, then to stop near a table before to grasp a bracket, and nally to walk toward the fuselage to stick the bracket on it. In this demonstrator, the humanoid robot HRP-4 had to either perform locomotion or manipulation; as it was walking with an object some would consider it as loco-manipulation. This success was achieved thanks to the latest results in various scientic disciplines: multi-robot QP (Quadratic Programming) control, vSLAM (visual Simultaneous Localization And Mapping), 6D object model-based visual tracking, and robust humanoid robot locomotion..

The main objective of this thesis is to enhance our multi-robot QP controller with many novel additional features to endow humanoid robots with the capability to manipulate large objects while walking and using whole-body motion for assembly. Compared to what is achieved in [START_REF] Kheddar | Humanoid robots in aircraft manufacturing: The airbus use cases[END_REF], we want to go one step further to consider the loco-manipulation use-cases.

The humanoid has to walk and manipulate an object simultaneously and not sequentially.

In addition, we do not consider objects that the robot could lift-up and hold in its hands, instead we consider large-scale objects.

A wheelbarrow is an example of such an object used in loco-manipulation by workers in building sites. Yet the wheelbarrow does not involve contact changes during the locomotion... Instead we consider a rolling object that will involve such complex and frequent contact transitions. Large bobbines found in many industries to store dierent materials such as cables, wires, papers... are generally large (the one we use in this thesis has 1.330 meters diameter and weighs 140 kg). To the best of our knowledge, this is the rst time that a such objects is hard to track by the camera embedded on the robot. Therefore, we developed a dedicated visual estimator for this category of objects. Our goal is not to reinvent the wheel in all the technological ingredients needed to realize this skill. Instead, when this is possible, we rely on well-established technologies and see how they can possibly be enhanced or tailored and integrated to our planning and control strategies. The humanoid robot shall be able to start from any place in the factory environment and localize itself robustly and continuously.

For example, the robot can go to take a wheelbarrow that is not well located nor tagged and bring it to an operator or to another place. In another example, the humanoid robot could move a bobbin to free workers from such a none-added value task. As in [START_REF] Kheddar | Humanoid robots in aircraft manufacturing: The airbus use cases[END_REF], we aim to bring the latest research in dierent disciplines together to successfully bring the humanoid robot to perform autonomous tasks.

Our thesis report highlighting our contributions are organized as follows:

Chapter 2, extends the QP controller to multi-robots at the heart of our recent mc_rtc framework 1 implementation and to achieve force tracking. The term multi-robot is inclusive as it does not mean a set of what we call generally a robot, but includes any entities a robot interacts with: ranging from a solid non-actuated object to complex articulated system that can be fully, partially or not actuated, and xed or oating-base. As an example, a printer can be modeled as multi-body body systems with usual joints and hence can be seen as a fully passive (not actuated) robot. Another example is a humanoid robot; it can be seen as a single robot or as sub-robot systems (arms, legs, trunk) that are connected by bilateral joints. A wheelbarrow is also a robot where the handle and the containers are a rigid body and the wheel another one connected with a rotational joint. Due to this principle, we need a model for each of them; allowing the controller to compute the state of the overall system and their contact forces. When these systems interact through contacts, we show that a task-space formulation allows coherent force planning and tracking at a time. We assess our approach with a set of experiments on dierent robotic platforms performing dierent sets of actions with dierent objects (which are considered as robots within our multi-robot QP controller). [START_REF] Bouyarmane | Quadratic programming for multirobot and task-space force control[END_REF].

In chapter 3, we enhance our multi-robot QP controller developed in chapter 2 with an additional perception capability using a RGB camera. In particular, we focus on articulatedobject considered as robots with passive joint(s) such as printers with a paper drawer (prismatic joint) or cabinet with a door (rotational joint). Indeed, if we consider any object [START_REF] Chappellet | Benchmarking Cameras for OpenVSLAM Indoors[END_REF].

Finally, in chapter 5 we showcase the use of a humanoid robot in an indoor industrial context to manipulate heavy/large-scale objects while walking. To estimate these largescale objects, we enhance our multi-robot QP controller with additional features. This time with a fast dense 3D model-based estimator using wide-angle depth image a new modality that we used within our new visual estimator. It is used in conjunction with an existing vSLAM (RTAB-Map) both included in a whole framework with our multi-robot QP controller where the robot achieves complex loco-manipulation autonomously and safely.

We assess our result with two humanoid robots, HRP-5P and HRP-2Kai, manipulating a bobbin with the objective to bring it inside a specic structure. This chapter is based on an unpublished work yet.

Chapter 2

Quadratic programming for multi-robot and task-space force control

Task-space sensory control in [START_REF] Samson | Robot control the task function approach[END_REF] has reached a considerable level of maturity and has diverse implementations in kinematics and inverse dynamics such as [START_REF] Lee | A whole-body control framework based on the operational space formulation under inequality constraints via task-oriented optimization[END_REF][START_REF] Mansard | A unied approach to integrate unilateral constraints in the stack of tasks[END_REF][START_REF] Sentis | Compliant control of multi-contact and center-of-mass behaviors in humanoid robots[END_REF].

It has been ported in a large variety of robots, especially redundant ones in [START_REF] Nakamura | Taskpriority based redundancy control of robot manipulators[END_REF][START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF], achieving multi-objective complex tasks under various constraints.

In a previous work of ( This idea was introduced and its applicability was demonstrated in graphic animation of avatars [START_REF] Vaillant | Multicharacter physical and behavioural interactions controller[END_REF], summarized in Section 2.3. In this chapter, we present its adaptation to robotics. The dierence over the material presented in [START_REF] Vaillant | Multicharacter physical and behavioural interactions controller[END_REF] is to add all missing components for implementation on real robots, namely contact sensing and force control, and to demonstrate its applicability in real-application, real-world, real-robots scenarios with thorough experimentations on various platforms. My contribution on this work has been the force control formalization and the realization of the dierent experiments.

This idea is nowadays largely adopted in robotics as (i) it is easy to implement we also provide the software implementation of the proposed framework in open-source 1 and (ii) it allows us to ease task specication to its simplest expression, i.e. at the level of interactions.

For example, when a robot has to open a fridge, our method does not ask to build specic geometric constraints as in [START_REF] Borghesan | Introducing geometric constraint expressions into robot constrained motion specication and control[END_REF] or virtual mechanisms in [START_REF] Bowyer | Active constraints/virtual xtures: a survey[END_REF][START_REF] Luc | Imposing motion constraints to a force reecting telerobot through real-time simulation of a virtual mechanism[END_REF][START_REF] Kheddar | Teleoperation based on the hidden robot concept[END_REF][START_REF] Kosuge | Tele-manipulation system based on task-oriented virtual tool[END_REF], nor to implement a specic planning or control strategy in [START_REF] Berenson | Task space regions: A framework for pose-constrained manipulation planning[END_REF][START_REF] Burget | Whole-body motion planning for manipulation of articulated objects[END_REF][START_REF] Dalibard | Manipulation of documented objects by a walking humanoid robot[END_REF][START_REF] Karayiannidis | An adaptive control approach for opening doors and drawers under uncertainties[END_REF][START_REF] Ott | Employing cartesian impedance control for the opening of a door: a case study in mobile manipulation[END_REF]. Instead, we model the fridge as a `robot' with as many degrees of freedom as possible. The user must design the fridge model (e.g. as a ROS urdf le) and our controller integrates it with that of the robot and considers interaction tasks as dened through areas of interaction (contacts). The core idea here is that the model already embeds the constraints (the kinematics and the dynamics ones) instead of explicitly dening them as in [START_REF] Bruyninckx | Specication of force-controlled actions in the task frame formalism a synthesis[END_REF][START_REF] Villani | Handbook of Robotics, chapter Force control[END_REF].

1 mc_rtc open-source framework available at https://jrl-umi3218.github.io/mc_rtc/ Integrating the kinematic model of the manipulated mechanism has been proposed in previous work of [START_REF] Berenson | Task space regions: A framework for pose-constrained manipulation planning[END_REF][START_REF] Burget | Whole-body motion planning for manipulation of articulated objects[END_REF]. However, they remain at the geometric level, and only the kinematics of the planned mechanisms are accounted for. The planned congurations and motions in these studies do not account for the dynamics and inertia of manipulated mechanism(s), although these will inuence robot balance through motion. In [START_REF] Karayiannidis | An adaptive control approach for opening doors and drawers under uncertainties[END_REF] the dynamics of the articulated mechanism is accounted for, yet they restrict the study to one-DoF mechanisms, and robot balance is not an issue (manipulator), we also refer the reader to the references therein for a review of previous door and drawer opening studies and their limitations. A Cartesian impedance method for the opening of a door is proposed in (104) for a mobile manipulator without balancing issues and with the door opening motion being designed for the specic task at hand.

Instead, we compute desired states that have coherent contact interaction forces. Many of the intended manipulation and co-manipulation applications rely on friction (manipulation of a free-oating box for example) and necessitates the generation of the right amount of normal and tangential forces. Therefore, it is important to master force control under a QP controller framework, even on position-controlled robots, which has not been previously proposed to our knowledge. Hence we propose it as another contribution in this work.

This chapter is organized as follows. Section 2.2 introduces the state of the art regarding QP control. Section 2.3 recalls the multi-robot QP formalism from [START_REF] Vaillant | Multicharacter physical and behavioural interactions controller[END_REF]. Section 2.4

introduces QP force control to track the manipulation forces resulting from multi-robot QP when applied on position-controlled robots. Finally, Section 2.5 presents experimentation results where our new controller is applied in very challenging scenarios involving Kawada's HRP-4 humanoid robot, Softbank's ROMEO arm and Shadow's dextrous hand.

QP control's background

QP control has been proposed in the robotics and computer graphics communities to solve the control problem of multi-body systems with oating bases subject to friction limitations.

The approach appeared particularly suited to humanoid robots and humanoid virtual characters that typically feature such properties. A QP is instantiated at every control/simulation time-step minimizing the error of multiple desired task accelerations under all physical and structural constraints of the robot, which have the characteristic of being linear in the optimization vector variable composed of the control torques, contact force coecients along the linearized friction cones, and joint accelerations. The multi-task problem is cast as a multi-objective optimization program that can be solved with a weighted-sum scalarization or a lexicographic ordering scheme, among other possible multi-objective optimization 2.2. QP control's background or multi-criteria decision making resolution techniques. Of the studies that opted for the weighted-sum scalarization, (1) is worth citing in the eld of computer animation as one of the rsts that proposed the method for tracking in physics simulation a motion capture data clip with a standing humanoid character in a multi-contact posture. In [START_REF] Collette | Dynamic balance control of humanoids for multiple grasps and non coplanar frictional contacts[END_REF] the approach is enhanced by accounting for bilateral grasp contact more complex balancing strategies for an animated model.

(45) combined the QP controller with higher-level nite state machine and used it for locomotion with cyclic feet contact switching, whereas (14) applied the approach for humanoid robot in acyclic multi-contact locomotion, applied later in DARPA Robotics Challenge-like scenarios in simulation in [START_REF] Bouyarmane | Exploring humanoid robots locomotion capabilities in virtual disaster response scenarios[END_REF] and to real robot HRP-2 climbing a vertical ladder in [START_REF] Vaillant | Multi-contact vertical ladder climbing by an HRP-2 humanoid[END_REF].

(118) deals with soft control to obtain a hierarchy between tasks to solve them simultaneously while taking in account the robot-environment contacts. In soft control, each task is pondered by a weight; thus it gives a solution which is a compromise as oppose to hard control where we ensure to at least achieve the highest ranked task. Those weights are usually dened manually by an operator. Extending their previous work with soft control, [START_REF] Salini | Synthesis of complex humanoid whole-body behavior: A focus on sequencing and tasks transitions[END_REF] proposed a strategy to compute the weights to ensure continuous transition between task activation/deactivation. However, an open problem remains: non of the proposed formulation works properly in closed-loop formulation. We do not aim to tackle this peculiar problem, but [START_REF] Djeha | Adaptivegains enforcing constraints in closed-loop qp control[END_REF] redesigned a class of constraints formulation to improve their behaviors when they reached their bounds.

There are many other studies that use the QP in other schemes such as force control distribution e.g. [START_REF] Herzog | Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics[END_REF][START_REF] Liu | Interactive virtual humans: A two-level prioritized control framework with wrench bounds[END_REF][START_REF] Righetti | Quadratic programming for inverse dynamics with optimal distribution of contact forces[END_REF]. [START_REF] Herzog | Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics[END_REF] proposes a simplication of the optimization problem to compute desired torque for humanoid's legs robot (14 DoFs) at 1kHz. This high frequency allowed them to remove the classical joint PD controller. In addition, they evaluated balance and tracking experiments on a real robot showing that such an approach is feasible. [START_REF] Liu | Interactive virtual humans: A two-level prioritized control framework with wrench bounds[END_REF] rstly computes the optimal task wrenches for a virtual human to achieve a desired motion.

Those are used in a second step to compute joint torques to control the said virtual human.

We aims to compute the desired torques and applied contacts wrench in one iteration for all robots considered in the optimization problem. [START_REF] Righetti | Quadratic programming for inverse dynamics with optimal distribution of contact forces[END_REF] used quadratic programming to solve the inverse dynamics while implicitely minimizing costs in the contact forces.

In the following, we extend our framework to multi-contact manipulation of articulated mechanisms and oating objects by humanoids, and to multi-robot collaboration (e.g. robotrobot co-manipulation).

Multi-robot QP formalism

In this section, we briefely recall the multi-robot QP formalism from [START_REF] Vaillant | Multicharacter physical and behavioural interactions controller[END_REF]. Let us consider a system of n `robots' which can be actual robots, free-oating rigid objects, or passive articulated mechanisms such as a door, a drawer, or a valve for example. A typical minimal manipulation system would consist of n = 2 `robots': the actual manipulating robot and the manipulated object or mechanism; a typical minimal collaboration system would consist of n = 3 `robots': the two collaborating robots and the collaboratively manipulated object; a dexterous hand with m ngers manipulating a rigid object would consist of n = m + 1

`robots', each nger and the object. We use the unied term `robot' here to refer to all these systems, since they are all instances of the general multibody model. Indeed, each of these systems i ∈ {1, . . . , n} can be modeled as a xed base or free-oating base kinematic tree structure for which the degrees of freedom (DoFs) q i obey the following equation of motion (EoM):

M i (q i )q i + N i (q i , qi ) = J T i f i + S i τ i . (2.1)
Equation (2.1) encompasses all types of robots and accounts for all underactuation possibilities (free-oating base for humanoids and for free-oating rigid objects, non-actuated joints of passive mechanisms) through the actuation-to-DoFs mapping matrix S i . Note that we use Newton-Euler-based algorithms for the derivation of (2.1) in our implementation [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF].

In this framework the parts of qi and qi corresponding to a free-oating link (i.e. the whole object in case of a free-oating rigid object or the base link of a humanoid) are abusive notations for V i and Vi respectively, where V i is the SE(3) velocity of the free-oating link.

The vector f i stacks all point contact forces applied on the surfaces of robot i. These contact by another robot j (e.g. the forces applied on the hands of a humanoid by a manipulated object). The latter forces come in pairs of action/reaction forces among the system of robots according to Newton's third law, and opposite forces applied by the robot ion the robot j appear inside vector f j . We thus decompose the forces f i as

f i = (f 0 i , f - i , -f + i ) such that f 0 i
stacks the forces applied by the xed environment on the robot i, f - i stacks the forces applied by the robots j < i on robot i, and f + i stacks forces applied by robot i on robots j > i. We then denote F 0 , F -, F + , respectively, the vectors stacking all the vectors f 0 i , f - i , f + i . Let K be the total number of forces in F -, i.e. such that F -∈ R 3K . By virtue of Newton's third law, there exists a permutation matrix Π ∈ R K×K such that

F + = (Π ⊗ I 3 )F -, (2.2)
where ⊗ denotes the Kronecker product, see Fig. 2.1. We denote Ψ = Π ⊗ I 3 (itself a permutation matrix). Let K i be the number of forces in f + i , i.e. such that f + i ∈ R 3K i . The permutation matrix Ψ is decomposed into selection matrix blocks Ψ i ∈ R 3K i ×3K in the form:

Ψ =    Ψ 1 . . . Ψ n    , (2.3)
such that for each i we can write f + i = Ψ i F -. Finally the equations of motions (2.1) take the form:

M i (q i )q i + N i (q i , qi ) = J T i,0 f 0 i + J T i,-f - i -J T i,+ Ψ i F -+ S i τ i , (2.4)
where J i,0 and J i,-and J i,+ are the matrices obtained by extracting from J i the columns corresponding to the positions of f 0 , f -, f + in f , respectively 1 . We stack together all the 1 we use the index notations 0, +, -in the superscript of vectors and subscript of matrices, to avoid conflict with the transpose notation of matrices.

2.3. Multi-robot QP formalism equations (2.4) with the following matrices and vectors q = (q 1 , . . . , q n ) , M(q) = blockdiag(M 1 (q 1 ), . . . , M n (q n )) ,

(2.7) J 0 (q) = blockdiag(J 1,0 (q 1 ), . . . , J n,0 (q n )) ,

(2.8) J + (q) = blockdiag(J 1,+ (q 1 ), . . . , J n,+ (q n )) , (2.9)

J -(q) = blockdiag(J 1,-(q 1 ), . . . , J n,-(q n )) , (2.10) S = blockdiag(S 1 , . . . , S n ) , (2.11) N(q, q) = N 1 (q 1 , q1 ) T • • • N n (q n , qn ) T T ,
(2.12) to get our synthetic Newton's third law-consistent EoM for the whole system of robots:

M(q)q + N(q, q) = J T 0 F 0 + J --Ψ T J + T F -+ Sτ . (2.13)
The kinematic constraint that expresses the coincidence of the contacts points corresponding to an action/reaction pair can be synthetically written using the matrix Ψ and the principle of virtual work as

J + q = ΨJ -q , (2.14)
which is equivalent to, given that a permutation matrix is orthogonal Ψ T Ψ = I 3K , J --Ψ T J + q = 0 .

(2.15) This latter form of the constraint is consistent with the fact that F -can be interpreted as the constraint's Lagrange multiplier in (2.13). This constraint has to be complemented with the xed environment contact kinematic constraint that writes

J 0 q = 0 , (2.16)
for which F 0 also appears as the corresponding Lagrange multiplier in (2.13).

Note that the proposed mathematical Lagrange multiplier interpretations of F -and F 0 do not oppose the fact that both F -and F 0 consist of physical contact forces (as they had been initially constructed earlier in the section by concatenation of point contact forces).

As a consequence of their physical nature, F -and F 0 are indeed the correct subjects of the Coulomb friction cone constraints F -∈ C -and F 0 ∈ C 0 (which would not have been 2.3. Multi-robot QP formalism necessarily a justied hypothesis if we had derived (2.13) directly using a Lagrangian approach on the whole system made from all of the robots). These friction cones are then approximated as polyhedral cones with generators stacked as columns of matrices denoted C -and C 0 respectively in [START_REF] Stewart | An implicit time-stepping scheme for rigid body dynamics with coulomb friction[END_REF]. The coecients of F -and F 0 along the generators are denoted λ -and λ 0 respectively, such that F -= C -λ -and F 0 = C 0 λ 0 . These coecients are constrained to be non-negative component-wise:

λ = (λ -, λ 0 ) ≥ 0 .
(2.17)

The constraints of the problem are completed with the appending of joint limits, velocity limits, torque limits, and velocity-damper-based collision avoidance constraints between any links l a and l b , all of the initial forms

q min ≤ q ≤ q max , (2.18)
qmin ≤ q ≤ qmax ,

τ min ≤ τ ≤ τ max , (2.19)
)

ḋist(l a , l b ) ≥ ξ dist(l a , l b ) -δ s δ i -δ s , (2.21)
where the parameters ξ, δ i , and δ s represent respectively the velocity damping coecient, the inuence distance between links below which the constraint starts to act, and the security distance between links that the constraint ensures will never be reached. The constraints (2.18), (2.19) and (2.21) are rewritten in terms of constraints on q as follows:

qmin - q ∆t q qmax - q ∆t , (2.22)
q min -q -q∆t 1 2 ∆t 2 q q max -q -q∆t 1 2 ∆t 2 , (2.23) d ist ≥ 1 ∆t -ξ dist -δ s δ i -δ s -ḋist .
(2.24)

These formulations allow us to write the control problem for the system of robots as a single QP:

min q,τ ,λ M k=1 w k ||g k -gd k ||
where g k denote the tasks (possibly multi-dimensional) and gd k the desired task accelerations that can for example take the following form:

gd k = gref k -P k e k -D k ėk , e k = g k -g ref k , (2.26)
with P k and D k denoting the task gain matrices designed such that 0

I -P k -D k is a
stable (Hurwitz) matrix, and where g ref k is a reference trajectory or a xed set-point of the task such as [START_REF] Bouyarmane | On weight-prioritized multitask control of humanoid robots[END_REF].

Once a contact state for the system of robots has been specied, the eectiveness of the formulation (2.25) lies in the fact that a task can be specied for any feature of any single robot or group of robots of the system in a uniform way. For illustration, it is sucient to specify a task in terms of position and orientation of a free-oating manipulated object; the control commands for the manipulating robot (or the co-manipulating robots) will automatically be induced from the contact constraints through (2.25), without the need of explicitly specifying any task for the manipulating end-eectors. Similarly, if it is a mechanism that is being manipulated, it is sucient to specify a task in terms of the conguration of the mechanism (opening angle of a door, rotation angle of a valve) rather than tasks for the manipulating end-eectors. As an further illustration of the expressiveness of (2.25), the balance of a biped robot manipulating an object with a non-negligible mass can be written in terms of a single task on the center of mass (CoM) of the whole system.

QP Force Control

The QP controller (Section 2.3) outputs accelerations q, forces coecients λ, and joint torques τ for the robots. We use it in our applications with position-controlled robots (Section 2.5), by double integrating the output q and feeding the resulting q to the lowlevel motor position controller. However, in view of the eective use of the multi-robot QP in interaction tasks (e.g. robots co-manipulations), it is necessary to ensure that the planned manipulation contact forces are adequately matched during the execution, even when the framework is applied on position-controlled robots. As demonstrated in Section 2.3, the formulation (2.25) does produce accelerations, and hence position commands, that are consistent with the QP-predicted contact forces λ at a given control time-step. However there are two issues with this prediction:

1. it is based on the QP-used models of the robots and the objects, implying that any discrepancy in these models would result in inexact predicted forces; 2.4. QP Force Control 2. it supposes that the robot is in a given contact state that was planned beforehand, without actually knowing whether the robot has eectively reached that contact state and whether the contact has been established. If not, the QP would still base its calculations on the assumption that the robot is in its planned contact state and will output contact forces that are in reality null, see Fig. Therefore, we need a method that feeds back the information from the force sensors and realizes the tracking of the predicted forces by the sensored ones. Such a tracking method should also be able to ensure that the actual contact states match the planned ones by making sure that any planned contact has eectively been established in the current contacts state.

EF Tra jec tory task and the sensed force is converted into velocity with diagonal matrix gain K I . This velocity is low-pass ltered into a reference velocity for the end-eector QP tracking task. The target force comes either from the force output of the QP (switch 2 up) or from a external user command (switch 2 down). That external user command can also alternatively be incorporated inside the QP (switch 1 closed) in order to inuence the force output of the QP when using the latter as a target force (i.e. with switch 2 up).

( -%&' ) + (̇-̇% &' ) + (̈-̈% &' ) - = 1 ( 34%5&3 -6&768% ) ̇% &' ∫ %&' ̈% &' QP EF Force task <= -> - > <= 6&76 8% > 34%5&3 ∑ B 1 Lowpass Butterworth Clamp ( CD7 , C4F ) (EF = End Effec tor) -+ Switch 1 Switch 2

QP Force Control

Force control has been extensively studied in robotics, see a thorough review in the monograhs in [START_REF] Gorinevsky | Force control of robotics systems[END_REF][START_REF] Siciliano | Robot Force Control[END_REF] and in the handbook of robotics of [START_REF] Villani | Handbook of Robotics, chapter Force control[END_REF]. Force control in the task space for xed-base robots was also developed and experimented in [START_REF] Bruyninckx | Specication of force-controlled actions in the task frame formalism a synthesis[END_REF]46,[START_REF] De Schutter | Constraint-based task specication and estimation for sensor-based robot systems in the presence of geometric uncertainty[END_REF]. Contrarily to [START_REF] Borghesan | Introducing geometric constraint expressions into robot constrained motion specication and control[END_REF][START_REF] Bruyninckx | Specication of force-controlled actions in the task frame formalism a synthesis[END_REF], task specication including force control is simplied and made straightforward with the QP built-in multi-robot constraints specication, since interaction forces are part of the QP decision variables. Active force control can be achieved either directly, through explicit closure on the force, or indirectly through compliance, impedance or admittance control as in [START_REF] Villani | Handbook of Robotics, chapter Force control[END_REF]. The multi-robot QP control framework allows us to have both, and also allows us to consider oating-base under-actuated or xed-base robots. As compared to exiting controllers, the added value of the QP control is not in a structural way or in how basic force control is made, but rather in the way force control is integrated into the whole multi-objective task space control, accounting for limitations in the force wrench explicitly and in full multi-unilateral-contact settings. For example, prohibiting sliding (unilateral contact) is made simply by adding built-in non-sliding constraint task; limiting the force or moment in any direction is easy. Yet, the real added-value with respect to existing force control frameworks, is the ability for the controller to suggest (i.e. plan) the references forces to be used in a given set of tasks and current state conguration and use that output to close the loop on such generated reference forces. For example, consider a multi-contact setting where we ask a humanoid robot to move one leg and its body in multi-unilateral-contacts using the other foot and one hand as supporting contacts. In order to shift the center of mass and moving the leg, we may let the QP decide what force to generate on the supporting hand under torque and non-sliding constraints and close the loop on such computed force wrench during the movement. We could also suggest a threshold force on the hand or a desired force behavior that must be met at best. If not, we would have to select by hand what the force trajectory is(for all components) that need to be generated. As we will see later, our controller can servo on a user-specied or otherwise planed desired force wrenches, or on its own computed force-wrenches (if no desired force wrench is specied) and nally, it can servo on both (i.e. on user-specied or planed desired force wrenches while driving the controller to generate a behavior and consequently contact forces as close as possible to the desired ones, but keeping physical and constraint consistencies).

To achieve the previously described force control behaviors in the multi-robot QP controller, we propose the scheme represented in Fig. 2.3. For a given end-eector (or more generally any link) of the robot equipped with a force/torque sensor/observer, we proceed with an admittance controller that takes as an input the error between a target force F target and the corresponding sensed force F sensor , and transforms it into a QP end-eector task through the following stages. First we convert the force error into a velocity command with a diagonal gain matrix K I (inverse of a damping):

Robot

v = K I (f target -f sensor ), (2.27)
then we clamp that value between (v min , v max ) to prevent the end-eector from moving too fast nearby the contact surface if not reached yet (i.e. if F sensor = 0, which happens when the end-eector is searching the surface it is supposed to be in contact with), this is a guarded motion with

v clamp = min (max (v min , v) , v max ) , (2.28)
to which we apply a low-pass lter (order 3 and cuto frequency 20Hz Butterworth in our implementations). The ltered signal ṽ is converted into a QP end-eector task by taking it as the reference velocity trajectory ( ġref = ṽ) and by deriving it from the reference position g ref and acceleration gref trajectories. The latter are then sent to the QP as an end-eector trajectory tracking task (here, similar to an impedance). Note that the contact zero-acceleration constraint is dropped for any link that is subject to the admittance task.

We retained three possible strategies to incorporate the admittance scheme in our framework, depending on the states of the switches 1 and 2 that appear in Fig. 2 The conguration as it appears in the case of Fig. 2.3, i.e. with switch 1 open and switch 2 up, implements an autonomous behavior where the controller tracks the force output by the QP as the QP gures it out from the other tasks of the problem. However, the user might want to have some control on interaction forces, that might not turn out to be satisfactory for them (typically, in the applications and experiments of Section 2.5, we considered that the forces output by the QP on the hands of the HRP-4 robot can be too important given the relative fragility of the hands, regardless of the nominal manufacturer's torque limits, so we wanted to produce less force on the hand). Hence we oer the user the possibility to specify a desired force F d that can be used in two ways. The simplest one is with switch 1 open and switch 2 down, this allows the sensor force to track F d independently of the other physical constraints of the robot. This is not a safe strategy as the user might specify unrealistic forces F d given the current conguration and contact state of the robot, it can be used as a last resort only if the user is sure that the specied F d is safe/consistent. The other way to use F d is through the QP, with switch 1 closed and switch 2 up by adding the term ||f -f d || 2 to the cost function of the QP, this we call a QP force task. This ensures that the user-specied force F d is ltered through physical constraints that are taken into account in the QP and produces an F target that is as close as possible to F d while remaining physically consistent.

First, we conducted preliminary QP force tasks experiments with a humanoid HRP-4 equipped with built-in 6-DoF force/torque sensors on each foot; in addition, our robot has customized 6-DoF force/torque sensor on each hand (Nano 40 from ATI). In these experiments (that can be made with any robot) the reference force is put to zero F d = 0 for each limb and in all directions. The goal is to determine (i) the range of gains matrices K I and (ii) the clamping speeds and direction for each controlled limb.

Each value of K I correlates to the response time of the desired force/moment. To not account for dynamic equilibrium constraints, we hung the robot in the air, xed the waist, Each hand is controlled with a dierent paradigm.

Once the gain ranges are determined, the robot is put in four contacts conguration, the center of mass is shifted to lie within the center of the right foot. Fig. 2.6 shows the experimental setup that was used to assess the QP force control. It is put near a table in a half-sitting initial posture (i.e. the knees are slightly bent). A posture is computed so that both hands are just hovering above the table while the feet are rmly on the ground. The force control goal is specied for each hand and the left foot independently, but simultaneously. The right foot is kept as a supporting non-controlled contact for the coherency of the force control. As we only have unilateral contacts, we must insure that the forces are not controlled in a non-consistant way. Obviously, we cannot control a force F i of a given contact C i and, at the same time-control, a force in another contact C j =i that lies in the opposite direction of F i = -f j =i , serving as a reaction. Another example, with only feet as the contact, controlling the force along the z-axis (normal to the ground aligned with the gravity eld vector) of both feet at the same time will result in oscillatory behaviors and none convergence up to instability; this is because the CoM shift that results in controlling the wrench of one foot, will have a direct consequence on the other, based on the action-reaction principle. This problem is not encountered in force control of xed-based robots. This is the reason why accounting for the QP computed forces and encompassing non-sliding and other bound forces in the multi-robot QP allows us to keep the concistency of the force control in multi-unilateral-contacts in most cases. In the rst method, the robot follows the user command but reaches torque limits (over-torque errors appeared on the robot during both executions). The second method (right hand), the QP autonomously saturates the exaggerated force command to keep the robot within the torque limit constraint. The second method is thus the safest for the robot.

Experimentations

We previously mentioned the friction issue in unilateral contacts. In our experiments, we could easily identify the friction coecients of the feet and hands in all directions using incremental reference translational forces. When we provide the QP with desired forces that lie outside the identied friction cone for a given contact, the latter slides and this is noticeable when we plot the measured forces: the desired forces cannot be reached. Torsional frictions are more dicult to identify. We could experimentally validate in part the observations in [START_REF] Caron | Stability of surface contacts for humanoid robots: closed-form formulae of the contact wrench for rectangular support areas[END_REF]: the torsional friction and bounds on rotational sliding depend not only on the normal force and the Coulomb friction coecient, but also on the distance between the center of pressure and its distance to the closest vertex of the contact surface (that we cannot estimate in practice). For instance, we found a torsional friction of the left leg (around z, the vertical axis aligned with gravity and the contact normal to ground) to be 0.5; but this z-axis torsional friction decreased to 0.4 and up to 0.3 when we asked the feet to also be controlled with reference moments on the xor y-axes.

All in all, this work allows non-sliding constraints and bounds on forces to be integrated as part of the whole-body controller explicitly. An extension of this work made by [START_REF] Saeid Samadi | Humanoid Control Under Interchangeable Fixed and Sliding Unilateral Contacts[END_REF] allows to tackle the case of sliding unilateral contacts. After experiments with the reference force wrenches in force tasks, i.e. without using the QP computed forces, the plots in Figs. 2.8 and 2.9 illustrate a comparative study of all combinations that can be achieved with our controller. The transient phase to converge to the desired command is due to the fact that the same unied control is used both for searching for the contact and for regulating the magnitude of the contact force once contact has been established. As for the previous canonical experiments, the speed of convergence can be mainly tuned with the admittance gain matrix K I but also to some extent with task admittance gains P and D, Fig. 2.3. The later also correlate to the time response as for any other tasks of the QP. Fig. 2.9 illustrates how the regulation scheme does not track forces when they are not consistant with the constraints and automatically compute the closest desired force wrench to the user-dened one. The transient phase it takes several seconds for the hands to reach the table even by starting close to it to search the contact is improved by an extension of this work presented in by [START_REF] Wang | Impact-aware humanoid robot motion generation with a quadratic optimization controller[END_REF]. Thanks to it, we are now able to establish a contact with non quasi near-zero velocity with an impact while keeping robot's Zero Moment Point (ZMP) in its support zone. Once the contact is done we regulate the forces applied as presented in this chapter.

Experimentations

We experimented with the multi-robot QP controller on various challenging scenarios. The scenarios use three dierent robots: Kawada HRP-4 humanoid robot, SoftBank Robotics the control was performed in real-time, as the multi-robot QP was consistently solved in times below 5ms per iteration. Once the dynamic parameters of the box are identied, the contact forces during manipulation can be computed from and controlled by the MQP to achieve the same trajectory of the box with less internal forces instead of using the user-dened contact forces. In these experiments, we illustrate the capabilities of the controller to manipulate every-daylife objects with articulated mechanisms. Two manipulation scenarios were used in these experiments: door opening and printer tray opening. The robot is the HRP-4 humanoid that is provided with the urdf models of the objects. The door is a regular (not self-closing)

Box manipulation experiment

Manipulation of articulated mechanisms

door of the laboratory room intended for everyday use. It is modeled in the multi-robot QP as a `robot' with a two-DoF xed-base mechanism. One DoF is the passive revolute joint at the hinges of the door; the second one is a spring-loaded revolute joint at the handle of the door. The printer is a commercial printer (model Canon i-sensys LPB7680Cx). It is also modeled in the multi-robot QP as `robot' with a xed-base mechanism (although it could have been more accurately modeled as a oating-base mechanism in unilateral contact with a support table as in ( 106)) with one passive prismatic joint for the tray (the model can also include the other non-used trays and also all the buttons as prismatic joints).

In the printer experiment, the user provides a desired force F d = 10 N along the local z-axis on the left hand in order to prevent its slippage (to compensate for the modeling approximation that we make consisting in dening a planar surface on the hand of HRP-4 which is not perfectly planar) and a desired force F d = 5 N along the local z-axis on the right hand to rmly insert it inside the tray handle prior to the tray pulling motion, and F d = 0 N for the remaining axes, see Fig. 2.12. Both force commands are sent using the switch 1 closed, switch 2 up combination of the controller in Fig. 2.3. Putting the left hand on the printer is suggested by the planner in (15) to create a closed kinematic chain so as to pull the tray without causing equilibrium or force application problems. The Fig. 2.13 illustrates snapshots from the door opening experiment with the same controller. Another experiment is a door opening using a position control scheme of the handle, with another robot posture where the door is opened with the left arm, pushed with the right one and nally crossed using a walking controller (3). This is an example of sequencing the multi-robot QP controller with other controllers such as a walking controller in this case.

2.5.3

Robot-robot co-manipulation The multi-robot QP controller computations were completed on an external computer and sent to both robots using dedicated communication architecture. We plan in the future to embark the multi-robot QP control computations on one of the robots and use the other robot's computational resources for auxiliary tasks such as vision for example. Figure 2.17: The green spots are the predened contact areas.

Experimentations

We chose an illustrative manipulation problem where the manipulated object, a click pen, is again an articulated mechanism, but as opposed to the door and printer this time it is a free-oating base mechanism. The cardboard boxes in the previous experiments were also free oating but without articulations. Hence with this last example we cover all typologies of manipulated objects. The clicking articulation is modeled with a spring-loaded prismatic joint in the multi-robot QP. The contact surfaces on the ngertips and the cylindrical body of the pen were faceted (approximated with planar surface patches). The task in this example was specied on the conguration of the pen `robot' such that the position of the clicking part reaches its joint limit to trigger the exit of the writing tip.

See Figs. 2.16 and 2.17.

Practical considerations

All the experiments presented are performed with a consistent methodology: we use the MQP control framework without customization for all the experiments and we use the same desired force specication tasks and coding template with the switches in Fig. 2.3 for each robot/contact. The dierences lie in the urdf of the robots in play and eventually the tasks (CoM, posture, force, etc.) for each scenario. For the previous experiments, Table 2.2 provides a list of the tasks that have been used (all coded as templates).

Since objects in the environment that are interacting with robots for given tasks are considered as `robots', an increasing number of objects come with an increase in the computational cost. This is partly true since:

1. we do not systematically include all robots or objects considered as such, of a scene in a single MQP. Instead, we dene clusters of MQPs on the basis of eective interactions between the robots/objects of interest; each of which is computed separately. Given the application and the context, we may consider the MQP computation as being remote, cloud computed or distributed;

2. the MQP is sparse; therefore, we can benet from any QP solver that exploits the sparsity of the problem; this is being currently investigated and since our framework is independent from the QP solver per se, any ecient sparse QP solver can be used with little development eorts.

The box manipulation experiment highlights the benets in having an estimation of the parameters such as inertia and the pose of objects that are considered as `robots', whereas they are not. For the case of the box experiment or any well structured articulated rigid objects, it is possible to add the inertia parameters identication in the task space (as they 2.5. Experimentations can write as a quadratic program [START_REF] Jovic | Humanoid and human inertia parameter identication using hierarchical optimization[END_REF]) that allow more precise contact force computation and control during the manipulation when this is possible. As for the pose and conguration, such objects do not have encoders and hence estimating their conguration can be made using the robot embedded camera as we did in [START_REF] Paolillo | Interlinked Visual Tracking and Robotic Manipulation of Articulated Objects[END_REF]. If such two identication features can be gathered, then we may consider extending our MQP framework to deal with human-robot cooperative tasks. Although our control framework is able to identify a large number of non-consistent multi-contact force control, there is no guarantee, as for now, that we handle all of them properly. For example, if we ask a humanoid robot to achieve a desired same F d force on both foot in the same x-axis direction (front), the robot will bend behind up to falling in trying to fulll such force references if there are no tasks to constraint the center-of-mass.

Moreover, we need through an explicit formalism to detect in a multi-unilateral-contacts setting if we are asking in specic directions for force control of both the action and its 2.6. Conclusion reaction. Automating the detection of such user-specied inconsistencies is an interesting issue to investigate furthur.

Conclusion

We have shown the benet of integrating task-space QP controllers as a single problem to handle multi-robot interactions. By multi-robot we mean that the controller can deal with any cluster of objects or robots or mechanisms that are passive, partially passive or totally capable of interaction. Not only does such an approach ease the specication of the tasks (as a complement to planning) to its simplest expression (i.e. the interaction level), but it also computes physically consistent contact interaction forces. Subsequently, we devised force control algorithms for QP controllers and show that we can achieve reliable closed-loop force control where the QP can track at best the desired forces (and does its best when they are not QP controllers are currently emerging as a gold standard to handle multi-objective tasks in redundant robots such as humanoids [START_REF] Fahmi | Passive whole-body control for quadruped robots: Experimental validation over challenging terrain[END_REF][START_REF] Kim | Computationally-robust and ecient prioritized whole-body controller with contact constraints[END_REF][START_REF] Xin | An optimization-based locomotion controller for quadruped robots leveraging cartesian impedance control[END_REF]. We have proven that they are capable of controlling position, torque [START_REF] Cisneros | Robust humanoid control using a qp solver with integral gains[END_REF] and now multi-robots and force. Investigations in terms of stability have been conducted in [START_REF] Bouyarmane | On weight-prioritized multitask control of humanoid robots[END_REF]. Singularity problems are also considered in [START_REF] Pfeiffer | Singularity resolution in equality and inequality constrained hierarchical task-space control by adaptive nonlinear least squares[END_REF]. We are also conducting promising research using QP control as an adaptive controller [START_REF] Bolotnikova | Adaptive task-space force control for humanoid-to-human assistance[END_REF][START_REF] Samy | QPbased adaptive-gains compliance control in humanoid falls[END_REF].

Additional issues appear to be important for further investigations. For example, what is the granularity of the objects that need to be considered and integrated as `robots' when we consider tasks such as gathering small pieces in a box (more linked to perception); how to extend the MQP framework to deal with deformable objects; and nally, how the formalism can be extended to other types of robots such as complex wheeled robots, ying and sea robots, and cable robots.

The next chapter is dedicated to extend our MQP controller with visual servoing [START_REF] Paolillo | Interlinked Visual Tracking and Robotic Manipulation of Articulated Objects[END_REF] which makes it a multi-modal controller as vision, force, impedance/admittance, and position tasks can be specied together [START_REF] Bolotnikova | A circuit-breaker use-case operated by a humanoid in aircraft manufacturing[END_REF]. [START_REF] Paolillo | Interlinked Visual Tracking and Robotic Manipulation of Articulated Objects[END_REF]. My contribution on this work has been the guided hough transform, the handling of obstructions, the integration with our MQP controller framework and the realization of the dierent experiments.

Introduction

In the perspective of having domestic or oce robots manipulating and interacting with articulated human-tailored objects, such as households appliances, they shall be embedded with perception capabilities allowing to monitor the state of such objects, whose models are usually available. For example, to manipulate the drawer of a dresser (see Fig. In doing so, the estimation and the manipulation processes have to be jointly integrated with the whole body control of the robot, to be consistent with other motion constraints such as (auto)collision avoidance, joint torque and state limits, etc.

The motivation of this chapter is to close the loop on a multi-objective task-space controller formulated as a quadratic program (QP) that models the manipulated object as an augmentation of the robot structure as seen in the previous chapter and in [START_REF] Bouyarmane | Quadratic programming for multirobot and task-space force control[END_REF][START_REF] Vaillant | Multicharacter physical and behavioural interactions controller[END_REF]. Indeed, robot(s) and manipulated objects are integrated in our multi-robot system that is controlled with a single QP when they come to interact. Subsequently, the decision variables of classical QP frameworks controlling a single robot that are the generalized joint acceleration and contact forces, are augmented by those of the manipulated (articulated) objects resulting in a multi-robot QP (MQP) control framework. The MQP requires the current state value of the overall multi-robot system to close the loop and update the models used in the tasks and constraints. Unless they are other robots, the manipulated articulated objects are not equipped with sensors measuring their conguration. Therefore, since the robot can be embedded with a camera, we discuss in Section 3.2 considered existing articulated objects tracking methods that reveal limitations. We devised a conguration estimator as a virtual visual servoing in Sect.

Articulated Ob ject Tracking's background

In order to be integrated into a robotic feedback control loop, a tracking algorithm has rely on standard robot computational and sensory equipment and to be free from the following drawbacks:

(i) computationally demanding (ii) not validated against ground-truth measurements (only superposed skeleton on image rendering)

(iii) can experience convergence issues (iv) some have jumps or jerkiness in frame-to-frame estimation (v) some assume xed cameras (vi) not be tailored for a specic item

The last drawback (vi) not be tailored for a specic item is particulary true for one type of articulated object: the hands. It is a special case of articulated object which is widely cover as in [START_REF] Vysocký | Analysis of precision and stability of hand tracking with leap motion sensor[END_REF] where they evaluated the quality tracking of Leap Motion Sensor, in [START_REF] Park | Ecient 3D hand tracking in articulation subspaces for the manipulation of virtual objects[END_REF] where they relied on a 3D model of the hand and (32) review the use of machine learning technique in this eld. However, we do not aime to track hands as articulated object but object that could be manipulated by a robot. Some methods require joint encoder a measured of the angle on the estimated object such as in [START_REF] Garcia Cifuentes | Probabilistic articulated real-time tracking for robot manipulation[END_REF][START_REF] Michael Krainin | Manipulator and object tracking for in-hand 3D object modeling[END_REF][START_REF] Schmidt | Depth-based tracking with physical constraints for robot manipulation[END_REF][START_REF] Schmidt | Dart: Dense articulated real-time tracking with consumer depth cameras[END_REF] in addition of the depth image input; we do not have such sensor on the articulated object that we target. In [START_REF] Sturm | A probabilistic framework for learning kinematic models of articulated objects[END_REF] authors attempted to learn the model of the manipulated object with a prior learning requiring markers; we do not aim to fulll this point as we assume to already possess the manipulated articulated object model for our MQP presented in the last chapter also we want to perform markerless articulated object tracking.

RGB-D images are used to estimate pose, shape and structure of an object in [START_REF] Martín Martín | An integrated approach to visual perception of articulated objects[END_REF]. In [START_REF] Klingensmith | Closed-loop servoing using real-time markerless arm tracking[END_REF] a depth-based method, robust to calibration errors, estimates the state of a robotic arm in closed-loop manipulation tasks. However, for many manipulation tasks, the object-camera distance may not exceed the minimum range required by some depth sensors. Moreover, depth sensors using infrared do not work outdoor and those using LIDAR are computationally expensive. Stereo-vision, instead, needs additional steps to solve the reconstruction problem. This motivates us to use only monocular images.

Model-based approaches using single cameras are suitable to achieve our goal. The object geometric and appearance model are used in [START_REF] Nickels | Model-based tracking of complex articulated objects[END_REF] to estimate the joints position and velocity, 3.3. Formalism but not that of the oating base. In (49), the tracking of complex structures is handled imposing motion constraints to rigid objects tracking. Another interesting articulated object tracker has been proposed in [START_REF] Comport | Kinematic sets for real-time robust articulated object tracking[END_REF]; it uses the virtual visual servoing (VVS) paradigm [START_REF] Comport | Real-time markerless tracking for augmented reality: the virtual visual servoing framework[END_REF] and has real-time estimation capabilities [START_REF] Gratal | Virtual visual servoing for real-time robot pose estimation[END_REF]. Therefore, we took inspiration from (39) but our solution has a lighter formulation, is easier to implement, and estimates directly the object conguration. Moreover, we formulate the problem as a QP to be merged with QPbased control frameworks. In a previous work [START_REF] Paolillo | Visual estimation of articulated objects conguration during manipulation with a humanoid[END_REF], we used points as visual features and formulated the problem as a classic VVS; structuring the object with known markers was required and that is no more the case in the presented chapter.

Summing-up, the main features of our framework are:

• formulation of the articulated objects tracking as a QP;

• a perception algorithm providing the feedback based on an image processing procedure enhanced with the tracking information; relies on line features, easily detectable at the object edges, are considered as feedback in the tracking;

• experiments assessing the eectiveness of our approach and the interlink between the estimation and the control.

Formalism

We address the articulated objects estimation problem with the VVS paradigm. Visual servoing [START_REF] Chaumette | Visual Servo Control, Part I: Basic Approaches[END_REF] provides the camera velocity v c to achieve a cartesian task, zeroing the error between measured (s) and desired (s * ) visual features. Similarly, VVS computes the velocity of a virtual camera whose unknown pose p in the cartesian space is dened in correspondence of some virtual visual features s(p). Those features are reconstructed by using the scene and the camera projection model. The real camera pose p * is dened in correspondence of s * ; the same virtual visual features as measured on the image plane. The convergence of s(p)

to s * implies the convergence of p to p * . Thus, the VVS can estimate the camera pose integrating v c . (40) introduced this technique was as an augmented reality tool to estimate the pose of an object moving in the camera frame, and extended to articulated objects in [START_REF] Comport | Kinematic sets for real-time robust articulated object tracking[END_REF]. This technique introduced [START_REF] Comport | Real-time markerless tracking for augmented reality: the virtual visual servoing framework[END_REF] as an augmented reality tool estimate the pose of an object moving in the camera frame, and is extended to articulated objects in (39).

image plane Fig. 3.1 shows a robot looking at the drawer of a dresser that needs to be opened to a given amount. The robot camera frame F c is the reference having the focal point as origin with the z-axis aligned with the focal one. The image plane is dened by the u-v coordinates system, where the abscissa axis is oriented as x c and the ordinate as y c ; F o is a given oating base frame of the object. The real conguration of the articulated object is

s * i θ * i ρ * i u v features of interest F c F o ℓ i zc xc yc
q * = p * o T ϕ * o T q * j T T ∈ R m , ( 3
u cos θ i + v sin θ i = ρ i (3.2)
where ρ i is the length of the segment perpendicular to the i-th line and joining the origin of the image plane, and θ i is the angle between this segment and the abscissa axis (see Fig. 3.1).

The motion of the real visual features is induced from the articulated object-camera relative motion. Their parameters θ * i and ρ * i (i = 1, . . . , f ) are stacked in s * ∈ R 2f :

s * =        θ 0 * ρ 0 * . . . θ f * ρ f *        (3.3)
The conguration q of the virtual articulated object, aects the motion of the virtual visual features θ i (q) and ρ i (q), collected in s(q) ∈ R 2f :

s(q) =        θ 0 (q) ρ 0 (q) . . . θ f (q) ρ f (q)        (3.4)
They depend on: (i) the current estimation of q, used to update the geometric model and calculate the lines in F c and, (ii) the camera intrinsic parameters, used to project the lines on the image plane. Since F c is the reference, the camera extrinsic parameters are not needed for the projection. The tracking algorithm estimates q * given the object geometric model, containing N ordered edges m ( ⊆ m ) and vertices p m expressed in F o . The model is used to build s(q) while s * is measured on the image.

Perception of the Visual Features

In this section, we explain the methods used to measure real visual features s * and reconstruct virtual visual features s(q) needed to estimate the articulated object conguration. The whole algorithm is based on the so-called edges_table that contains the following data for each of the N edges of the object model:

• id: identication number

• part_id: identication number of its object link

• tracked: boolean indicating if tracked

• visible: boolean indicating if visiblity on the image

• θ and ρ: virtual visual features line parameters on the image plane

• θ and θ: upper and lower bounds of its θ use with real visual features

• ρ and ρ: upper and lower bounds of its ρ use real visual features

• p 1 and p 2 : extreme points of the segment on the image plan; used to dene a region of interest (ROI) where to search for it on the image plane

• l: length

The id and part_id elds are known in advance and comes from the object geometric model.

All the other data are updated at each new acquired image by the perception algorithm, as described below. We only use the geometric information (θ, ρ, p 1 , p 2 , l) for the tracked edges, i.e., ∀ id ∈ T = {1, . . . , N | tracked id = true }.

Initialization

To make the algorithm start, we initialized the edges_table and the articulated object conguration vector q. A common way is to perform the initialization step manually; either by specifying a good initial guess of the conguration, or either by providing a good initial guess of the real visual features. For simplicity, we chose the second option it only requires an operator to click on the image; hence none of the camera and articulated object should move in the meantime. We chose a set of f edges to track; tracked value is set to true and for the others to false in the edges_table. We pointed on the image the f chosen edges one by one by clicking on their extreme points; it denes p 1 and p 2 from which we compute (θ, ρ) parameters and ll the edges_table (see Fig. 3.2). The upper and lower bounds are initialized as:

θ i = θ i -m θ θ i = θ i + m θ ρ i = ρ i -m ρ ρ i = ρ i + m ρ (3.5)
∀ i ∈ (i = 1, . . . , f ); m θ and m ρ are two heuristic margins, used to be conservative with the lines searching operations. Those heuristic margins have to be chosen carefully for the real visual features; low margins might result in loose of then in fast motion, and large margins might slow down their computation. Lastly, to have a guess of the oating base, we solve a perspective-n-point problem by selecting 4 points on the image of the object, refering to vertices of the object in F o as dened in the model. After those steps, we have initialized our edges_table and the articulated object oating base.

Tracking of the Real Line Features

We processed each new image I to construct the real visual features vector s * of f elements; the i-th element of s * and s(q) refers to the same edge in the edges_table. We estimated the line's parameters (θ * , ρ * ) associated to an edge in the image plan with our Hough-guided 3.4. Perception of the Visual Features procedure.

We designed the Hough-guided procedure as follow. We used the Canny edge detector on image I to obtain a black and white image; a white pixel belongs to an edge. For each tracked line i , we compute a region of interest accordingly to p 1 and p 2 and two margins m ROI,x and m ROI,y . We applied the Hough transform only in the region of interest; it gave us k estimations of segments; it is dened as two points p est 1 and p est 2 . From this k segments, we computed k lines representated as (θ est , ρ est ). We kept the one ranging from (θ, ρ) to ( θ, ρ) leaving us j estimations and discard the rest. Thus, we estimate (θ i * , ρ i * ) as:

θ i * = 1 j j h=0 θ h est ρ i * = 1 j j h=0 ρ h est (3.6)
the maximum and minimum points p est However, if the detection process fails, the tracked entry for that line is set to false.

In this case, a recovery strategy is activated to nd a substitute among the lines in the table with a corresponding part_id, and whose visible value is true. As soon as we nd a line that is detected, with the Hough-guided procedure, it is considered in the tracking instead of the one we lost. Otherwise, the last detected edge is kept in the table. The recovery strategy also helps the tracker with temporary failures of the detection, trying to regain the convergence to the real object from the current status of the edges_table.

We attempted to use the litterature moving-edge algorithm designed to track the lines at the borders of an object as in ( 39) and ( 40); we encountered problems with our articulated object as shown in Fig. 3.4. The tracked edge on the oating base remained attached to the articulated body; making this estimation unsuitable for our needs.

Reconstruction of the Virtual Line Features

The reconstruction of the virtual lines consists in lling the virtual visual features s(q) given the current value of the object conguration q where a virtual visual feature (θ i , ρ i ) correspond to a real visual feature (θ i * , ρ i * ). We obtained s(q) by executing three steps. We used the current estimate of the articulated object q to update the model lines m in F c . We selected f features among those model lines m to be considered for the VVS; they corresponded to the ones with tracked eld at true in the edges_table the correspondence is done at this stage, both i-th element refer to the same id. Finally, we projected the model lines on the image plane; it gives us the virtual visual feature vector s(q). During this process, we updated visible, p 1 and p 2 in the edges_table for all edges. The visibility of an edge is estimated based on the others edges vertices z component and object's faces. We also updated the (θ, ρ) bounds as:

xid = max (x id , x id + ẋid T s ) + m x x id = min (x id , x id + ẋid T s ) -m x (3.7)
∀ id ∈ T and x ∈ {θ, ρ}. In the above relationships, the derivative term can be computed numerically or, if the tracker converges, using the information available in the VVS, i.e., ( θid , ρid ) T ≈ A i q.

Visual Tracking of Articulated Ob jects

In this section we describe our approach. Firstly, we recall the classical VVS and then propose the QP-based extension.

Classic VVS-based tracking scheme

The VVS error is the dierence between virtual and real features, e = s(q)-s * . Each virtual feature i motion obeys ṡi (q) = -L i J i (q) q = A i q

(3.8)
where the dot over the variables denotes the time derivative, L i is the 2 × 6 image Jacobian associated to the i-th line [START_REF] Sa | Inspection of pole-like structures using a visual-inertial aided VTOL platform with shared autonomy[END_REF]:

L i = λ θ i c θ i λ θ i c θ i -λ θ i ρ -ρc θ -ρs θ i -1 λ ρ i c θ i λ ρ i s θ i -λ ρ i ρ i s θ i (1 + ρ i 2 ) -c θ i (1 + ρ 2 i ) 0 (3.9)
with s x being sin(x) and c x being cos(x),

λ θ i = (a i s θ i -b i c θ i )/d i and λ ρ i = (a i ρ i s θ i + b i ρ i c θ i + c i )/d i . The scalars a i , b i , c i , d i
ω o = 2E T σo = 2E T J ϕ φo .
(3.12)

E is derived from the quaternion propagation rule:

E = -ε T ηI 3 -S(ε) (3.13)
where I 3 is the 3 × 3 identity matrix and S(ε) the skew symmetric matrix associated to ε. In (3.12), J ϕ is the Jacobian of σ o w.r.t. ϕ o , given in [START_REF] Sebastian | Practical parameterization of rotations using the exponential map[END_REF] and here reported element-wise:

J 1,j ϕ = ∂η ∂ϕ j = -ϕ j sᾱ 2α , (3.16)
J i+1,j ϕ

= ∂ε i ∂ϕ j = ϕ 2 j ( cᾱ 2α 2 -sᾱ α 3 ) + sᾱ α if i = j, ϕ i ϕ j cᾱ 2α 2 -sᾱ α 3 if i = j, (3.17)
From (3.12) we obtain J o as:

J o = I 3 O 3 O 3 2E T J ϕ (3.18)
where O 3 is the 3 × 3 zero matrix. We also need the Jacobian of the i-th link expressed in F c :

J i = R o S(p o )R o O 3 R o J o i = V c o J o i (3.19)
where J o i is the Jacobian of the link in F o and V c o the camera-object velocity twist transfor- mation. Thus, J i is composed as follows:

J i = (J o O 6×n ) if i ∈ oating base, J o J l O 6×(n-l) if i ∈ l-th link, (3.20)
Then, the dynamics of the error writes as ė = A qṡ * , where A = (A 1 , . . . , A f ) T is the 2f × m articulation matrix relating the oating base and joints velocity to that of the visual features. Imposing a stable dynamics of the error, we have:

q = -λA # e + A # ṡ * (3.21)
where λ is a positive gain and A # the pseudoinverse of A. At steady state, (3.21) gives an estimate of the object oating base and joint velocities, from which q is obtained by integration.

3.5.2

Formulation of the VVS-based tracking as a QP

We formulate the estimation problem as a QP for the following reasons:

(i) take into account modelling and measurement error [START_REF] Xinjilefu | Dynamic state estimation using quadratic programming[END_REF] (ii) add constraints (equalities or inequalities) on the estimation variables (e.g. joint limits) (iii) integrate the estimation to the control in the same unied framework Indeed, since many robotic control framework are based on a QP that also includes visual servoing tasks (2), it is reasonable to think about the articulated object tracker written as a QP.

The VVS rationale in Sect. 3.5.1 can be written as a minimization problem. Since the decision variable of the QP robot controller is the acceleration of the multi-robot conguration, it is convenient to dene q as state of our QP-VVS. Thus, the objective function writes:

f o (q) = 1 2 k(s * -s(q)) + b(ṡ * -ṡ(q)) + (s * -s(q)) 2 (3.22)
with k a positive constant gain and b = 2 √ k. From (3.8), we have:

si (q) = Ȧi q + A i q (3.23)
where Ȧi = -Li J i -L i Ji , i = 1, . . . , f . To compute Ji , we need the derivative of J ϕ expressed element-wise as:

J1,j ϕ = -1 2α φj s ᾱ + ϕ j α cᾱα-2sᾱ 2α , (3.24)
Ji+1,j

ϕ = ϕ j φj α 2 c ᾱ -sᾱ ᾱ + α α cᾱ 2 -sᾱ α + ϕ 2 j α α 2 s ᾱ 12-α 2 4α 2 -3cᾱ 2α if i = j, (3.25)
Ji+1,j

ϕ = ϕ i φj + φi ϕ j α 2 cᾱ 2 -sᾱ ᾱ + ϕ i ϕ j α α 2 s ᾱ 12-α 2 4α 2 -3cᾱ 2α if i = j, (3.26)
with α = (ϕ • φ)/α; using Taylor expansion to approximate sine and cosine for α 0 (60). From (3.12) and (3.18) we obtain Jo as:

Jo = O 3 O 3 O 3 2 ĖT J ϕ + E T Jϕ . (3.27)
and as Ji = Vc

o J o i + V c o Jo i , thus
Ji can be composed as follows:

Ji = ( Jo O 6×n ) if i ∈ oating base, ( Jo Jl O 6×(n-l) ) if i ∈ l-th link. (3.28)
And we compute the derivative of L i (eq. (3.29)) and the variables needed for its computation as:

LT i =          λθ i s θ i + λ θ i c θ i θi λρ i s θ i + λ ρ i c θ i θi λθ i c θ i -λ θ i s θ i θi λρ i c θ i -λ ρ i s θ i θi -λθ i ρ i -λ θ i ρi -λρ i ρ i -λ ρ i ρi ρi s θ i + ρ i c θ i θi -s θ i θ(1 + ρ 2 i ) + 2c θ i ρ i ρi ρi c θ i -ρ i s θ i θi -c θ i θi (1 + ρ 2 i ) -2s θ i ρ i ρi 0 0          (3.29) λθ i = (-ȧi c θ i + as θ i θi + ḃs θ i + bc θ i θi )d i + (a i c θ i -bs θ i ) ḋi d 2 i (3.30) λρ i = ( ȧi ρ i s θ i + a i ρi s θ i + a i ρ i c θ i θi + ḃi ρ i c θ i + b i ρi c θ i -b i ρ i s θ i θi + c i )d i -(a i ρ i s θ i + b i ρ i c θ i + c i ) ḋi e d 2 i (3.31)
Then, substituting (3.8) and (3.23) in (3.22), the QP providing the double derivative of the articulated object conguration can be written as follows:

q = argmin q∈S 1 2 qT Qq + c T q S = {q | Kq ≤ k} (3.32)

Visual Tracking of Articulated Objects

from which q is obtained by numerical integration. In (3.32), we have:

Q = A T A c = -A T [k(s * -s(q)) + b(ṡ * -A q) + s * -Ȧ q] (3.33)
where s * and ṡ * are obtained by numerical derivation. S represents the set of feasible q accounting for the kinematic and dynamic constraints of the articulated object. For instance, to impose limits on q, one could set:

K = [-I m , I m ] T k = [-2(q T min -q T -qT T s )/T 2 s , 2(q T max -q T -qT T s )/T 2 s ] T (3.34)
as in [START_REF] Bouyarmane | Quadratic programming for multirobot and task-space force control[END_REF], where I m is the m × m identity matrix; q min and q max dene the range in which q is constrained; and T s is the sampling time.

To summarize, Alg. 1 presents the whole routine computing q. An edge tracker (see Sect. 3.4.2) processes the image I acquired by the camera to provide the vector s * . Then, using the current estimation q, the model of the object is updated, so that the positions of the features are also estimated and available for subsequent computations. Thus, for each visual features i = 1, . . . , f the following actions are carried out:

• Jacobian J i and its time derivative Ji are computed using current q and q • a supporting plane π i is computed using the line Plücker coordinates and the current estimate of the oating base position; the derivative πi is also numerically computed

• virtual visual feature s i (q) is obtained projecting i on the image plane, using the camera projection model

• the image Jacobian L i and its time derivative Li are computed using the estimate of the plane and the coordinates of the visual feature, and their time derivatives

• the i-th block of the articulation matrix, A i , is obtained At the end of these steps, the articulation matrix is fully built, Q and c can be computed and the QP solved. Since the estimate is accurate only when the tracking converges, an inner loop repeats the tracking operations until the norm of the VVS error on θ and ρ (e θ and e ρ , respectively) decreases below desired precision thresholds e th,θ and e th,ρ .

Algorithm 1 Algorithm realizing the VVS-based tracking of articulated objects.

for each new image frame I do s * ← Track Features(I) while ||e θ || > e th,θ ∧ ||e ρ || > e th,ρ do ← Update Model(q) for each visual feature i do J i ← Compute Jacobian(q) Ji ← Compute Jac. Derivative(q, q)

π i ← Compute Supporting Plane( i , q) s i (q) ← Project( i ) L i ← Compute Image Jacobian(π i , s i (q)) Li ← Compute Im. Jac. Der.(π i , s i , πi , ṡi ) A i = -L i J i , Ȧi = -Li J i -L i Ji end for q = argmin 1 2
qT Qq + c T q q ← Double Integration(q) end while end for

Experimentations

The presented results are obtained by tracking two articulated objects: a printer with the prismatic joint of its paper tray, and a cabinet with its door revolute joint. The tracking provides the objects oating base pose and the joint variable (m = 7).

Our results are obtained using dierent numbers of line features. Indeed, the VVS estimation depends on the number of the f tracked lines and their relative position. For example, to correctly estimate the oating base pose, a minimal set of 3 non-coplanar lines is required to ensure that the stacked image Jacobian, and consequently the articulation matrix, is full rank (the same problem is remarked in the dual context of VS ( 116)). The lines coplanarity can be detected with additional computation. However, we decided to track more lines than the minimal required, to create redundancy and avoid rank deciency. This solution, paid in terms of a higher computational cost, is shown to be feasible in the results presented below. Furthermore, increasing the dimension of the feedback helps to have more robust tracking results with respect to the perception process noise and bad detection of the line features. Thus, we chose the lines number f driven by the trade-o between high tracking performance and low computational cost. Note that the algorithm (shown in Alg. 1) scales linearly with f . In the experiments with the printer we used 4 lines on the oating base and one on the paper tray; for the cabinet we used 5 lines on the oating base and 2 on the door.

The algorithm run on a PC with an i7 2.60 GHz CPU and 8 GB of RAM, processing the images from the monocular camera of a XtionPRO live RGB-D sensor, that gives a video stream of 640 × 480 pixels at 30 Hz. Since the frequency of the estimation process is, in general, higher, the remaining time is used by the inner loop to make the VVS converge.

An oine calibration procedure provides the camera intrinsic parameters used to compute the projection model. The real lines detection algorithm is based on the OpenCV library.

A low-pass frequency lter is used to clear the noise from the conguration estimate, the detected lines and their derivative.

We propose two sets of experiments. The rst aims at evaluating the tracking performance, the latter shows the eectiveness of the tracking for robotic manipulation tasks.

Visual Tracking of Articulated Objects

As detailed in Sect. 3.5, we used as feedback only the lines that are detectable at the borders of the objects. However, for the rst set of experiments, we also placed two markers boards on the oating base and on the link of the objects. These markers are not considered in our estimator but processed by the Aruco library (58) to reconstruct the objects conguration, considered for comparison in our work. For this set of experiments, the thresholds of the error used to stop the inner loop were tuned to e th,θ = e th,ρ = 0.015.

In rst experiment, the average time spent by the lines tracking process was 5.79 ms, and 8.31 ms by the VVS estimation (corresponding to an average of 366 iterations of the inner loop). The parameters for the lines searching algorithm (Sect. 3.4.2) were set to m θ = 0.035 and m ρ = 12 whereas the margins on the ROI were set to m ROI,x = 30 and m ROI,y = 10. The VVS-QP gain was heuristically set to k = 750. The tracking process steps and performance are shown in Fig. 3.5.

In the second experiment, we tracked the cabinet (see Fig.

Robotic Manipulation of Articulated Objects

In the second set of experiments we made HRP-4 robot manipulate the articulated objects.

To this end, the estimation output given by the VVS-based scheme is used as feedback in the MQP framework to control the robot+object system, as mentioned in Sect. 3.1. Among the others, the framework allows to dene contact constraints between the robot and the object to be manipulated, actually coupling the two parts in a single system as seen in the previous chapter.

Once that the contact is established, the overall system can be controlled with both cartesian and postural tasks. Indeed, in order to grasp the object, a cartesian error τ h = p h -p h,d is dened, where p h is the current value of the robot hand pose and p h,d is the object pose to be grasped, given by the tracking algorithm.

For the manipulation task, an error is dened as τ q = q 1 -q 1,d , where q 1,d (desired object joint value) is provided by some form of planning, while q 1 (current object joint value) is estimated by our method. To execute the grasping or the manipulation task, a new term is properly added in the cost function of the MQP, that tries to zero the corresponding error.

Each term is given a gain, imposing a decrease rate of the task error, and a weight, dening the priority of the task. The constraints and the tasks are added or removed in the MQP with a state machine.

In this set of experiments, we tuned the parameters of the perception algorithm as follows:

m θ = 0.35, m ρ equal to 12 for the printer and 10 for the cabinet, m ROI,x = 40 and m ROI,y = 10. The gain of the VVS-QP was tuned to k = 750 for the printer and k = 1250 for the cabinet; nally, we set to e th,θ = e th,ρ = 0.005.

In the rst manipulation experiment, HRP-4 operates the paper tray of the printer (see The plot of Fig. 3.11 shows the manipulation commands (black dash-dot line) that are well followed by the printer prismatic joint, as estimated by the VVS method (blue line with triangular markers). With reference to the plot, the contact constraint between the robot hand and the printer is activated around time 8 s, after which the robot moves the tray toward the default desired position, that is 0 m; the rst opening motion is sent at 12 s.

One can observe a lag between command and tracking. This is due to the backlash in the humanoid-printer system, and the MQP task error decrease rate. One could also observe a not perfect positioning of the tray (e.g., at 38 s or 72 s). This is due to the high friction of the tray mechanism, preventing a smooth manipulation motion and a ne positioning. In this experiment, the lines and object tracking process took an average of 9.79 ms and 8.8 ms (409 inner iterations). In the second manipulation experiment we achieve both grasping and operation of the cabinet door. Three signicative snapshots taken from the experiment video are shown in Fig. 3.15. Fig. 3.14 shows that the angle of the door remains constant while the robot hand is reaching the object. At time 22 s, the hand touches the door and accidentally closes it by 0.1 rad. Then, at 25 s, the contact constraint is activated and the robot steer the door at the default command (0.8 rad). Finally, the commands are sent at about 29 s and HRP-4 performs the opening/closing motion as specied by the user. On an average, the tracking of the lines used 10.38 ms and the estimation process 8.71 ms (for 277 inner loop iterations). [rad]

VVS command

Conclusion

Our online articulated objects estimator can be written as a virtual visual servoing quadratic program tracker in the acceleration space. The estimator and the control framework cooperates to achieve the desired manipulation task, sharing information (such as the object model). We have also shown that our algorithm is able to track non-structured objects, cope with missing/occluded features and cluttered environments. However, the use of line features, as well as any other geometric features, represents also a limitation. First, not all objects can be identied with simple geometric features. Second, this kind of information is prone to be mistaken with similar visible features not related to the object. Future work will investigate the possibility to include in the tracker also other kind of sensory information, such as reconstructed depth (under certain working conditions), robot joints encoders (when the robot is in contact with the object) and other features that can be learned robustly oine.

Tracking and manipulation experiments carried out with HRP-4 have shown the eectiveness of our approach to be used in closed-loop control. We believe that our algorithm can be a good basis for an extension to the tracking of more complex structures and employed in the eld of physical human-robot interaction.

After extending our MQP controller with force control and vision, we now need to give the capacity to the robot to localize itself in its environments vSLAM. The next chapter is dedicated to present our methodology, and results, to choose the most suitable camera for an indoor environments and our needs.

Chapter 4

Benchmarking Cameras for OpenVSLAM Indoors

vSLAMs and benchmarking background

Simultaneous Localization And Mapping is a rich research area ranging from ltering algorithms to loop-closure detection, exploiting various sensors as lidars, sonars, inertial measurement units (IMU) and cameras, to cite a few [START_REF] Bresson | Simultaneous Localization And Mapping: A Survey of Current Trends in Autonomous Driving[END_REF]. In this chapter, we deal with visual SLAM [START_REF] Taketomi | Visual SLAM algorithms: A survey from 2010 to 2016[END_REF] To the best of our knowledge, vSLAM technology started with the rst visual SLAM running in real-time based on key-points detected in Figures of a conventional camera [START_REF] Davison | MonoSLAM: real-time single camera SLAM[END_REF].

Since then, one of the key contributions have been architectural, by running in parallel the features tracking and the mapping stages [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF]. They were later renamed as front-end and back-end, respectively. The other type of key contribution is related to the visual feature considered such as very close to the camera measurement, i.e. pixel intensities [START_REF] Forster | SVO: Fast semidirect monocular visual odometry[END_REF], or much more abstract as Oriented fast and Rotated Brief (ORB) features [START_REF] Mur-Artal | ORB-SLAM: a versatile and accurate monocular SLAM system[END_REF]. All these works are done with the target of robustness and precision while keeping real-time localization and mapping, i.e. at the conventional RGB camera frame rate.

Redundancy brought by stereovision [START_REF] Engel | Large-scale direct SLAM with stereo cameras[END_REF][START_REF] Forster | SVO: Semidirect visual odometry for monocular and multicamera systems[END_REF] as well as considering the additional Depth modality (RGB-D), either in physics-based [START_REF] Bu | Semi-direct tracking and mapping with RGB-D camera for MAV[END_REF][START_REF] Kerl | Dense visual SLAM for RGB-D cameras[END_REF][START_REF] Whelan | Elasticfusion: Real-time dense slam and light source estimation[END_REF] or learning-based (19, [START_REF] Gabriel | Topometric localization with deep learning[END_REF][START_REF] Wang | DeepVO: Towards end-to-end visual odometry with deep recurrent convolutional neural networks[END_REF][START_REF] Yin | GeoNet: Unsupervised learning of dense depth, optical ow and camera pose[END_REF][START_REF] Zhou | Unsupervised learning of depth and ego-motion from video[END_REF] features modeling, also improve precision and robustness of localization and mapping thanks to a better handling of partial occlusions, illumination changes and low textured (mainly indoors) environments.

Monocular cameras of very wide eld-of-view, i.e. 180 deg panoramic or more with a sheye or catadioptric optics, have also been considered for vSLAM [START_REF] Caruso | Large-scale direct slam for omnidirectional cameras[END_REF][START_REF] Gutierrez | Adapting a real-time monocular SLAM from conventional to omnidirectional cameras[END_REF][START_REF] Matsuki | Omnidirectional DSO: Direct sparse odometry with sheye cameras[END_REF]. These methods mainly benet from higher opportunities in catching strong image features than with a conventional camera. And also, from the better conditioning of camera pose estimation that such optics bring. Both characteristics being known to be capable of estimating more reliably trajectories with visual odometry than with a conventional camera [START_REF] Zhang | Benet of large eld-of-view cameras for visual odometry[END_REF]. Following a similar way, recently, vSLAM has been adapted to the combination of conventional 44), ( 76), ( 95), [START_REF] Forster | SVO: Fast semidirect monocular visual odometry[END_REF] (50) ( 55), (161) ( 23), ( 72), ( 151) 103), ( 147), ( 156), (160) ( 92), ( 29), ( 63), ( 123) ( 96), ( 82), ( 159)

(a) (19), (
(b) (81)
(c)

(67) (c) (136)
(a) at least for learning, (b) as diverging optical axes, (c) as two opposite optical axes.

Properties of Camera types and Algorithms

cameras [START_REF] Kuo | Redesigning SLAM for arbitrary multi-camera systems[END_REF] as well as the combination of sheye cameras [START_REF] Im | All-around depth from small motion with a spherical panoramic camera[END_REF][START_REF] Sumikura | OpenVSLAM: A versatile visual SLAM framework[END_REF], to reach a eld of view of up to 360 deg, i.e. a full spherical eld of view.

Among the above related works, some are generic enough in terms of visual feature representation, or in terms of camera projection model to handle natively several types of camera [START_REF] Forster | SVO: Semidirect visual odometry for monocular and multicamera systems[END_REF][START_REF] Kuo | Redesigning SLAM for arbitrary multi-camera systems[END_REF]82,[START_REF] Mur | ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras[END_REF][START_REF] Sumikura | OpenVSLAM: A versatile visual SLAM framework[END_REF][START_REF] Zhao | GSLAM: A general SLAM framework and benchmark[END_REF]. However, to our best knowledge, OpenVSLAM [START_REF] Sumikura | OpenVSLAM: A versatile visual SLAM framework[END_REF] was the only framework that can natively handle RGB and RGB-D modalities, monocular or multi-camera setups of conventional or up to 360 deg eld-of-view stereoscopic at the time of the study. That is why we considered OpenVSLAM to evaluate the impact of the camera type on the vSLAM results in term of localization, with the same visual feature type (ORB), estimation method (pose-graph optimization) or implementation.

Properties of Camera types and Algorithms

OpenVSLAM is an indirect vSLAM; input frame(s) are pre-processed to extract features ORB features. These features are then processed to get 3D points used in the translation and rotation estimation of the current camera pose with respect to the map made of keypoints, whether during the mapping or the localization-only process (when the map is already available). However, depending on the camera type, frames are of dierent nature and the process to obtain keypoints is dierent as well. Thus, we briey recall the dierent properties and algorithms by camera type as well as their consequences on estimations of both maps and camera poses.

Stereo

A stereo conguration links rigidly two monocular cameras. So, features are extracted from the stereo-frame, ie. two input frames, left and right. For each feature of the left frame, its corresponding feature is matched on the right frame assuming stereo rectied frames and using epipolar lines. A keypoint is composed of the left feature coordinate and the horizontal right match [START_REF] Mur | ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras[END_REF]. Knowing the baseline and the focal length, these keypoints are triangulated to estimate scale. Thus, maps and camera poses are estimated at scale from one stereo-frame.

RGB-D

Features are extracted from the RGB frame. For each of these features, the equivalent horizontal right match is obtained by using the corresponding measured depth value(96).

Evaluation metrics

Then, as in the Stereo case, maps and camera poses are estimated at scale, but from measured depths, not estimated ones.

Monocular

Features are extracted from the RGB frame. Depth information is not observable from a single frame in case of a monocular conguration. It requires an initialization using structure from motion methods [START_REF] Mur | ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras[END_REF]

Evaluation metrics

The two main outputs of a vSLAM system are the estimated camera trajectory along with a map building of the environment. Evaluating the quality of the outcome map is a challenging work that involves 3D model of the existing environment, blueprints [START_REF] Watanabe | Robust localization with architectural oor plans and depth camera[END_REF], or topological data in order to build an accurate groundtruth. Therefore, we will use the estimated camera trajectory that the vSLAM system outputs to evaluate its accuracy during both mapping and localization processes.

For the evaluation, we consider two sequences of poses: estimated trajectory P 0 , . . . , P n ∈ SE(3) and groundtruth trajectory Q 0 , . . . , Q n ∈ SE(3). As they may come from dierent sources such as camera or motion capture system, these sequences may have dierent length, sampling rates and possibly missing data that require to perform a data association as additional step. This is taken care automatically by evo framework (62) based on the common timestamps between the dierent sources. To simplify the following notations, we assume that sequences are time-synchronized, equally sampled and have the same length. A sequence is a succession of homogeneous transformation matrices of a frame from a reference frame. The considered frame of a monocular camera is often the optical frame. For a stereo rig, the center between optical frames is the rst frame of the map used during the acquisition.

In case of the motion capture system, the considered frame is arbitrarily deduced from the tracked markers and the reference frame that we chose to be associated to the ground during the calibration process.

In the following part we recall two common evaluation metrics for vSLAM [START_REF] Sturm | A benchmark for the evaluation of RGB-D SLAM systems[END_REF][START_REF] Zhang | A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry[END_REF]. These metrics must be as low as possible to assess a good result. In addition, we also want to evaluate coherence of the localization with respect to a pre-built map by comparing two kidnapped poses and a trajectory between these two poses. with operator(E i ) dened as follow:

E i = (Q i -1 Q i+∆ ) -1 (P i -1 P i+∆ ).
• `trans': translation part such as operator(E i ) = ||trans(E i )||

• `rot': rotation part such as operator

(E i ) = ||rot(E i ) -I 3×3 || F • `full': operator(E i ) = ||E i -I 4×4 || F • `angle': rotation angle of E i dened as operator(E i ) = |angle(log SO(3) (rot(E i )))|

Absolute Pose Error

The absolute pose error (APE) evaluates the global consistency of the estimated trajectory by comparing the absolute error between the estimated and groundtruth poses over a trajectory.

As previously mentioned, the reference frame for dierent trajectories may be dierent. We must align them before comparison. This alignment is done by least-squares estimation of the rigid-body transformation T between P 0:n and Q 0:n (142). For a monocular setup, the scale cannot be recovered with vSLAM. An additional step of rescaling the estimated trajectory is necessary. This method requires as input both trajectories and n the number 4.5. Experimental Setup of frames to use in order to compute T . The estimated T slightly changes depending of n.

For this study, after proceeding to the data association step, we choose to use the length of the trajectory for n. Once this transformation matrix is estimated, the absolute pose error is dened as:

E i = Q i -1 T P i . ( 4
• × 42.5 • × 77 • (± 3 • ) 360 • * 90
• × 59 •
*Theoretically the FOV is 360 Such characteristics lowers the opportunities to sense features in low textured areas of any environment, particularly when the environment is narrow like ours. This is the reason why OpenVSLAM did not produce any result with the D435i. Thus, no mapping nor localization results with it could be shown and exploited to conduct this study.

• ×360 • but

Motion Capture System

For each trajectory we use the MotionAnalysis motion capture system for groundtruth OpenVSLAM is no more publicly available; however one could use another vSLAM with same capability (ORB-SLAM2 could be an example).

Environment

Data acquisition methodology

In this section we introduce the data recorded during various acquisitions used to produce an estimated trajectory with vSLAM. Records are clustered in two groups, one per purpose: mapping or localization. Indeed, we emphasize that some data are dedicated to obtain a map of the environment prior to perform a localization process with the remaining others.

Recall that the main idea behind this decoupled process is to be able to navigate later on in the entire known environment to perform tasks (see a practical example in ( 73)).

Whereas dense vSLAM algorithms produce maps which are often visually checked by looking at ghost walls or duplicates of the same actual environment, sparse maps of featuredbased vSLAM, as OpenVSLAM, makes dicult a similar analysis. Hence, to quantify its quality we use the RMSE trans of APE between the groundtruth and the estimated trajectory at the localization only stage.

Mapping

For a given environment, there is an innite number of possible paths along which one can acquire Figures to create a map with vSLAM. The path has usually a signicant impact on estimations. For example, relatively light variations in altitude along a given path would impact the map [START_REF] Zhang | Benet of large eld-of-view cameras for visual odometry[END_REF].

In this study, the cameras are always about 1.2 meters height. The almost constant height is a reasonable experimental characteristics when targeting mobile robots applications.

However, to limit the experimental biases, we imposed ourselves few rules during acquisitions:

Localization

For localization, we considered two cases shown in Fig. 4.5:

1. Stationary pose: four poses named A, B, C and D, are described by a triplet (x(m), y(m), θ(deg)).

From A to D, we have (1.96, -0.11, 168), (-1.11, -0.11, 158), (-0.99, 1.24, 121) and (1.68, 0.37, -28) respectively.

2. Path: a motion between two stationary poses described by a pair (length(m), ∆θ(deg)).

We consider three dierent paths: A to B (3.48, -7), C to B (1.66, 39) and C to D (4.58, -151).

Results

Poses A, B, C and D were dened within the environment in front of several areas of various content. For each case, we record three dierent sequences in order to evaluate statistically the estimated localization within a pre-built map. We repeat this procedure under three dierent conditions:

• Nominal : default environment in which there is no variation between mapping sequences and localization sequences;

• Lighting: prior to record the Figures sequences we modify the lighting conditions of the environment by switching o a subset of the available ceiling lights.

• Scenery: prior to record image sequences, we moved several objects of various size (Fig. 4.6).

Note that OpenVSLAM is not designed to support these perturbations. However these conditions reects the possible changes that may happen in any environment. Therefore, they are used in this study to quantify how considered cameras are sensitive to these perturbations.

Results

First, we compare three mapping runs to select the best reference for the purpose of a fair comparison for further localization-only runs.

Mapping

Table 4.3 shows indirectly the mapping error thanks to the APE (RMSE trans ) computed for the three MapA, MapB, MapC runs (Fig. 4.4). Before computing errors, estimated and measured camera trajectories are aligned to transform the former in the groundtruth reference frame. Only Theta S estimations are scaled. Maps are not modied to be used as they were estimated during the mapping phase for later localization only. To sum up, the Azure camera has the lowest RMSE trans of APE, followed by the T265 and lastly Theta S 4.8. Discussion among the three runs, each having a path turning at both extremities around A and B.

Among the three runs, T265 camera has the smallest deviation whereas both Azure and Thetas S cameras share their lowest error within MapA. Then, MapA is selected to serve as reference for a fair comparison between the various cameras.

Localization

Using MapA as input for localization, we obtain estimated poses and trajectories for the localization of each stationary and moving cases described in Section 4.6.2. Estimations are transformed in the groundtruth frame, for further evaluation, using transformations kept from the map alignments (Sec. 4.7.1).

Discussion

Focusing, rst, on vSLAM localization rates in narrow environment under nominal conditions, our study conrms the recommendation of using a wide FoV camera for visual odome- try in conned environment [START_REF] Zhang | Benet of large eld-of-view cameras for visual odometry[END_REF]. In our study, Theta S equirectangular camera is the only monocular one but it leads to better localization rates under nominal conditions than both Azure RGB-D and T265 stereo-sheye cameras (Tab. 4.7). Clearly, the widest FoV allows matching numerous keypoints in the pre-built map, in accordance with [START_REF] Zhang | Benet of large eld-of-view cameras for visual odometry[END_REF]. Furthermore, only Theta S could localize itself at every stationary pose in nominal conditions (Tab. 4.5) However, still in the nominal case, Theta S is the best, only regarding localization rates.

Indeed, T265 has the smallest RPE trans when it comes to translation component (Tab. 4.7),

showing that a wide FoV alone, even the widest, is not sucient to get the best local accuracy.

The combination of depth estimation (Sec. 4.3) with the wide FoV permits T265 to reach this result. Azure has a depth sensor, but it does not compensate its narrower FoV.

Looking at Tab. 4.8 about rotation component, Theta S has the smallest RPE but the biggest APE; it has an accurate estimation of the rotation but it is a noisy one. In opposite smallest RPE and biggest APE, we have the T265; it has smooth estimation but it is less accurate. The rotation analyses results are in adequation with Davidson and al. (43) as they wrote A signicant factor in [these issues] is the narrow eld of view of the camera used: the features which can be seen simultaneously are fundamentally close together, and this leads to high uncertainty in the camera position estimate attainable. Azure is a trade-o in between; it has a narrower eld of view and access to depth information to estimate its rotation. High ambiguity between rotation and translation is typical, especially when the features observed have a small depth range, a common situation.. And it makes it the best in global accuracy in translation, ie. the smallest APE trans (Tab. 4.5 and 4.7), thanks to its depth sensor and despite the FoV importance for local accuracy. Indeed, T265's depth is estimated (Sec. 4.3), thus more sensitive to calibration and matching errors than Azure. Theta S, after scaling its estimations, has 57.4% worse global accuracy than Azure and 19.6% worse than T265, due to scale drift. These results and analyses must be put into perspective of the map quality. The APE trans from MapA (Tab. 4.3) is a quantitative indicator of the quality of the pre-built map. It is similar to APE trans values in Tab. 4.7 thus assessing the direct impact of the mapping on the global accuracy of later localization processes.

Beyond conclusions of [START_REF] Zhang | Benet of large eld-of-view cameras for visual odometry[END_REF], as in nominal case, lighting and scenery conditions (Tab. 4.7) lead Azure to the lowest APE trans , T265 to the lowest RPE trans and Theta S to the highest localization percentage (tied by T265 is 1 case over 9). As in (53), looking at the rotation part (Tab. 4.8), it yiels dierent results; T265 has the lowest RPE rot , Theta S has the lowest APE rot and the highest localization percentage, and Azure is in the between. Overall, Azure is the most sensitive to variations in the scenery whereas Theta S is the most sensitive to lighting variations (Tab. 4.4). Complementing that, overall, Azure is the less sensitive to 4.9. Conclusion lighting condition whereas Theta S is the less sensitive to variations in the scenery. The Time-Of-Flight sensor used to acquire the depth of Azure camera is by nature tolerant to lighting conditions thus making the Azure less sensitive to light variations. However ORB features [START_REF] Rublee | ORB: An ecient alternative to SIFT or SURF[END_REF] are known to be resilient to lighting variations but non-invariant. Therefore T265 and Theta S cameras, by only relying on features to estimate poses, are more impacted by high lighting variations than Azure. The decreasing trend of APE trans (Tab. 4.4) is correlated to the localization percentage decrease since only localized frames are, obviously, considered in APE and RPE. Theta S is the less impacted by Scenery changes thanks to its wide FoV, allowing to extract more non-altered features to estimate poses than T265 and Azure. Then, the narrowest the FoV, the highest the impact of scenery modications on RPE trans and percentage localization.

Finally, T265's global accuracy is the least impacted by lighting and scene changes and is 2 nd for the other metrics.

We would like to emphasize that this study was conducted with a handed camera whereas our nal objective is to use it on a humanoid robot. When it comes to locomotion with a humanoid robot our nal target, impact due to contacts between foot and ground can perturb the whole system (132); we did not take this in consideration in this study. The collection of data required a tremendeous work of acquisition; to setup the whole system, to synchronize all data in realtime, to collect all the data four cameras stream and to write them in hard memory, to repeat the same acquisition several times. The issue of using a humanoid robot to collect the data is to always ensure that it will arrive at the given pose;

during walking, there are high chances of slippery and thus it will not nished the walk exactly where it should be this is why we want to use vSLAM, to ensure that we always go where we want to go. In the end, having a dataset for vSLAM recorded by a humanoid robot so with exploitable data from humanoid robot such as IMU, force sensors, joint position would be nice and protable to the reasearch eld; it has be done by [START_REF] Zhang | Humanoid robot rgb-d slam in the dynamic human environment[END_REF] but only for a RGB-D sensor the classic sensor.

Conclusion

In this chapter we evaluated the performance of Microsoft Azure (wide FoV and RGB-D), T265 (stereo sheye), Theta S (dual-sheye) and D435i (RGB-D) using OpenVSLAM. The environment was chosen to be representative of common robotic applications: an indoor setting where the environment is susceptible to have light variations or changes in the scene after building maps (e.g. indoor production industrial site, homes, oces, hospitals). We quantitatively evaluated localization within pre-built maps using each of these cameras.

depth information enhances the global consistency. Lighting variations and changes in the scene have less impact on stereo sheye camera.

As future work, this benchmark could be extended to real indoor industrial and hospital setups that are conned environments, considering locomotion while manipulation by a humanoid robot [START_REF] Tanguy | Closed-loop MPC with dense visual SLAM stability through reactive stepping[END_REF]. Thus, our methodology could be applied in these environments to assess the results while the camera is embedded on a real robot. Finally, for a camera choice, when it comes to vSLAM, stereo sheye is the best compromise thanks to its wide eld of view and its depth estimation. However, we chose to use the Azure Kinect camera on our robots: HRP-2KAI and HRP-5P; the reason why will be explained in the next chapter.

The next chapter is dedicated to our industrial application; we use our MQP with force control and visual capacity with humanoid robots in a loco-manipulation context a robot has to move a large and heavy object in an autnomous way from and to a given pose.

Introduction

The car or the object are the outliers of interest. The images obtained from the robot's embedded camera contains both the surrounding environment and part of the outliers of interest. Ideally, the vSLAM would automatically extract features from the image corresponding only to the environment. When the outliers of interest are static w.r.t the robot camera (i.e. they do not move over time), a mask can be applied as in [START_REF] Chappellet | Benchmarking Cameras for OpenVSLAM Indoors[END_REF] to cancel them from the image. Moreover, such a mask is easy to design when the image outliers are limbs of the robot [START_REF] Tanguy | Closed-loop rgb-d slam multi-contact control for humanoid robots[END_REF]. However, when the outliers of interest move w.r.t camera frame typically because the robot is not attached to the car in (i) and the manipulated objects are not attached to the robot in (ii), they need to be eciently recognized and tracked to be canceled from the image and not considered during the SLAM process (4). In (i), it is important to track the car w.r.t the camera frame for the robot to manipulate the car accessories to drive it; whereas in (ii) it is also important for the robot to know where it stands w.r.t the object to be manipulated. In this case, vSLAM cannot be used to localize both the object and the robot. We also encountered in [START_REF] Kheddar | Humanoid robots in aircraft manufacturing: The airbus use cases[END_REF] situations where the humanoid evolve in narrow and conned space, where vSLAM would systematically fail because of the lack of textures; in this case, visual tracking could substitute SLAM for localization. The challenge that has not been considered in humanoid loco-manipulation, is to allow humanoid robots to use the image acquired by its embedded camera to solve both the localization of the robot for its surrounding and manipulated objects or systems that move with the robot. Moreover, since such objects are close to the sight of the robot and within its reach, they can take a large part of the image compared to the surrounding environment. In this case, wide-angle cameras are the best sensors for both vSLAM and visual tracking. and HRP-5P.

Background

Visual SLAM aims to estimate the robot ego-motion and the structure of the surrounding environment for robot navigation. Other solutions, not considered as vSLAM exist and rely only on depth images like in [START_REF] Maier | Real-time navigation in 3d environments based on depth camera data[END_REF] the robot looks mainly at the ground in a low-texture environment, but in our case the robot looks at the horizon to have enough information to locate itself and track the manipulated large-scale object. For those reasons we decided to look toward vSLAM literature. Many variants have been proposed exploiting various sensors as lidars, sonars, inertial measurement units (IMU), or cameras [START_REF] Bresson | Simultaneous Localization And Mapping: A Survey of Current Trends in Autonomous Driving[END_REF]. The use of a camera as a single (or main) sensor specializes SLAM as Visual SLAM or vSLAM [START_REF] Taketomi | Visual SLAM algorithms: A survey from 2010 to 2016[END_REF]. The standard pipeline of vSLAM systems consists of three main components (37): (i) camera pose tracking, (ii) scene mapping, and (iii) loop closing. Once the scene is mapped, the map can be reused for localization-only purpose. vSLAM systems have been made for various types of cameras, ranging from the most conventional in seminal vSLAM works [START_REF] Davison | MonoSLAM: real-time single camera SLAM[END_REF][START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF], to stereovision [START_REF] Forster | SVO: Semidirect visual odometry for monocular and multicamera systems[END_REF], panoramic [START_REF] Matsuki | Omnidirectional DSO: Direct sparse odometry with sheye cameras[END_REF], 360 [START_REF] Sumikura | OpenVSLAM: A versatile visual SLAM framework[END_REF] and active RGB-D [START_REF] Kerl | Dense visual SLAM for RGB-D cameras[END_REF][START_REF] Gabriel | Topometric localization with deep learning[END_REF][START_REF] Wang | DeepVO: Towards end-to-end visual odometry with deep recurrent convolutional neural networks[END_REF][START_REF] Whelan | Elasticfusion: Real-time dense slam and light source estimation[END_REF] vision. Other solution, not considered as vSLAM directly exist and only rely on depth We previously shown in chapter 4 that the use of RGB-D camera leads to the highest precision for both mapping and localization stages when the visual information is made of sparse feature points [START_REF] Chappellet | Benchmarking cameras for OpenVSLAM indoors[END_REF]. The precision of camera pose tracking is even increased when considering dense direct information, i.e. pixel brightness directly [START_REF] Zubizarreta | Direct sparse mapping[END_REF]. It is at the price of a tighter basin of convergence. This is not a problem if the camera velocity is bounded regarding its acquisition framerate, RGB and Depth streams are synchronized and the camera calibration is correct [START_REF] Civera | RGB-D Odometry and SLAM[END_REF][START_REF] Schöps | BAD SLAM: Bundle Adjusted Direct RGB-D SLAM[END_REF].

A recent benchmark about vSLAMs [START_REF] Merzlyakov | A comparison of modern generalpurpose visual SLAM approaches[END_REF] concludes that OpenVSLAM (used in chapter

3) is indeed one of the best for its diversity in term of sensors and results in diernt type of environment. However for indoor environment, the dense and direct RGB-D vSLAM named real-time appearance-based mapping (RTAB-Map) ( 82) is also a strong choice with a RGB-D

sensor such as the one we are using and it is the most deterministic in evaluations, containing no variation in all 250 executions from (93) conclusion. Thus, as users, RTAB-Map appears a convenient solution as it is also open-source since 2013 and still under developpement, thus largely evaluated by the community. Furthermore, RTAB-Map has recently been used for humanoid robots (Pepper [START_REF] Pfeiffer | Uts unleashed! robocup@home sspl champions[END_REF], HRP-2Kai ( 141)).

3D visual tracking methods are diverse and some are optimized for dierent types of the many kinds of cameras that are nowadays available. It is dicult to make a fair coverage of all the rich literature in visual tracking. We report however recent works that are close to our circle of interest either because they are available.

Wide-angle camera (89)( 27) use a model-based approach to track an object relying on geometrical features such as lines. However, these geometrical approaches, known to be not computationally expensive, tend to lose track in case of fast motion due to motion blur. With an omnidirectional camera, a mobile robot is localized within roads modeled as a color point cloud by using image intensities [START_REF] Crombez | Using dense point clouds as environment model for visual localization of mobile robot[END_REF]. While this method is accurate, it requires computing a 3D rendering at each step of the optimization thus making it computationally expensive.

In addition, it is tailored for texture objects or environments. Thanks to wide eld-of-view (FoV) cameras large parts of big objects are captured in one image. As we want our robots to concurrently manipulate or localized themself to even textureless objects, these methods are not suitable for our requirements. Figure 5.13 shows 3D models including metallic parts or transparent material that we are aiming to use with a robot.

RGB camera is used by [START_REF] Henning Tjaden | A region-based gauss-newton approach to real-time monocular multiple object tracking[END_REF] to track dierent small objects such as phones or kid toys within the scene. Whereas in chapter 3 [START_REF] Paolillo | Interlinked Visual Tracking and Robotic Manipulation of Articulated Objects[END_REF] we track articulated objects operated by a

robot. An alternative is to exploit a camera with a depth sensor such as a RGB-D camera.

RGB-D camera to track object 3D model (150)(153) [START_REF] Garon | Deep 6-dof tracking[END_REF]. On the other hand, articulated objects are also tracked by [START_REF] Schmidt | Dart: Dense articulated real-time tracking with consumer depth cameras[END_REF] taking advantage of the depth modality of an RGB-D camera. This modality provides geometrical information close to the geometry of an object's 3D model. In [START_REF] Wüthrich | Probabilistic object tracking using a range camera[END_REF] [START_REF] Issac | Depth-based object tracking using a robust gaussian lter[END_REF] objects that a robot has to manipulate such as books or drills, are tracked based on their 3D model. These works have been conducted with a pinhole camera.

Works like [START_REF] Roxas | Real-time variational sheye stereo without rectication and undistortion[END_REF] aim to produce a wide-angle depth from a wide-angle stereo camera or [START_REF] Perez-Yus | Scaled layout recovery with wide eld of view rgb-d[END_REF] that expands the depth of a pinhole camera to a sheye camera. These methods provide articial depth while the Azure Kinect camera Time-of-Flight (ToF) depth sensor oers a measured wide-angle depth (5).

Whole-body Locomanipulation (126) uses a mobile-based robot PR-2 to manipulate a cart in an indoor environment. It has to follow a path in a 2D map while slightly turning the cart in narrow passageways. However, the manipulation itself does not require a grasping sequence nor tracking of the cart. In ( 102) humanoid robot HRP-2 is manipulating a wheelchair. The robot can track the wheelchair and thus move itself and the wheelchair w.r.t. to its estimation. It is following orders of the person in the wheelchair and thus does not use its sensors to make decisions. One could say that the person is acting as a high-level operator giving inputs to the robot. ( 54) use NAO to showcase the approach motion to grasp an object smoothly. ( 129) formalized a plan of primitives to achieve loco-manipulation to avoid slow-down the motion between the dierent steps. The vision is not the main part of this work, however, it is used to detect the object to grasp.

Multi-ob ject tracking

Robot localization in the environment alone is not enough to move an object from its initial to the desired pose. The robot has to estimate the object pose for at least two reasons:

• computing the next motion

• ensuring a re-grasping sequence for object motion

The multi-aspect is required to be able to track the robot itself, and more than one object if we aim at nal assembly operations. We do not consider the robot as an articulated object as in ( 106) but as a fully rigid object with a joint conguration provided by the controller.

Each object is estimated in the camera frame attached to the robot (egocentric frame). The outcome of the visual tracking is also used to compute ecient dynamic masks in order to cancel these objects from the image used also by vSLAM. That is to say, all vSLAM features that superpose the mask (and hence the object visible part in the image) are removed from consideration in vSLAM. The mask cancellation eect on vSLAM performances are discussed later in Sec. 5.6.3. Regarding multi-object tracking, we went in two dierent directions to estimate the 3D pose of our objects:

• use of geometrical features, such as line and circle, with sheye stereo input However, we did not manage to obtain a susccessfull tracking of the object as seen in Fig. 5.5;

we did not investigate further with geometrical methods and the Intel RealSense T265. As presented in chapter 4, a new modality appears; the wide-angle depth with the Azure Kinect Camera. We decided to investigate it and nally to use it; we build our multi-object tracking based on it and this is the subject of the next section.

3D object's model

Our multi-object tracking is illustrated in Fig. 5.6 for one object o (large bobbin) w.r.t the camera's depth image c, at time t:

v T o = v R o v tr o 0 1 ∈ SE(3) (5.1) with v R o ∈ SO(3), v tr o ∈ R 3 .
The camera image depends on a known projection and distortion models. At a given time t, we acquire a wide-angle depth-image D * t , e.g. Fig. 5.6 Depth, where D * t (u) ∈ R is the depth measure (mm), and u = (u, v) ∈ U ⊂ R 2 is the pixel coordinates in the wide-angle depth-image domain U. As shown in Fig. 5.6, some pixels are depthless (the white ones), thus we have: 3D model is subdivided. A renement scheme (a common practice in computer graphics) is applied to this mesh. This process, that we achieve o-line, takes the original 3D mesh and subdivides it, creating new vertices and new faces. The result of this renement is a higher resolution 3D mesh that we can deform according to the distortion parameters of the camera; the CAD model and their renement are shown in Fig. 5.13 and Fig. 5 Our objective is to estimate the transformation between the camera and an object such as:

D * t (u) = d if depth is measured 0 otherwise (5.
c T ot = c T o t-1 o t-1 T ot (5.3)
where C X O,t-1 is the estimated object pose at time t -1. As the object is rendered within a virtual camera we have V X O,t , and:

c T ot = w T ct -1w T vt v T ot (5.4)
where W is the world frame. By re-writting (5.4) we have: Tvt being the current estimation in the iterative process. The operator Ω t (u) denes the set of pixel u as:

w T vt = w T ct c T vt (
Ω t (u) = null if          D t (u) > 0,
(u) × P N * t (û) ≤ δ θ (5.7)
where Rt is the rotation part of c T vt , δ d is a threshold in meter and δ θ is a threshold in radian.

One step in this process estimates a pose of the tracked object, however the geometry of the projected object model depends on its previous pose; thus the estimated pose may not be optimal. To ensure better convergence, we repeat the described process another time using the new estimated pose to project the CAD model of the object; D * t (u) and D t (u) should be more similar. This time we use only the highest level of the pyramid for one iteration (full resolution of the wide-angle depth); CAD model projection, pyramids construction, data association and resolution. We use this new estimate as the estimated pose of the tracked object.

It was the description for one object to estimate its pose. In case of multi-object tracking, in one pass, a single wide-angle depth image is computed for every object. Simultaneously, a corresponding image of IDs is created: each object has a unique ID. Thus, for each pixel of the rendered wide-angle depth image, we know to which object it is belonging. We solve Eq. 5.6 for each object by performing the data association step respecting the object's ID information.

Architecture

The objective is to let a humanoid robot move autonomously a large-scale object from a known initial pose to the desired pose in the environment. To do so, we are using the architecture shown in Fig. 5.9 made of four components: (i) a robot, (ii) vSLAM, (iii) The enriched map creation will be explain in 5.4.2. We will describe the interactions between the components in the rest of the section with a focus on the multi-object tracking as it is based on a new modality: the wide-angle depth.

Humanoid robot

To achieve loco-manipulation and to ensure to move a large-scale object to the desired pose, the humanoid robot should have a number of characteristics. Firstly, the robot must have gripping capabilities to manipulate the desired object. Secondly, the loco-manipulation relies on the humanoid robot's ability to stabilize while exercing forces on an object [START_REF] Murooka | Humanoid loco-manipulations pattern generation and stabilization control[END_REF]. It requires to have force sensors for each foot and gripper and an IMU embedded in the robot.

In addition, the gripper's force sensors are used to ensure the contact with the manipulated object. Lastly, the use of vSLAM and our multi-object tracking constrains the robot to have an RGB-D camera with a wide-angle depth sensor. We use only one camera for both vision components to reduce the number of required sensors embedded in the robot.

Visual Simultaneous Localization and Mapping

The vSLAM produces a 3D map of the rigid and static environment by moving a camera inside the said environment while estimating the camera pose. We target known closed environments as manufacturing industries. Typically, in a such context, the environment is known and has low instant change variability. With this pre-requisite, similarly as in chapter 3, we can use the vSLAM in two separate steps. The rst step is to create a map of it. The second step is to use this map to localize the robot during the experiment.

The rst step, map creation, is done by manually handling the camera in our environment.

The 3D map can be represented as a unique point cloud: it must contain the ground, the initial and desired object pose and the initial robot pose. In this point cloud, we will perform the registration of the ground 3D model and the desired object pose 3D model given us their poses on the map. The ground pose is then used by the controller to compute footsteps in vSLAM frame. We detail this point in 5.4.3. To move the object from its initial to the desired pose, the robot moves the object following a succession of waypoints. They are manually computed relatively to the registered desired pose in the map. These waypoints can be automatically computed taking into account the occupancy grid generated by the vSLAM, the robot-object system and the desired pose in the map. They could even be dynamically recalculated to avoid moving obstacles such as humans inspired by [START_REF] Missura | Fastreplanning motion control with short-term aborting a[END_REF]. But to show the feasibility of the approach, we rst decided to calculate them manually. We detail their use in 5.4.3. The robot's initial pose does not have to be pre-dene. However, the robot must be able to localize itself inside the map at the beginning of the experiment. A map with the ground and waypoints pose is what we call an enriched map.

The second step is to use this enriched map during the experiment to localize the robot. A rst diculty emerge during this step: the accurate localization relies on the map's keyframes structure. It is an issue as an accurate localization during the experiment is only possible if a similar point-of-view was used during map creation. To bypass this issue, during the experiement vSLAM is adding information to the pre-existing enriched map with data coming from robot's camera. However, the object is in the camera's FoV. It is also added to the map as a static part of the environment which leads to localization issues. To solve this point, we are masking the object to the vSLAM component. By doing so, it is not added to the map and we do not have localization issue. As the object is moving, we can not pre-compute a static mask nor we can use the controller's object pose due to possible modeling errors.

Thanks to the objects' estimated poses by the multi-object tracking, we can create a dynamic and accurate mask of the manipulated object. As input, vSLAM is using an RGB and a depth image which has the same intrinsic properties; it uses an RGB pixel if and only if it has depth information. Thus, we are not creating a new input for the vSLAM but setting manipulated object pose in the environment: its starting pose. The objective is dened relative to the object, we move straight with the object of a given distance or we turn the object of a given angle. With only stepping one time, the robot can only move the object of a given amount to reach the desired objective. Thus, after each step, the next necessary motion is updated w.r.t to the current manipulated object in the environment. This process is repeated until the object reaches its desired pose. If the MoveObject action objective is to move in a the straight line, the robot might have to a re-grasp the object in case of a bobbin (a rolling object), the robot has to regrasp it; in case of a wheelbarrow, there is no need of a re-grasping sequence. The correct re-grasping of the object is to ensure thanks to Finally, by combining all these actions the robot is able to move the manipulated-object to its desired pose. Based on the estimated object pose and the next targeted waypoint, the robot is able to choose the correct motion to eectuate for MoveObject action as illustrated in Fig. 5.10. In addition, to ensure a smooth transition between the two dierent motions the robot performs a succession of actions. Those actions depends of the robot-object con-guration and if the manipulated-object requires a re-grasping sequence; they are executed only if it is necessary. By example, for the bobbin case:

• To go from straight motion to turn motion, rst, the robot walks relatively to the object to be aligned with and grasp the object with the two grippers before initiating the turn motion

• To go from turn to straight motion, rst, the robot walks relatively to the object to be aligned with it and release one of the gripper of the object before initiating the next motion In case of re-grasping sequence during the straight motion, the released gripper is chosen to ensure the feasability of the straight motion. The robot should not perform two straight motions with the same gripper in contact of the object if it will lead to a robot's posture close to its kinematic limits. In the case of the bobbin, during the turn in place motion, the robot is slightly rolling the object forward; it is necessary to ensure a rotation along its central axis and not one of the side. Thus, during this motion, the robot might have to perform a re-grasping sequence to lower the pose of its grippers. At any moment during a MoveObject action, an operator can stop the motion safely, and takes the upper-hand on the next action to perform by the robot.

Finally, once the robot reaches a waypoint, when the estimated distance and lateral are below given thresholds, the robot aims to bring the object to the next waypoint. If it was the last waypoint to reach, the object is at the desired pose and the robot release it. Finally, the robot is standing in front of the object and it achieved the desired loco-manipulation.

Experimental Setup

We experimented bobbin loco-manipulation with a Kawada Robotics humanoid robot: HRP-5P. The bobbin is a rolling and symmetrical object found in many large-scale industries and contains generally cables of dierent kind that weights approximately 140kg. It has to be inserted in a xed cassette in the environment; both object models are shown in Fig. 5.13 .

The robot is steered with our task-space controller framework mc_rtc 1 .

We choose to mount on the robot an Azure Kinect camera thanks to a 3D printed support designed by ourselves. Azure Kinect camera integrates a 1-Megapixel Time-of-Flight depth camera (5) which oers an operating range from 75 • × 65 • to 120 • × 120 • while the RGB sensor has an operating range of 90

• × 59 • . In comparison, the Kinect V2 has an operating 1 https://github.com/jrl-umi3218/mc_rtc On an implementation level for the multi-object tracking component, we render the objects 3D model with OpenGL. Then the multi-scale pyramids are computing also with OpenGL while the system construction is done thanks to an adaptation to Azure Kinect camera using Brown-Conrady distortion model ( 21) of ICPCUDA 1 with CUDA. The data from OpenGL to CUDA are shared thanks to the availability of interoperability functions. Lastly, the pose estimation is performed on CPU with Eigen thus we only copy a 6 × 6 matrix and a 6 × 1 vector from device (CUDA) to host (CPU). It allows us to have real-time capability to track the desired object with an Azure Kinect camera. We run the tracking on a laptop equipped with a GeForce RTX 2060 NVidia GPU.

We recall that we choose to use RTAB-Map as vSLAM for its wide community and already proven results. We created a map of our environment, an indoor laboratory, to achieve the experiment. We performed the registration of the objective, the cassette in the environment, to nally have the enriched map.

With all these elements we are ready to perfom the experiements and to discuss their results in the following section.

Experimental Results

In this section, we discuss the results of: the multi-object tracking for dirent objects, its use with HRP2-Kai to grasp and re-grasp a bobbin, the generated mask and its inuence on vSLAM estimation, and lastly the presented work for loco-manipulation of large-scale object.

Multi-Object Tracking

In this subsection, we provide results about the multi-object tracking with four dierent objects: a bobbin, a cassette, a wheelbarrow and a subpart of an A400M. Those results does not involve the humanoid robot; the camera or/and the object were manipulated by hand.

1 https://github.com/mp3guy/ICPCUDA

Experimental Results

We need a CAD model for each object as illustrated in Fig. 5.13. In 5.3 we explain the need to subdivide the CAD model before to use it; we use the Blender built-in simple subdivide operation to preserve the original shape. The result in changes of vertices number are given in Tab. 5.1 and illustrated in 5.14. On all the plots presented in this section, the time never starts at 0s; the reason is simple, before the operator was initializing the tracking manually.

The operator has to manually move the object in translation and to rotate; to do this, it has to visually put the object CAD model in a pose that will t the point cloud created thanks to the wide-angle depth frame as in Fig. 5.12. The estimation of the cassette is particularly challenging; it is a metallic object with a in our loco-manipulation experiment the robot has to bring the bobbin inside this object.

Compare to the bobbin, and to the other objects as well, the number of possible inliers is small due to the nature of the object; it is more sensible to camera motion. Fig. 5.17 shows the result of the tracking. The front part of the cassette is a slope, which is an inclined plane. Due to its similar geometry with the ground also a plane, it could happened that the slope was matched with the groud leading to a wrong estimation. (g), forward and backward motion, the bobbin's pose estimation follows the bobbin's pose in the task-space control. Lastly in step (h), the robot opens its grippers and release the bobbin. In the task-space control the bobbin remains at its last pose while in reality it is rolling in direction of the robot. HRP-2Kai is in front of the bobbin and we can repeat the same set of steps from (a) to (h): we did it four times in a row. HRP-2Kai susccessfully grasped the bobbin each time thanks to the bobbin's pose correctly estimated.

vSLAM and dynamic mask

In this section we will discuss the results and highlight the necessity to mask the manipulated object for the vSLAM component. During the experiment we recorded the data from the camera and perform the analysis at posteriori. We initialy tried to perform the experiement by using vSLAM in localization mode only.

In this case, the estimation result heavily depends of the keyframes existing in the pre-built map. The position estimation is shown in Fig. 5.24a. It contains noise that impacts the accuracy of robot pose estimation. Moreover if there is no already existing keyframes in the pre-built map near the camera pose, it is fairly possible that vSLAM will not be able to estimate a pose. Both of these points heavily reduce the success rate of the experiment.

To prevent the lack of keyframes in the pre-built map there are two ways: pre-build a map with more keyframes or create new keyframe during the experiement itself. Virtualy, there is an innite possibility pose for a keyframe in a pre-built map. Therefore the rst solution does not prevent to fall in our discussed issue. The second solution leads to another issue: the manipulated is moving with the camera and thus it is map inside the map during the experiment. Figure 5.23b shows the additionnal information to the pre-built after the end the robot loco-manipulation. In conclusion, this conguration is not suitable to perform loco-manipulation.

An example of rectied depth where the object is masked and used as input for vSLAM is shown in Fig. 5.25c. Figure 5.23c displays the addition to the pre-built map when we are applying a mask to the manipulated object for vSLAM. It does not appear corrupted nor it contains the manipulated object and reect the real environment. This is directly reected in Fig. 5.24c where the estimated position does not jump and it is smoother than localization only. This conguration allowd us to localize the robot in its environment to ensure that the object is moved toward its desired objective.

HRP-5P loco-manipulation

Similarly to HRP-2Kai, HRP-5P will have to perform numerous grasping during the locomanipulation of the bobbin; it has to grasp it in-between its spokes to ensure a solid grasping when closing its grippers, especially to turn the object in place. During a motion, due to the bobbin inertia or friction with the ground, it is possible that it does not end its motion at In fact, it does not mean that the bobbin is perfectly aligned with the waypoint, we might be on the verge of the threshold. We can see this in Fig. 5.27 between 310s to 325s.

It leads to a turn motion that interrupts the straight motion as visible in Fig. 5.26. At the end of the turn motion, the bobbin has a better alignement to reach the waypoint.

At the end of the experiment, after 425s in Fig. 5.27 the estimated lateral and distance are in-between the dened thresholds. It means that the bobbin is correctly standing at its desired pose. Also, HRP2-Kai is smaller than HRP-5P in height; again during the turning motion, the robot steps laterally. Due to this dierence, for a given distance of step, HRP2-Kai leg was going near its kinematic limits to achieve the motion; by doing this motion again and again it leads to undesirable and dangerous motion. We had either to reduce distance of a lateral step and thus making alsmost the double of required step to turn for a given angle compare to HRP-5P or to nd a solution. The solution was to introduce a CoM motion in altitude to preserve the distance between the sole of the foot and the CoM at any moment of the turning motion if the distance does not change, the leg can not go toward the kinematic limit in extension.

HRP-2Kai loco-manipulation

Conclusion

We showed that a humanoid robot is able to manipulate a large-scale object to move it toward its desired pose relying on embedded sensors. The presented architecture has susccessfully demonstrated its capability to be precise enough to insert a rolling object, the bobbin, inside its cassette. It is a challenging motion as the bobbin is a non-holonomic object. The success of the whole experiement relies on the improved estimation of camera by vSLAM thanks to the dynamic mask. We also demonstrated the interest of the new modality, the wideangle depth, oers by the Azure Kinect Camera when it comes to tracking large-scale object.

However, this modality is not yet exploited as it fullest as vSLAM is relying on rectied depth image. Finally, in a larger scale environment, the burden of pre-computed the waypoints could be replace by using Any-angle path planning algorithm with the 2D obstacle map obtained with RTAB-Map.

environment. More specically, we wanted to allow humanoid robots to manipulate largescale objects while walking the case of loco-manipulation in an industrial context, and autonomously . We achieved this goal by adding new features to our multi-robot QP controller: force control, a visual estimator for articulated objects, and a visual estimator for large-scale objects. They allowed us to create an autonomous loco-manipulation framework for humanoid robots.

Force control is now at the heart of many interactions the robot may have with its environment: bipedal walking requires regulating the forces under each foot, making and keeping contact with an object also requires regulating forces, and grasping an object may require minimizing the forces applied during gripper closure.

Force control alone was not enough. In fact, it does not provide knowledge about the placement of an object with respect to the robot; this requires visual acuity. For this, we equipped the robot with a RGB-D camera, the Azure Kinect Camera; it has a wide-angle depth. We have developed two real-time visual estimators using dierent modalities. The rst one estimates the conguration of an articulated object using RGB images and relies on the tracking of lines, geometric features, in the image plane. The second one estimates the conguration of a large-scale object using wide-angle depth images and relies on the resolution of an ICP. Now the robot can perceive and interact with objects in its environment; objets do not have to be placed at a predened position.

This was still not enough. To be autonomous, the robot must be able to move in its environment, and more particularly to localize itself. A well-established technology that addresses this problem is vSLAM; we use RTAB-Map. However, the robot must move largescale objects in the environment; a moving object in the scene is a disturbance for RTAB-Map and many other vSLAMs. We use our visual large-scale object estimator to estimate the pose of the object and to mask it from the depth image; its features are no longer used by the vSLAM to compute the pose of the robot in the environment.

With these new additions, we have built a loco-manipulation framework based on our multi-robot QP controller. Our framework the rst of its kind is dedicated to the locomanipulation of large-scale objects and allows humanoid robots to evolve autonomously in their environment; the robot can move to a bobbin, catch it and move it until it is inserted in its housing, thus freeing workers from this non-value added task. We validated our framework and the realization of this complex task experimentally with the humanoid robots HRP-2Kai and HRP-5P handling a bobbin of 1.330 meters in diameter and weighing 140kg.

Through this thesis, we have validated our results experimentally with dierent humanoid robots and dierent objects: HRP-4, HRP-2Kai and HPR-5P; a box, a door, a printer, a

2 . 1

 21 Force notation illustration. . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. 3 .

 3 From top to bottom: both switches 1 and 2 open, switch 1 open and switch 2 down, switch 1 closed and switch 2 up, switch 1 closed and switch 2 down. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Figure 2 . 6 (

 26 here the desired force wrench F d is given directly to the QP and F QP is not used). The vertical bar indicates the moment where a desired force wrench F d is changed from either one of the hands or feet. Horizontal lines indicate the desired values and continuous curves are measured taking their values from the appropriate force sensor. When there is no dotted lines, it means that there is no explicit desired force sent to the robot. From up to down: left, right hands, and left foot three components of forces and moments. . . . . . . . . . . . . . . . . . . . . . . . 21 2.8 Three dierent executions of the experiment in Fig. 2.6 to compare the different proposed QP force control methods. Each row represents the data for one run of the experiment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

  . . . . . . . . . . . . . . . . 23 2.10 Example of on-line dynamic inertial parameter estimation experiment for a manipulated box. Each image shows a posture way-point. . . . . . . . . . . . 25 2.11 Sequence of printer tray opening with HRP-4. . . . . . . . . . . . . . . . . . 26 2.12 Forces from printer tray opening with HRP-4. . . . . . . . . . . . . . . . . . 27 2.13 Door opening with HRP-4 using the force control scheme of the multi-robot QP. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 xi LIST OF FIGURES 2.14 Multi-robot collaborative manipulation between HRP-4 and ROMEO's left arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 2.15 Robot-robot co-manipulation motion. Resulting coordinated motion (position of HRP-4 and that of ROMEO's hand link frames) from single task command (position of box frame). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 2.16 Dexterous hand clicking a pen. . . . . . . . . . . . . . . . . . . . . . . . . . . 30 2.17 The green spots are the predened contact areas. . . . . . . . . . . . . . . . 30 3.1 A case example of domestic robotic manipulation: a humanoid pulls the drawer of a dresser. The scene observed by the robot camera is used to estimate the dresser conguration and achieve the manipulation task. . . . . 38 3.2 An exemple of selected elements for the initialization: P1, P2, P3, P4 are used to estimate the oating base; 1, 4, 8, 2, 18 are the edges pointed in the image with their id . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41 3.3

91 5.6 6 -

 916 Fig. 3.7 after 50s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.7 Second tracking experiment: the cabinet revolute joint angle. . . . . . . . . . 52 3.8 Second tracking experiment: the cabinet oating base position. . . . . . . . . 53 3.9 Second tracking experiment: the cabinet oating base orientation. . . . . . . 54 3.10 Third tracking experiment: the printer in a cluttered evironment (a). Detail of the tracker using the the QP-VVS (b) and the classic formalism (c). . . . 54 3.11 First manipulation experiment: the printer prismatic joint position. . . . . . 55 3.12 First manipulation experiment: time corresponds to Fig. 3.11 (a) rst opening at 12s; (b) second opening at 40s; (c) second closing at 50s; (d) third closing at 60s; (e) last opening at 72s; (f ) last closing at 80s . . . . . . . . . . . . . . 56

Figure 2 . 1 :

 21 Figure 2.1: Force notation illustration.

( 2 . 5 )

 25 τ = (τ 1 , . . . , τ n ) ,

Figure 2 . 2 :

 22 Figure 2.2: Predicted forces (green) in planned contact state {Contact 1, Contact 2, Contact 3} versus sensor forces (yellow) in real contact state {Contact 1, Contact 2} (the predicted forces are resultant at the sensor locations of the point forces in dashed lines computed at the vertices of the contact prints). In this situation the QP controller assumes that the robot is in the planned contact state and therefore predicts forces that do not correspond to the actual force repartition, since the hand contact (Contact 3) is not yet established.

Figure 2 . 3 :

 23 Figure 2.3: Block diagram for force control with the QP. The error between the target force

Figure 2 . 4 :Fig. 2 . 3 .

 2423 Figure 2.4: Simplied representations of the dierent switch positions in the block diagram of Fig. 2.3. From top to bottom: both switches 1 and 2 open, switch 1 open and switch 2 down, switch 1 closed and switch 2 up, switch 1 closed and switch 2 down.

. 3 .

 3 Fig. 2.4 illustrates a simplied representation of the dierent switch combinations in the block diagram of Fig. 2.3.

1 )Figure 2 . 5 :

 125 Figure 2.5: The eect of the gains in diag(K I ) on the force time response and fast interactivity of the right hand of the HRP-4 robot to human interactive manipulation. The same behavior (dierent gains for feet) are observed in the both hands and feet that are force-control to zero-force reference (F d = 0 in all components). The admittance gains are changed on-line three times during the manipulation from its initial (high damped) value to double and double again from the values. The higher the admittance gain, the faster the response and the more interactive the robot is to human guidance.

Figure 2 . 6 :

 26 Figure 2.6: Base experiment for comparing the dierent proposed QP force control paradigms.

Figure 2 . 7 :

 27 Figure 2.7: Simultaneous multi-unilateral-contact force control of the humanoid robot HRP-4 in the setting of Figure 2.6 (here the desired force wrench F d is given directly to the QP and F QP is not used). The vertical bar indicates the moment where a desired force wrench F d is changed from either one of the hands or feet. Horizontal lines indicate the desired values and continuous curves are measured taking their values from the appropriate force sensor. Whenthere is no dotted lines, it means that there is no explicit desired force sent to the robot. From up to down: left, right hands, and left foot three components of forces and moments.

Figure 2 . 8 :

 28 Figure 2.8: Three dierent executions of the experiment in Fig. 2.6 to compare the dierent proposed QP force control methods. Each row represents the data for one run of the experiment.

Figure 2 . 9 :

 29 Figure 2.9: Comparison between the two methods that account for a desired force command F d , in two additional instances of the experiment in Fig. 2.6. In the rst method, the robot

Figure 2 . 10 :

 210 Figure 2.10: Example of on-line dynamic inertial parameter estimation experiment for a manipulated box. Each image shows a posture way-point.

Figure 2 . 11 :

 211 Figure 2.11: Sequence of printer tray opening with HRP-4.

Figure 2 . 12 :

 212 Figure 2.12: Forces from printer tray opening with HRP-4.

Figure 2 . 13 :

 213 Figure 2.13: Door opening with HRP-4 using the force control scheme of the multi-robot QP.

Figure 2 .

 2 Figure 2.14: Multi-robot collaborative manipulation between HRP-4 and ROMEO's left arm.

Figure 2 .Figure 2 . 16 :

 2216 Figure 2.15: Robot-robot co-manipulation motion. Resulting coordinated motion (position of HRP-4 and that of ROMEO's hand link frames) from single task command (position of box frame).

Force

  task on left hand after contact 2D CoM of robot 1D position of the printer tray after grasp Door 6D position of right hand before contact 2D CoM of robot Force task on right hand just before contact and when pressing the handle 1D angle of the handle after grasp 1D angle of the hinge after handle task complete Romeo 6D position and orientation of the HRP-4 hand before contact Force task on HRP-4 hand after contact 2D CoM of HRP-4 6D position and orientation of Romeo arm before contact 6D position and orientation of the box after contact Pen 19D Grasp conguration of the hand before contact 1D position of the click joint of the pen after grasp All Full conguration rest (or zero) task (low weight task to ensure QP is positive denite)

  feasible) but also plan them if not specied. The implementation code of the multi-robot controller is open source; it has been interfaced with vRep (now CoppeliaSim), Gazebo, Choreonoid and recently AGX from Algoryx simulators. The code is already distributed to several teams worldwide, it is sustained, and has been implemented on other humanoid and robotic platforms (e.g. ARMAR, Nao, Pepper, HRP-2Kai, HRP-5P, KuKa arms, Panda amrs, UR arms, etc.).

  3.1), a robot needs the knowledge of the handle pose to grasp it, and the drawer joint position to control the opening motion.

Figure 3 . 1 :

 31 Figure 3.1: A case example of domestic robotic manipulation: a humanoid pulls the drawer of a dresser. The scene observed by the robot camera is used to estimate the dresser conguration and achieve the manipulation task.

. 1 )

 1 where p * o and ϕ * o are the 3D position and orientation of F o w.r.t. F c ; q * j ∈ R n is the n joint coordinates vector of the articulated object. The oating base pose is expressed with 6 parameters (i.e., m = 6 + n). The choice of the orientation parametrization is crucial, since F o can change substantially w.r.t. F c . We choose a practical 3D orientation representation by applying the logarithmic map to the quaternion. This allows to use a minimal representation with the benet of the unit quaternion (see more details in (60)). Let be a vector of n object features of interest; we use lines as features of interest. The 3.4. Perception of the Visual Features projection of on the image plane provides f visual lines, each one expressed in the form

Figure 3 . 2 :

 32 Figure 3.2: An exemple of selected elements for the initialization: P1, P2, P3, P4 are used to estimate the oating base; 1, 4, 8, 2, 18 are the edges pointed in the image with their id

1 and p est 2 among

 2 the j estimations allows us to compute a distance; if it is superior to a threshold, we consider that the detection is a success. Fig.3.3 illustrates the result of our proposed Hough-guided on one of the printer's edge.

Figure 3 . 3 :

 33 Figure 3.3: Hough-guided used in this work. The edge is not disturb by the moving part.

Figure 3 . 4 :

 34 Figure 3.4: Moving-edge used in[START_REF] Comport | Kinematic sets for real-time robust articulated object tracking[END_REF]. The edge remains attached to the moving part.

  are the coecients of a plane π i supporting the i-th line, computed as π i = P i p o . P i is the dual Plücker matrix associated to the line i , and p o is the estimated position of F o . Both P i and p o are available in the estimation routine as described below. J i is the 6 × m Jacobian of the object link, to compute it we need J o the object oating base Jacobian. To compute it we start by the computation of the oating base orientation ϕ o = (ϕ 1 , ϕ 2 , ϕ 3 ) T ; it is obtained by applying a log map[START_REF] Sebastian | Practical parameterization of rotations using the exponential map[END_REF] to the unitquaternion σ o = (η, ε T ) T , ε = (ε 1 , ε 2 , ε 3 ) T : ϕ o = log σ o = 2 cos -1 η |ε| ε.(3.10) From ϕ o , the rotation matrix R o from F c to F o is obtained applying the exp map to express the orientation in quaternion: σ o = exp ϕ o = cos ᾱ, α = |ϕ o | and ᾱ = α/2; then, R o is extracted from the quaternion using the well known relation. The oating base angular velocity is related to the orientation parameters derivative:

  3.6). We intentionally bad initialized the algorithm using the manual procedure. The parameters of the algorithm were set as follows: m θ = 0.035, m ρ = 10, m ROI,x = 45 and m ROI,y = 25; k = 2500. On average, the lines tracking took 5.63 ms, the estimation 12.83 ms (435 inner iterations). During the experiment the camera moves w.r.t. the object and vice-versa. Figures 3.8 and 3.9 show the plots of the cabinet position and orientation (transformed in roll-pitch-yaw angles), respectively. After an initial transient time required to recover the bad initialization, the signals provided by the VVS (blue traces with triangular markers) converge to the pose of the cabinet. The results are compared with the Aruco signals (red dashed lines). The door joint angle is plotted in Fig. 3.7: the VVS and Aruco output match quite well along all the experiment.

Figure 3 . 5 :

 35 Figure 3.5: (a), the virtual features (blue lines) converge to their real red counterparts (b), and correctly track the printer oating base and the paper tray motion (c). The algorithm copes with visual occlusions (d), and accidental failures of the lines detection: e.g., in (c) line 2 is substituted with line 8.

Figure 3 . 6 :Figure 3 . 7 :Figure 3 . 8 :

 363738 Figure 3.6: Cabinet tracked during the second experiment: (a) initial pose; (b) we opened/closed the cabinet's door, see Fig. 3.7 around 30s; (c) we moved the cabinet itself, see Figures 3.8 and 3.9 around 48s; (d) we fully closed the cabinet's door, see Fig. 3.7 after 50s

Figure 3 . 9 :Figure 3 . 10 :

 39310 Figure 3.9: Second tracking experiment: the cabinet oating base orientation.

Figures 3 .Figure 3 . 11 :

 3311 Figures 3.12 and 3.13). The robot starts from its operational conguration, standing in

Figure 3 . 12 :Figure 3 . 13 :

 312313 Figure 3.12: First manipulation experiment: time corresponds to Fig. 3.11 (a) rst opening at 12s; (b) second opening at 40s; (c) second closing at 50s; (d) third closing at 60s; (e) last opening at 72s; (f ) last closing at 80s

Figure 3 .

 3 Figure 3.14: Second manipulation experiment: the cabinet revolute joint angle.

Figure 3 .

 3 Figure 3.15: Second manipulation experiment. HRP-4 is arbitrarily placed in front of a cabinet (a). The VVS estimate is used to steer the robot hand to the door, grasp (b) and open it (c) by adding MQP cartesian/postural tasks.

4. 4 . 1

 41 Relative Pose Error (RPE)Given a xed increment ∆ ∈ N * of frames, the relative pose error evaluates the local accuracy of the trajectory over ∆ frames. A common and valid choice is to use ∆ = 1; which means it evaluates the drift per frame. Another choice is to use ∆ = Hz where Hz is the acquisition rate of the camera, therefore it is estimating the drift over one second. To be fair, we use ∆ = 1 for Theta S and ∆ = 2 for Azure and T265 cameras thus considering them at 15 Hz to compare them on the same traveled distance. The relative pose error for a given time step i is dened as:

(4. 1 )

 1 In order to have a single value representing the local accuracy of the trajectory, we compute the root mean squared error (RMSE) of the chosen operator for a sequence of length n (where m = n -∆) as: RMSE operator (E 0:n ,

  pose A and B respectively sequences of poses P A0 , ..., P An ∈ SE(3) and P B0 , ..., P Bn ∈ SE[START_REF] Joven Agravante | Walking pattern generators designed for physical collaboration[END_REF] and Figures of a trajectory between these two poses T AB0 , ..., T ABn ∈ SE(3). Then we synchronize the rst second of P A with the rst second of T AB and respectively the last second of T AB with the rst second of P B . From there we compute RM SE trans of APE for starting and ending poses. In order to remove the positioning error made during the acquisition, we also do the same computations with the ground truth data. The obtained error with the ground truth data is then subtracted from the error computed using the camera data. the cameras' data are simultaneously recorded in one-go for each acquisition with a laptop on Ubuntu 18.04 and ROS melodic. The cameras were connected to the laptop while the motion capture system, from MotionAnalysis, was running on another Windows 7 desktop computer. The Cortex software provided by MotionAnalysis allows to publish the markers data on a socket. Both computers were on the same local network linked by an Ethernet cable. It allows us to record all the data using ROS capabilities and to have a synchronized dataset between groundtruth and the various cameras we consider in the benchmark. It simplied the later association process to compare the dierent data within evo framework.

Figure 4 . 1 :

 41 Figure 4.1: 3D printed support holding cameras and markers used by the motion capture system

Figure 4 . 1 shows

 41 the support made in order to attach rigidly together every camera and markers. Then, we could record every sequence in one-go, bounding experimental biases: every camera follow the same trajectory, up to a constant rigid transformation, at the same pace and sees the same environment with the same light variations. Considered cameras with properties shown in Section 4.3 are: 1. Intel RealSense T265 stereo sheye camera. It has a very wide eld-of-view. It can be used as a sheye only or as a stereo-sheye. 2. Microsoft Azure RGB-D camera. It has the particularity to have a sheye-depth sensor. The output RGB and Depth Figures are congurable. We chose to record the wide eld-of-view (WFOV) Depth image, 2 × 2 binned aligned on RGB image. 3. Ricoh Theta S, as an equirectangular camera. It has the widest eld-of-view. It is considered as a monocular camera.

  due to the mask, shown in Fig 4.2, the FOV is estimated to be 360 • ×[165;315] • .

Figure 4 . 2 :

 42 Figure 4.2: Required mask for Ricoh Theta S camera to remove undesired part for mapping and localization. It occludes 46.2% of the image.

  recording. The setup is composed of two dierent types of Infra-Red (IR) cameras for a total of thirteen IR cameras. There are two Kestrel 2200 and eleven Kestrel Digital IR cameras sharing the same specications such as a 2048 × 1088 resolution and 332 FPS. The limited number of available cameras and their coverage volume constrain the size of the described environment in 4.5.5. The motion capture system tracks the 3D position of each

  Azure (d) Theta S with mask

Figure 4 . 3 :

 43 Figure 4.3: Cameras eld of view at pose D

Figure 4 . 4 :

 44 Figure 4.4: Top-down views of dierent done trajectories during acquisitions used to build a map

Figure 4 . 5 :

 45 Figure 4.5: Environment within the motion capture system with poses A, B, C, D and paths illustrated. Each black arrow is the looking direction of the camera for each pose.

Figure 4 .

 4 Figure 4.5 shows the environment created in a specially equipped room of 8 meters by 5 meters with the motion capture system. It emulates a simple corridor along A-B path of approximately two meters of width and ve meters of length with two separated areas. Both of them include two separate parts with dierent feature densities. For example, behind pose C in Fig 4.5 there is mainly windows and gray planes which may produce few features compared to the zone in front of pose D with several objects. While moving from A to B, the camera motions are almost limited to a straight line in order to not get too close to a wall on each side. This constrained environment implies to have rotation near pose C and B or near pose D and A.

  slow motion, avoid rotation without translation and provoke loop closure by looking at previous recorded places. With this set of constraints in mind we recorded several maps in the environment of Fig. 4.5, a subset of which is illustrated in Figure 4.4 as MapA, MapB and 4.6. Data acquisition methodologyMapC. A map is described by a couple (length(m), duration(s)) where length corresponds to the length of the trajectory that the camera did to record the sequences; we have MapA(18.59, 162), MapB(26.94, 93) and MapC(17.52, 120).

Figure 4 . 6 :

 46 Figure 4.6: Two pairs of pictures depicting two areas prior and after scenery modications.

  result, * marks the worst result.

  Numerous camera-vision-based tracking and localization algorithms work quite well using various classical cameras such as Asus Xtion Pro Live or Microsoft Kinect (see a benchmark study in[START_REF] Chappellet | Benchmarking Cameras for OpenVSLAM Indoors[END_REF]). With the recent release of the Microsoft Azure Kinect camera introduced in chapter 4, existing visual tracking, and associated datasets became obsolete as they do not apply straightforwardly; it has a wide-angle depth sensor. To our best knowledge, other existing wide-angle depth image is estimated from stereo sheye cameras (78), e.g. IntelRealSense T265[START_REF] Roxas | Real-time variational sheye stereo without rectication and undistortion[END_REF] or StereoLabs Zed 2 with a lightweight neural network for stereo matching. We propose a new fast dense tracker for large objects pose estimation while manipulated by the robot largely inspired by[START_REF] Newcombe | Kinectfusion: Real-time dense surface mapping and tracking[END_REF] and[START_REF] Whelan | Elasticfusion: Real-time dense slam and light source estimation[END_REF]. This tracker is used concurrently with robot localization based on RTAB-Map (82), a vSLAM. This interplay allows localizing the robot in its environment during locomotion and, at the same time, tracking objects (more than one) while being manipulated during locomotion. This allowed us to extend the closedloop task-space mc_rtc controller[START_REF] Bouyarmane | Quadratic programming for multirobot and task-space force control[END_REF][START_REF] Bolotnikova | Task-Space Control Interface for SoftBank Humanoid Robots and its Human-Robot Interaction Applications[END_REF] to full body loco-manipulation tasks. We have 5.2. Background performed few experiments inspired by the current industrial use-case we are investigating to assess its performance with two dierent humanoid robots. The contributions are: • Integration of planning, perception and control within one architecture • Wide-Angle depth multi-object tracking • Demonstration by experiments with dierent objects and humanoid robots This chapter is organized as follows. Section 5.2 introduces state of the art regarding the dierent element we are using. Section 5.3 explains out multi-object tracking. Section 5.4 develops the proposed architecture. Finally, Section 5.6 presents experimentation results for the multi-object tracking and the application with two dierent humanoid robots: HRP-2Kai

Figure 5 . 3 :Figure 5 . 4 :

 5354 Figure 5.3: T265 camera's image

Figure 5 . 5 :Figure 5 . 6 : 6 -

 55566 Figure 5.5: Tracking failing case during "fast" motion during manipulation

  2) D * t (u) = 0 when nothing is detected within the range of the wide-angle depth sensor, or when D * t (u) is not dened in U because the wide-angle depth-image being an ellipse in a square image. We compute the 3D object model depth-image D t in Fig. 5.6 Depth, according to the camera projection and distortion models. The 3D object model is composed of a set of vertices and triangles. In a wide-angle depth-image, a line appears like a curve as illustraded in Fig. 5.7. To match the distortion model during the projection step, the mesh of the object (a) Point cloud of a 3D CAD model not subdivided of a cube (b) Point cloud of a 3D CAD model subdivided of a cube

Figure 5 . 7 :

 57 Figure 5.7: Point cloud representation of both subdivided (b) and not subdivided (a) cube models are for a given pose. It clearly appears that the not subdivided cube model as an issue as a straight line (an edge) appears as a curve in the point cloud.

Figure 5 . 8 : 2 ( 5 . 6 )

 58256 Figure 5.8: We estimate c T vt knowing c T o t-1 to nd c T ot .

Figure 5 . 9 :

 59 Figure 5.9: Illustration of system architecture

  the multi-object tracking component providing an accurate estimate of the object and the known grasping pose on it. If the MoveObject action objective is to turn the object in place, the robot has to apply a given force on the object depending of the desired angle; its sign is automatically computed. These forces are taken into account in the robot stabilization during loco-manipulation[START_REF] Murooka | Humanoid loco-manipulations pattern generation and stabilization control[END_REF].

( a )

 a MoveObject action next motion is to move straight (b) MoveObject action next motion is to turn in place

Figure 5 . 10 :

 510 Figure 5.10: MoveObject action next motion depends on lateral and radius value. Distance value allows to check if the object reached the waypoint.

Figure 5 . 11 :

 511 Figure 5.11: Diagram of the controller with the set of actions and decisions making

Figure 5 . 12 :

 512 Figure 5.12: Wheelbarrow CAD model manual initialization by an operator; the model ts the point cloud then the tracking is started

Figure 5 .

 5 Figure 5.13: CAD models

Figure 5 .Fig. 5 .

 55 Figure 5.14: CAD models wireframe after subdivision to preserve the geometry during the step of projection in image plane

Figure 5 .

 5 Figure 5.15: Bobbin position and rotation estimation from sequence

Figure 5 .Fig. 5 .

 55 Figure 5.17: Cassette: rst and third rows are camera input RGB and wide-angle depth frames; second and fourth rows are results of object tracking RGB and wide-angle depth frames

Figure 5 . 18 :Figure 5 .

 5185 Figure 5.18: Position and rotation from sequence of B1330

Figure 5 . 20 :

 520 Figure 5.20: Position and rotation from sequence of A400M

  (a) HRP-2Kai in front of a bobbin with a control in open-loop (b) Start updating bobbin pose within the control (c) Operator manipulating the bobbin in front of HRP-2Kai (d) HRP-2Kai in front of a bobbin with a control in open-loop (e) HRP-2Kai in motion to grasp the bobbin (f ) HRP-2Kai at the end of the forward motion (g) HRP-2Kai at the end of the backward motion (h) HRP-2Kai release the bobbin and backs to standing position

Figure 5 .

 5 Figure 5.22: HRP-2Kai grasping and re-grasping the bobbin after an operator moved it

( a )Figure 5 . 23 :

 a523 Figure 5.23: View of addition to the pre-existent map depending the setup

Figure. 5 .Figure 5 . 24 :

 5524 Figure 5.24: Camera position estimation with vSLAM on same sequence with dierent mode

  (a) RGB image (b) Wide-angle depth image (c) Rectified depth image with dynamic mask

Figure 5 .Figure 5 . 26 :

 5526 Figure 5.25: (a) and (b) are inputs to multi-object tracking and (c) is the output rectied masked depth for vSLAM

Figure 5 . 27 :

 527 Figure 5.27: Results of next action to perform based on Fig. 5.10

Figure 5 .

 5 Figure 5.29 shows HRP2-Kai performing the loco-manipulation similarly to HRP-5P to bring the same bobbin to the same cassette. The same controller within the same architecture was also used with HRP-2Kai. It is not exactly true, between the rst trial with HRP2-Kai and the last one, the controller evolved to solve numerous issues that arised due to the dierences between these two robots. HRP-5P has a yaw joint in the chest that HRP2-Kai does not have. This joint allows it to move its CoM with a greater liberty, especially when it forms a closed-kinematic chain by grasping the bobbin with its two grippers. During the turning motion, the robot has to move its CoM from left to right (or right to left depending

Figure 5 . 28 :

 528 Figure 5.28: Actions during the experiment with HRP-5P

Figure 5 . 29 :

 529 Figure 5.29: Actions during the experiment with HRP-2Kai

  This chapter is based on Quadratic Programming for Multirobot and Task-

	Space Force Control by Bouyarmane Karim, Chappellet Kevin, Vaillant Joris and Kheddar
	Abderrahmane published in T-RO (IEEE Transactions on Robotics) in 2009

  a `robot' (even if it's not), we need to estimate their state as they are not equipped with sensors such as encoders, IMUS, etc. Therefore, we propose an online estimator of objects to be (or that are) manipulated by the robot based on geometrical features marker-free for articulated-robots. This estimator is formulated as a virtual visual servoing written as a quadratic program; it estimates the oating base and the articulated joint(s). It is integrated in a closed-loop with our multi-robot QP controller. We assess its capabilities through experiments with the humanoid robot HRP-4 manipulating a printer and a cabinet. This chapter is based on Interlinked Visual Tracking and Robotic Manipulation of Articulated

1 mc_rtc open-source framework available at https://jrl-umi3218.github.io/mc_rtc/ as Objects a joint work with Paolillo Antonio, Chappellet Kevin, Bolotnikova Anastasia and Kheddar Abderrahmane published in RA-L (IEEE Robotics and Automation Letters) in 2018

[START_REF] Paolillo | Interlinked Visual Tracking and Robotic Manipulation of Articulated Objects[END_REF]

.

Since the humanoid robot is a mobile platform, a camera that serves the purpose of both localisation and manipulation perception is to be selected to be embedded on the robot. In chapter 4 we identify which camera is the most suitable to meet our needs and specications with respect to localization. We propose a methodology to evaluate several cameras, with dierent modalities, in terms of reliable localization and precision for visual Simultaneous Localization and Mapping (vSLAM). Such a benchmark was not found in the literature. As vSLAM, we choose OpenVSLAM for its versatility; it natively deals with perspective, sheye, 360 cameras in a monocular or stereoscopic setup, and RGB or RGB-D modalities We recorded a dataset with ground truth and camera data in one-go in various sequences containing light variation and scenery modications in the scene and report results of our investigations. This chapter is based on the paper Benchmarking Cameras for OpenVSLAM Indoors by Chappellet Kevin, Caron Guillaume, Kanehiro Fumio, Sakurada Ken and Kheddar Abderrahmane published in ICPR (IEEE International Conference on Pattern Recognition) in 2021

  [START_REF] Bouyarmane | Multi-contact stances planning for multiple agents[END_REF], 15) a multi-contact planner that considers robots and objects as multi-robot systems was devised. It also gathers non-gaited locomotion and manipulation in a single multi-contact planning framework. However, until now we have not proposed

a controller that can deal with generated plans, nor had we experimented with common ground planning on real robots.

We propose to extend the QP control methods to encompass the idea that other objects and entities can be integrated as parts of a single controller when they interact with the robot. This work is based on Quadratic Programming for Multirobot and Task-Space Force Control in collaboration with Bouyarmane Karim, Chappellet Kevin, Vaillant Joris and Kheddar Abderrahmane published in T-RO (IEEE Transactions on Robotics) in 2009

[START_REF] Bouyarmane | Quadratic programming for multirobot and task-space force control[END_REF]

.

Table 2 .

 2 1: Admittance range for force control: (slow/safe) lower bound; fast/interactive

	(higher bound)
	Force sensor	Forces	Moments
	hands	[0.001 -0.01]	[0.1 -1.8]

feet [0.001 -0.01] [0.001 -0.01] and manipulated interactively and simultaneously (2 operators are needed) every terminal point (both hands and feet).

Table 2 .

 2

		2: List of tasks for each experiment
	Experiment	List of tasks
	Box	6D position and orientation of the Box
		2D CoM of {robot, box}
		Force tasks on both hands
	Printer	6D position and orientation of both hands before contacts
		Force task on both hands just before planned contacts

  and vision-based only SLAM, i.e. without considering other types of sensors (e.g. IMU). Furthermore, we review the state-of-the-art within the spectrum of camera types including monocular, stereoscopic, multi-camera, either passive RGB (Red Green Blue) or active RGB-D (RGB plus Depth), of conventional, panoramic and full spherical eld-of-views.

Such line is complementary to existing vSLAM surveys and benchmarks

[START_REF] Liu | Evaluation of dierent SLAM algorithms using google tangle data[END_REF][START_REF] Nardi | Introducing SLAMBench, a performance and accuracy benchmarking methodology for SLAM[END_REF][START_REF] Saeedi | Characterizing visual localization and mapping datasets[END_REF]

, mostly focusing on visual feature types, ltering or optimization methods and map structures. Table 4.1 gathers and classies a restricted number of key existing vSLAM, highlighting the most versatile ones in terms; i.e. those dealing with various eld-of-views and combinations of cameras.

Table 4 . 1 :

 41 vSLAM related works with camera-type versatility.

		References
			(
		Spherical
	Field-of-view	Hemispherical
		Conventional
	Modality	RGB Depth
	Viewpoints	Single Multiple	Monocular Stereo Dual

  , ie. the triangulation of keypoints is done from several views of which poses are estimated up to scale. As scale is not constrainted by the camera itself, contrary to Stereo or RGB-D cases, the scale can drift over time, only corrected in case of loop-closure[START_REF] Mur-Artal | ORB-SLAM: a versatile and accurate monocular SLAM system[END_REF]. Hence, maps and camera poses are estimated up to scale, contrary to Stereo or RGB-D camera types.

Table 4 .

 4

	2: Camera devices

Table 4 .

 4 3: RMSE trans of APE in meter for MapA, MapB and MapC

		MapA	MapB	MapC
	Azure	0.0977	0.4710	0.1209
	T265	0.1640	0.1694	0.1634
	Theta S	0.1907	0.2401	0.2162
	bold highlights the best result for each camera.

Table 4

 4 .5 and table 4.7 gather estimation errors shown as the mean of RMSE trans of APE and RPE, respectively written APE trans and RPE trans hereafter, for shortness. APE trans column evaluates the absolute position within the environment. RPE trans column evaluates the position drift over the trajectory. The percentage column beside APE and RPE columns quanties the percentage of the trajectory estimated by OpenVSLAM with respect to the trajectory recorded in the groundtruth. Table 4.4 gathers percentage changes with respect to Nominal case computed as: 100 * (v condition -v N ominal )/v condition where condition ∈ {Lighting, Scenery} and v ∈ {AP E trans , RP E trans , %}, the closer to 0 it is the better it is.

Table 4 .

 4 4: Mean of percentage change w.r.t Nominal case within MapA for paths

	Camera	APE trans	RPE trans	%
		Lighting
	Azure	-7.10		4.96		-11.45
	T265	-4.54	11.23	-13.16
	Theta S	-24.05	*	-15.58	*	-27.41	*
		Scenery
	Azure	2.74	*		40.41	*	-6.90	*
	T265	1.83		34.08	-5.27
	Theta S	2.63		11.59	-2.98
	bold highlights the min,	* highlights the max.

Table 4 .

 4

	Mean of RMSE trans of APE and RPE in meter and localization rate w.r.t. ground truth within MapA for stationary	Nominal Scenery Lighting	Camera	APE trans RPE trans % APE trans APE trans RPE trans % RPE trans %	Azure	T265 0.1961 0.0097 88.1868 0.1971 0.0117 43.9128	Theta S 0.3314 0.0073 98.1345 0.3278 0.0088 95.4245	Azure 0.0651 0.0042 93.8273 * 0.0590 0.1198 0.0125 * 92.2759 * 0.0049 93.9026	T265 0.1676 0.0045 * 97.8788 0.1585 0.1621 * 0.0049 98.7390 0.0044 97.8540	Theta S 0.1083 0.0045 * 96.5242 0.1020 0.1025 0.0053 94.5744 0.0054 * 91.9277 *	Azure	T265	Theta S 0.2978 0.0158 95.0270 0.2700 0.0229 67.6553	Azure	T265	Theta S 0.1601 0.0086 87.1713	bold highlights the best result, * marks the worst result.
	5:	poses	Element			A			B			C			D

Table 4 .

 4

	6: Mean of RMSE rot of APE and RPE in degree and localization rate w.r.t. ground truth within MapA for stationary	Nominal Scenery Lighting	Element Camera	APE rot RPE rot % APE rot RPE rot % RPE rot % APE rot	Azure	A T265 2.5437 0.1410 88.1868 0.1697 43.9128 2.4923	Theta S 0.4838 0.1508 98.1345 0.2094 95.4245 0.4571	Azure 1.6670 0.0886 93.8273 * 2.0301 0.1739 92.2759 * 0.1009 93.9026 * 1.8294	B T265 2.8485 * 0.1047 97.8788 3.1309 * 0.1052 98.7390 0.1062 97.8540 2.9782 *	Theta S 0.2499 0.1388 * 96.5242 0.1101 0.1811 * 94.5744 0.1891 * 91.9277 0.1544	Azure	C T265	Theta S 0.8570 0.2530 95.0270 0.3172 67.6553 0.7682	Azure	D T265	Theta S 1.3294 0.3550 87.1713	bold highlights the best result, * marks the worst result.
	poses

Table 4 .

 4

	7: Mean of RMSE trans of APE and RPE in meter and localization rate w.r.t. ground truth within MapA for paths	Nominal Lighting Scenery	Element Camera	APE trans RPE trans % APE trans RPE trans % APE trans

Table 4 .

 4

	Mean of RMSE rot of APE and RPE in degree and localization rate w.r.t. ground truth within MapA for paths	Nominal Scenery Lighting	Camera	APE rot RPE rot % APE rot RPE rot % RPE rot % APE rot	Azure 1.5535 0.1648 81.9243 * 1.8121 0.2023 70.6892 * 0.1484 63.0214 * 1.8261	T265 2.6781 * 0.1354 99.1146 2.8594 * 0.1455 91.8179 0.1269 72.2806 2.7670 *	Theta S 0.2377 1.0376 * 99.1612 0.1632 0.9642 * 98.6805 0.8961 * 71.6138 0.2083	Azure 1.7510 0.1371 69.5875 * 1.9925 0.1525 63.2116 0.1562 58.7756 * 2.0951	T265 2.9405 * 0.1225 70.0007 3.1675 * 0.1419 61.8020 * 0.1597 60.5449 3.1274 *	Theta S 0.3122 1.0817 * 90.0909 0.1642 0.8413 * 83.0214 0.8468 * 61.6005 0.3320	Azure 1.7416 0.2745 67.6783 1.6971 0.3972 69.1554 0.2605 70.5691 * 1.9001	T265 2.1847 * 0.1979 69.8565 * 2.1779 * 0.2688 72.1434 * 0.2121 70.6285 2.3227 *	Theta S 1.3812 1.6668 * 92.0478 1.4872 2.1519 * 91.4822 1.6352 * 71.0494 1.3526	bold highlights the best result, * marks the worst result.
	8:		Element			A to B			C to B			C to D

  .14. The wide-angle depth-image of the 3D object D t is obtained by projecting the rened 3D mesh localized at v T ot w.r.t the virtual camera with v T ot = c T o t-1 . Multi-scale pyramids Depth, Vertices and Normals. As D t is obtained by using the same projection and distortion model that apply to D * t , we compute similarly P Dt , P Pt and P Nt from D t . By computing and not using the available normals of the object 3D model, we are enforcing that normals of N * t and N t are oriented toward the same direction.

	From a wide-angle depth-image D * t we obtain multi-scale pyramids P D * t	, P P * t	and P N * t	in
	Fig. 5.6

Table 5 .

 5 1: Number of vertices before and after subdivision

	Object	Before	After
	bobbin	9074	575232
	cassette	343816	1031728
	wheelbarrow	16548	99188
	A400M	765027	4845171

2.3. Multi-robot QP formalism

Acknowledgements

Prof. Abderrahmane Kheddar for encouraging me to undergo my PhD and also for his patience, and his motivation to keep pushing me further; and Prof. Fumio Kanehiro for accepting me to join CNRS-

In the previous chapters we focused on improving our MQP controller framework with force control and articulated object estimator. Now, we want to choose the vision sensor that will suit our needs at best; localization in environment and object tracking. This chapter is dedicated to present our methodology and results on benchmarking dierent cameras in an indoor context.

Introduction

We place ourself in a context of closed-known-environment (i.e. indoor), where an exploration phase for a robot is not necessary. Classicaly, we use visual Simultaneous Localization and Mapping algorithms to allow a robot to locate itself in its environnment. vSLAM can be used in two phases; to buil a map, and then to retrieve its position inside this map. This pre-built map will be used for further localization only. This recently proved to be very ecient in a robotic industrial context, e.g. [START_REF] Kheddar | Humanoid robots in aircraft manufacturing: The airbus use cases[END_REF]. Some works show that the eld of view, a multiple view points, and the depth have an impact on the localization. In this work, we rather investigate for the rst time, how the localization is impacted by the choice of various cameras. We use the unied feature-based vSLAM framework OpenVSLAM [START_REF] Sumikura | OpenVSLAM: A versatile visual SLAM framework[END_REF] which allows using various cameras as input. To our best knowledge, the only work evaluating practically the impact of the camera type on vision-based 3D motion estimation concerns visual odometry (hence no relocalization) [START_REF] Zhang | Benet of large eld-of-view cameras for visual odometry[END_REF]. The latter focuses on single cameras of conventional and panoramic eld-of-views, both indoors and outdoors, and in simulated and actual environments. It is found that a small FoV is preferred for large scale scenario while a large FoV is more suited for small conned environments. Our study goes beyond the previous one by: In the previous chapters, we shows how to use force control and an estimation coming from a camera. We also performed vSLAM benchmark to choose a camera that ts as best our needs and it lead us to our main point: the loco-manipulation in an autonomous way.

Introduction

Our recent experience in aircraft humanoid manufacturing [START_REF] Kheddar | Humanoid robots in aircraft manufacturing: The airbus use cases[END_REF] revealed that in large-scale industrial settings, reliably localizing both the robot within its environment and the manipulated object w.r.t the robot is important to achieve closed-loop localization and visual servoing assemblies. This is particularly challenging when the robot moves concurrently itself and an object through manipulation.

Nowadays, visual SLAM prove to be a well-documented technology for robot localization at large (65, 66). vSLAM has very known shortcomings when the environment is populated with moving objects or when there is an object that obstructs permanently the image being processed [START_REF] Cadena | Past,present,and future of simultaneous localization and mapping: Towards the robust-perception age[END_REF]. We explain the problem we address using two examples:

(i) a humanoid robot driving a car [START_REF] Paolillo | Autonomous car driving by a humanoid robot[END_REF] (ii) a humanoid robot moving a large object through manipulation while walking (locomanipulation).

• use of depth information and 3D object's model with a wide-angle depth sensor where the main similitude is the use of wide-angle sensor; one that gives stereo intensity frames, one that gives depth frame. We need to have access to a wide-angle sensor to see our object as a whole; or at least a large part of it. We aim to give the robot the ability to manipulate object bobbin or wheelbarrow in Fig. 5.2 while going to a desired pose in the environment; the object are closed to the robot, and the robot need to look at the environment to localize itself. We will rst discuss the use of geometrical features, and why we dropped this solution, and then we will present our solution based on the new modality wide-angle depth.

(a) (b) sheye images similarly to [START_REF] Guillaume Caron | Tracking planes in omnidirectional stereovision[END_REF]. We modelized our bobbin as a cylinder with two lateral circles; g. 5.4 is the beginning of results that we got. It contains a modication of the circle the depth value to 0 in the existing wide-angle depth where the object is estimated to be.

This wide-angle masked depth is then rectied to match the RGB image intrinsic properties and then used by the vSLAM.

Controller

The controller is the heart of the architecture; it is illustraded in Fig. 5.11. Its purpose, at an high-level, is to compute the next action to perform by the robot to achieve the desired objective; to manipulate an heavy and large-scale object and to bring it at a desired position.

It uses the outputs from all the others components vSLAM and multi-object tracking to take decisions autonomously; an operator can take over the command at any moment or in case of errors. An action is pre-dened among a set of actions: Walk, WalkRelative, GraspObject, ReleaseObject or MoveObject.

While most of the actions involve the object, only Walk action does not. This action is necessary for the robot to move freely in the environment without manipulating the object.

It relies on vSLAM robot estimation to compute the next footsteps to get to the desired destination. The next footsteps are computed using the pre-dened ground in the enriched map.

This action is mainly used to walk toward the known intial object pose in the environment.

All the others actions require to track the manipulated object. The multi-object tracking is initialized once the robot ends is Walk action at the known object initial pose. As we assume the object being approximately at a its initial given pose, we know its theoritical pose w.r.t robot camera. The controller is communicating this theoretical pose to the multitracking object to initialize its tracking. From now on the manipulated object is considered as track. As a security, if a detectable tracking issue would happen if a sudden jump in the estimation happened, the robot will end and stop its motion. Then, the operator can takes over the control to resolve the issue.

The WalkRelative assumes that the object is being tracked by the multi-object tracking component. Its objective is to align the robot with the object at a given distance. As the object is estimated w.r.t the robot, it is then easy to compute the necessary footsteps to perform this action.

The GraspObject and ReleaseObject rely on the estimated object pose, object model and a pre-dened grasp conguration. Using these elements, a target pose is computed to either grasp or release the robot's gripper.

Lastly, the MoveObject is about loco-manipulation as presented in [START_REF] Murooka | Humanoid loco-manipulations pattern generation and stabilization control[END_REF]. Two dierent motions are possible: move straight with the object or turn the object in place. As we are estimating in real-time robot and object pose, before starting the motion we know the Chapter 6

Conclusion

Our loco-manipulation framework is the rst prototype of its kind, not an achievement; many improvements are possible and will have to be realized before seeing one day humanoid robots in total autonomy in the industrial sector.

In the short-term (next year), we will work on improving the large-scale object estimator by taking into account RGB images, robustifying the estimation of normals and considering the kinematics of the robot during manipulation. In a medium-term perspective (after one year), we will work on task space planning based on visual feedback to reach the movement speed of a human. In a long-term perspective, we would like to be able to use a vSLAM solution that takes full advantage of the wide-angle depth, uses objects tracking similarly as in [START_REF] Loesch | Localization of 3d objects using model-constrained slam[END_REF] to constraint camera pose estimation and allows to dynamically compute waypoints in the environment for the robot-object system.