Practical whole-body elasto-geometric calibration of a humanoid robot: Application to the TALOS robot

The whole-body elasto-geometrical calibration of humanoid robots is critical particularly for their control and accurate simulation. However, it is often not considered probably since it is a nontrivial task due to the mechanical complexity and inherent constraints of anthropomorphic structures. Also, humanoid robots have to sustain great efforts on their support legs, leading to link and joint being deformed, and are prone to auto-collision. Thus, elastic parameters have to be factored in in addition to the geometric ones and to improve the precision of the pose of all robot segments. This is much more cumbersome and time consuming than the classical calibration of serial manipulators that deals solely with the estimation of the pose of the end-effector. Finally, due to the complexity of the task, a manual intervention in several steps of the calibration is no longer possible and a thorough automation of the approach is needed. Therefore, we propose to use a stereophotogrammetric system along with embedded joint torque sensors to calibrate the pose of all robot links with a fully automatic procedure. The generation of the minimal set of optimal calibration postures is based on a new iterative optimization process that leads to a stable maximum of an observability index. Then full set of geometrical parameters but also joint and base elastic parameters were calibrated using a single least-square optimization program. The proposed method was validated on a TALOS humanoid robot allowing to obtain an accurate whole-body calibration in less than 10 minutes. The proposed approach was cross-validated experimentally and showed an average RMS error of the tracked markers of 2.2mm.


Introduction
Humanoid robots are the focus of extensive research and their field of applications keeps expanding.Companies are already offering commercial versions of these robots but mainly for research purposes.For this reason, they are constantly evolving and often modified to meet new experimental requirements.Also they are frequently dismantled, repaired and reassembled after an incident.As a result the kinematics of robot models might be slightly modified.In addition to their light design, humanoid robots are more prone to inadvertent kinematic and non-kinematic errors, thus challenging the pose estimation of every links.Kinematics errors refer to the slight 3D position and orientation variations of the nominal values between two consecutive frames [21].Nonkinematics errors in humanoid robots are related to joint flexibilities, attributed to the use of harmonic drive gears, and to link deformations observed mainly at the leg level.Link deformations can be intrinsic to the design of the robot and used to absorb impacts [2] or can be undesired due to the large applied efforts observed during single support phase for example.In humanoids it is crucial to locate accurately each link for offline planning of steps for walking or contact point locations, or for generating complex whole-body manipulation motions while avoiding auto-collision on the real robot.Of course end-effector pose is important, as in industrial robotics, but it can be effectively achieved only if the pose of the floating base is also well estimated.The floating base pose usually relies on the FKM and IMU data that are, after a static transformation from the IMU frame to the floating base one, fused together in an adaptive filter [31].If these models are incorrect, so will be the pose estimation of each link.Each link pose estimation is also important for dynamics calculation when projecting external forces as this is needed to control dynamical balance.

Related work
The literature on industrial robotics contains numerous studies about the use of geometrical calibration for reducing the influence of kinematics errors [15].It relies on using position and/or orientation measurements or constraints of the end-effector, taken over specific joint configurations called calibration postures, relatively to a frame defined at the base of the robot, to estimate parameters of a linearized error model.Besides a purely kinematics source of errors the recent development of collaborative robots, that have by design relatively flexible joints, have seen the development of new multi-modal approaches [32,37].The modalities can rely on the measured joint torques [26], the estimated ones from motor position readings [25], or the ones calculated using the dynamic model [17] to estimate elasto-geometrical parameters.However, except for a few exceptions [19], the study of deformation in serial manipulators was done at the joint level.Indeed, it is a reasonable to assume that the links of serial manipulators are rigid.This might not be the case for humanoid robot legs as very large efforts can be applied to them.Adding measurements or constraints creates redundant information on the internal state of the robot which generates a system of equations depending on the robot's posture.In order to obtain more equations than unknowns and thus make the systems robust to measurement errors, it will be necessary to position the robot in a number of measurement configurations.The use of Optimal Calibration Postures (OCP), allows for better results of the calibration process compared to random postures [16,10].It is now apparent that a consensus has emerged to use an optimization process to maximize the influence of error parameters on the measurements.The choice of the cost function is still debatable and is dependent on the experimental setup [16,34].
Measurements can be taken from a wide range of commercially available or custom-designed measurement tools such as a Laser tracker [30] or Coordinate Measuring Machines (CMM) [25] or even, with a reduced accuracy, Inertial Measurement Units (IMU) and sensitive skin [37].Laser trackers usually measure, in static condition, 3D position or 6D pose of a link, usually the end-effector.However, at anyone time, a Laser tracker can only measure information about a single link.Therefore, a Laser tracker will not be able to monitor the possible modifications of the pose of the robot base between two static postures.For a floating base system it might significantly decrease the accuracy of the measurements with respect to the robot base frame.
Optical CMM are less expensive than Laser trackers and they can measure simultaneously several marker positions while being sufficiently accurate for geometric calibration of serial manipulators [30].However, such systems have a relatively limited measurement volume, are highly sensitive to marker occlusion, as the cameras are located on the same plane, and they can only measure the 3D position of a limited number of markers.
Other measurements can be provided by Stereophotogrammetric Systems (SS) that have not been used often for robots' calibration purposes, at least in scientific papers.They measure the 3D positions of numerous retro-reflective markers in a very large volume [8].They have a much lower accuracy than Laser tracker and CMM but were shown to have a correct precision for robotics applications [28].The accuracy of SS is influenced by several factors, such as the number of cameras and their setup, measurement and calibration volume, camera resolution, dynamic or static motions, etc.Thus, there is no exact value to be provided but it has been shown that in a large workspace the use of 21 cameras yields an accuracy of approximately 200µm [1].
In humanoids the calibration process has been much less studied than in industrial robotics [23,32].Except for one study that proposes to compute the whole body geometrical parameters using SLAM [36], most research actually focused on dual-arms robot and proposed hand-eye calibration methods [7,33,3,18].Using embedded sensors drastically reduces the complexity of the experimental setup and theoretically does not require the intervention of an operator.However, to the best of our knowledge, these approaches are not very accurate as they are limited to the arms and trunk joints and were not used to estimate the full set of kinematic parameters except for a recent simulation study [33].For example, Birbach et al. [3] identified solely the rigid transformation matrices related to the camera and neck frames and joint offsets and flexibilities of the arms.Moreover, they only assessed the accuracy of the calibrated robot using camera pixel re-projection error and a throwing ball demo.Stepanova et al. [33] proposed to calibrate the full set of kinematic parameters of the arms while using the pose information of the end-effector related to the contact point formed by two arms in interaction and an embedded camera looking at the contact point.They showed that with 100 postures they could calibrate the whole set of kinematic parameters with an accuracy of 1mm for the end-effector position.However, these results were obtained in simulation only and the method seems challenging when it has to be implemented on most real humanoid robots as it requires a calibrated sensitive skin.To the best of our knowledge only a limited number of iCub prototypes are equipped with such sensitive skin.Also, the torso joints, that are crucial to positioning the arms relatively to their base frame, were supposed constant.There is no explanation on how the calibration can be performed using embedded sensors for these joints.
The crucial leg calibration was only assessed in two studies that proposed to lay down the robot to avoid dynamical balance problems.This is not possible for all humanoids and can be done only through an operator manipulation.The measurement of the pose of the foot was done either using the embedded camera and a checkerboard pattern attached to the foot, with a limited field of view due to the laying configuration, or using an industrial collaborative robot controlled in impedance so as to manipulate the humanoid foot [23].The latter approach is promising and could be used for all end-effectors but it does not ensure that the base of the robot will not move during the calibration experiment.
As humanoids feature five end-effectors, one has to reduce the number of static postures required for the calibration process.Depending on the routine, performing calibration with such complex systems, can be highly time consuming while taking up human resources, thus leading to robot downtime.Beside using OCP, time reduction can be done by collecting other measurements than only the end-effector 3D position or pose for a given posture.Interestingly, if all joints motion could be monitored separately, it would greatly simplify the calibration process.This can be done using a SS.
In this context, we propose to assess the practicability of using a classical SS and numerous retro-reflective markers located on most of the links of a humanoid robot to measure a minimal set of optimal calibration postures for performing the whole-body calibration of a large size humanoid robot.

Paper Contribution and Overview
This study proposes the following contributions: • A framework based on a stereophotogrammetric system and joint torque sensors to estimate the elasto-geometrical errors of a humanoid robot without manual intervention.
• A simple handling of whole body joints and leg links deformations to estimate accurately the floating base pose.
• An optimization process avoiding local minima to solve the large scale problem of generating Optimal Calibration Postures (OCP) for humanoids.
• An experimental validation on TALOS humanoid robot by means of standard metrics of the calibration errors and with a reduced overall calibration run-time of 10 minutes.
The paper is organized as follows.Section II describes the robot calibration model that includes a simple way of taking into account the elastic deformations.Section III describes the pragmatic generation of whole-body calibration postures using an optimization process and a modified version of the Iterative One-by-One Pose Search (IOOPS) algorithm [10].Section IV and V present the experimental setup and results obtained with a TALOS humanoid robot.Finally, the paper discusses benefits and drawbacks of the proposed method.The humanoid robot TALOS is composed of N J =30 actuated Degrees-of-Freedom (DoF) and N L =17 links as depicted in Fig. 1.The relative 3D translation and rotation between two adjacent robot links were calculated using the classical Modified Denavith-Hartenberg convention [22].It is based on four parameters describing the joint angle θ i , the offset distance d i , the link length r i , and the so-called twist angle α i .As shown in Fig. 1 some consecutive axes of rotation of the TALOS' legs are parallel.Thus, to avoid numerical instabilities in the calibration process a fifth parameter β j allowing rotational displacement along the y-axis was added to the corresponding axes [13] and set to zero otherwise.It leads to the following transformation matrix from frame i to frame i − 1:

Mechanical and Error Models
The TALOS floating base frame is located at the waist level and its pose in the global system of reference R 0 is calculated using six parameters thanks to the following transformation matrix: where 0 T Mw is the transformation matrix from the frame created from three retro-reflective markers tracked by the SS and attached to the waist in the global system of reference.
As developed in section 4, clusters of three retro-reflective markers are attached to several links of the robots.A cluster can monitor separately the motion of a single joint or of a group of joints (see Section 4).Nevertheless, each cluster is attached to a given joint frame in the model.The 3D position of the k th marker is set in its corresponding joint frame using three parameters δ Mk = [δ M dk δ M βk δ M rk ] T .To estimate the 3D position vector y (3 × N M ) of the N M considered markers in the global system of reference, the Forward Kinematics Model (FKM) of the TALOS robot was used: where q and L are the joint positions and nominal segment length vectors, respectively.δ P = [δ θi δ ri δ di δ αi δ βi ] T is the vector containing the differential variation of the geometric parameter terms that should be identified.The contribution of each small errors in the kinematic parameters Ψ θ , Ψ α , Ψ d Ψ r , and to the resultant marker position errors ∆y can be calculated efficiently thanks to the well-known generalized differential model described by Khalil et al. [20].Thus, the geometric calibration model is the following: where )) is the total generalized Jacobian matrix.Ψ T0 ((3×N M ×N S )×6) and Ψ M ((3×N M ×N S )×(3×N M )) are the generalized Jacobian matrices related to the robot's base and local marker positions.
The generalized Jacobian matrix Ψ is not full rank, i.e., some parameters are not linearly independent, as they are structurally unidentifiable.The set of identifiable parameters was determined numerically in simulation using a set of one thousand joint configurations complying with the joint limits and a QR decomposition method [12].Since there is no unique set of regrouping equations, we used the fact that the QR decomposition which allows for the independent columns of the matrix Ψ to be grouped together.Doing so, we identify in priority the parameters that can be updated in the control system [21].For example, the estimate of the local marker positions is required only during the calibration as the markers will be removed from the robot in daily use.This is why the Ψ matrix was organized by placing first the joint offsets, then parameters r i and d i , the angles α i and finally the parameters related to the robot's base matrix T 0 and the local marker poses δ M .Once the redundant parameters had been identified, their corresponding columns in Ψ were deleted.The nominal values of these parameters were used in the model.The generalized Jacobian matrix containing the N b identifiable parameters is called the base generalized Jacobian matrix and denoted Ψ b .For the six DoF of the leg and with three clusters of markers, a total of N b = 44 parameters had to be determined.It includes 17 joints parameters and 27 parameters that are related to the local marker positions.Number 17 corresponds to the 4x3 calibration parameters of the hip and ankle joints and to the calibration DH parameters related to the knee joint, as the hip and knee joint are co-linear [19].Moreover, the joint parameters related to the first hip joint and last ankle joint cannot be dissociated from the marker's local position.Thus, they were set to zero as previously explained.For the four DoF the head-trunk system with two clusters of markers, the number of identifiable parameters was N b = 30.Among these, twelve were related to the joints.For the seven DoF of the arm with six clusters of markers it was N b = 74 with twenty identifiable parameters related to the joints.

Analysis of Non-geometric Parameters
As previously stated, humanoid robots are more subject to non-geometric errors than classical industrial manipulators as they use harmonic drive gearboxes and feature links that are subject to very large mechanical stress.Several studies have proposed to model the joint deflections of serial manipulators as a linear torsional spring to account for the constant compliance of each joint [17].Usually, the same joint configurations are used to estimate the geometric parameters and the joint flexibilities solely by adding a weight or by applying external wrenches on the end-effector.The aim is to avoid making the calibration procedure too complex from an experimental point of view.However, only the static components of the additional joint torques are then taken into account [17].Few papers have investigated the complete elastic deformations of joints and links.Indeed, modeling all link deformations increases dramatically the number of parameters to be estimated and does not significantly improve the kinematics accuracy in industrial robots [19].Despite an interesting theoretical framework, these studies incur a large computational cost especially for tree structures.Additionally, the estimate of deformations during impact phases, that are often observed in humanoids, might be difficult to obtain in real-time with the filtering delay needed to arrive at a proper estimate of joint accelerations.Fortunately, the TALOS robot is equipped with joint torque sensors.Consequently, the effect of both static and dynamic loads on the joints and on the links can be monitored.For the joint of the head-arms and trunk chains elasticity was modeled as [17]: where δθ K is the vector of the small joint deflections due to the loads applied onto the joints.K θ , (N J × N J ) and τ (N J × 1) are the diagonal stiffness matrix and the vector of the measured joint torques, respectively.For the legs another approach was chosen as modeling only the joint deflection infers that the deformations are co-linear with the joint axes.This might not be the case for link deformations.Additionally, compensating for the flexibilities at the joint level will only improve one dimension of the pose estimate of the links that are located after this joint.For the TALOS legs, the effect of the 3D link deformations on the pose estimate of the robot's floating base should not be neglected.Thus, to avoid the experimental and computational burden of using a link deformation model, it is proposed to calculate δT 0 , the influence of the link deformations onto the pose of the robot's floating base.This can be done by relying on the virtual force principle and by estimating the equivalent diagonal Cartesian stiffness matrix K w (6 × 6): where J in the Jacobian matrix describing the velocity of the frame attached to the waist in the support foot frame.Consequently the total elasto-geometrical vector of error parameters that should be compensated for is defined as:

Optimal Calibration Posture (OCP) Calculation
Calibration postures should be selected to increase the influence of the parameter errors on the pose correction of the considered link(s).Usually, a large number of measurements is used since adding measurement postures to a calibration process increases its robustness [11].A humanoid robot calibration procedure should focus on improving the accuracy of the pose estimation of all its links in order to avoid autocollisions and to control the dynamic balance.Thus, the number of required calibration postures for a humanoid robot might be very large.However, it has been shown that improvement of the accuracy of the calibration process reaches a plateau after a certain number of postures [14].Thus, to minimize the number of postures required to reach this plateau and to determine OCP, a dedicated algorithm should be used [10].Even for serial manipulators, determining the minimal number of postures and the OCP while avoiding mechanical limitations is difficult.Most of the algorithms fall into local minima owing to the large scale of the problem.Usually the number of postures is set a priori.The proposed approach allows to determine a minimal set of feasible OCP.

Constraints
Obviously, calibration postures should be feasible for the robot.Thus, a set of feasible postures C feasible was defined by the following inequalities: |τ j (q)|≤ τ + j (9) (8) means that the joint angles should remain within their boundaries, τ j (q) in ( 9) is the motor torque of joint j necessary to compensate for gravity in configuration q, τ + j is the maximal torque value of the j th joint.( 10) constrains the end-effector position and orientation to remain in a subset of their workspace defined in the waist frame.One subset is defined for each sub-chain in order to reflect the area of use of the end-effector.The sub calibration workspaces of the end-effectors were roughly set as the manipulation areas in front of the robot torso for the hands and the areas corresponding to large steps for the feet.The actual values are indicated in Fig 2 .a.For the orientation of the feet and hands, 3D variations of 60deg from the nominal orientation were allowed.To avoid auto-collision, spheres were used to represent the links as shown in Fig 2 .b.The radius of each sphere was set to fit at best its corresponding link cover width plus a small tolerance.The use of simple autocollision model allows for a safety margin to be set easily and to have an efficient collision detection at a low computational cost.This was done by forcing d spheres (N c ×1), the Euclidean distance between spheres, to be positive for N c possible contacts (Inequality ( 11)).For the head-trunk chain the number of contacts that were checked was N c = 4, for the arms it was N c = 15 and for the legs N c = 16.

Decomposition of the optimization problem
A pragmatic decomposition of the optimization problem is proposed.The OCP determinations for the head, leg and arm chains starting from the waist are performed separately.This decomposition is possible thanks to the use of numerous retro-reflective markers.For the left and right arms and legs, symmetric OCP are used.Thus, in the following sections three optimization problems are solved each with a different observation matrix Ψ b corresponding to each kinematic chain.

Selection of observability index
Numerous studies have compared observability indices [15,29,5,16] mainly built from the calculation of the singular values of the observation matrix.As recalled by Nahvi and Hollerbach [29], assuming that the errors in the parameters are included in a N − d sphere of unit radius, the resulting pose errors form an ellipsoid, the axes of which are the singular values σ of Ψ b .In the presence of a relatively high level of measurement noise, maximizing the product of the singular values leads to the best calibration results [16].It is similar to maximizing the determinant of Ψ b T Ψ b [5,34], which means increasing the volume of the error ellipsoid.This is interesting for our study as the SS accuracy and precision are much lower than the ones of laser tracker.Moreover, in the proposed setup some error parameters can have a much greater influence on the measurement due to different link sizes or marker locations for example.Then, it is usually recommended to normalize the columns of the observation matrix.Fortunately, the determinant of Ψ b T Ψ b is invariant under any non-singular linear transformation [34].Thus, we retained the root of the product of the singular values as observability index [5,16,34,29]: where N q is the number of considered postures, and σ i with i = 1, ..., N b are the singular values of Ψ b .The singular values were obtained using a classical SVD decomposition.

Feasible OCP generation
Determination of a vector of OCP q * = (q * 1 , . . .q * Nq ) ∈ C Nq feasible is essential for the calibration of manipulators.OCP are generated prior to the calibration following optimization procedures that meet the constraints defined in Section 3.1.

Definitions
We define by the set of vectors of feasible postures of any length.With this definition, O 1 defined in Equation ( 12) can be considered as a mapping from F to R.
Let N be a positive integer, and q = (q 1 , • • • , q N ) be a vector of N feasible postures.We denote by the vector of postures obtained by removing the posture at rank k.In the following developments, the determination of the posture that degrades the most index O 1 when it is removed was performed.For that, we define Finally, O 1 was optimized over a limited number of postures thanks to the following optimization function.Let I be a set of positive integers not greater than N and q 0 = (q 01 , • • • , q 0N ) ∈ C N feasible a vector of N feasible postures.Function partialOptim(I, q 0 ) was defined as that that optimizes O 1 over postures of indices in I, starting from q 0 : for any j / ∈ I For example, partialOptim({1, 3}, q 0 ) = argmax q1,q3 O 1 (q 1 , q 02 , q 3 , q 04 • • • , q 0N ) 3.4.2.Classical approach and limits An important question, still open, is the determination of the number of calibration postures N q .A lower bound of N q is given by the minimal number of postures N p needed to identify the parameters.To our knowledge, there is no generic result on the upper bound for N q but it should be large enough to minimize the influence of measurement noises.However, as mentioned earlier, this bound can be established using the plateau reached by the observability criterion when N q increases [14].
To solve the problem, different approaches have been proposed [22,39,40].However, the maximization of a non-linear criterion, such as O 1 (q), is sensitive to local minima.Nowadays, it is generally accepted that iterative search algorithms should be used to solve this problem leading to the determination of a sub-optimal set, noted q ⋆ [27,9].

Algorithm 1 Function COCP[q]
Input: a set of N calibration poses q 0 Output: a set of N optimized calibration poses q Result: Improves the observability for a set of N calibration postures.q = q 0 ; k = −1 repeat 1: The method consists in fixing a set of N > N p random feasible calibration postures denoted by q 0 = (q 01 , . . ., q 0N ).Then function partialOptim is iteratively called with the set of indices being defined as the singleton containing the output of function worstIndex.The method stops when the same index is returned twice by function worstIndex (see algorithm 1).
The results obtained are generally good but can be improved.Indeed, the obtained postures generally produce a local maximum for the O 1 criterion.Methods to tackle this issue have been proposed.For example, the algorithm IOOPS uses a Tabu Search approach to force the exit of the local maximum by forbidding the stopping of the algorithm [10].This greatly improves the value of the criterion until it reaches a plateau where the maximum is close to its global value.However, this approach is purely numerical and does not take advantage of the structure of the Jacobian Ψ b .

Our proposed algorithm
We propose an alternative inspired by a geometrical observation of the structure of the Jacobian Ψ b indirectly proposed in [35], in which, the authors introduced two indices based on the orthogonality of the rows (figuring calibration postures) and columns (figuring calibration parameters) of Ψ b .
In an ideal case, where Ψ b is a square matrix, the index O 1 is maximal (volume of the ellipsoid, see subsection 3.3) when the rows and columns of Ψ b are orthogonal to each other.In this case, an additional row, provided by a new calibration pose, cannot be orthogonal to all the previous ones and will contribute little to the increase of O 1 .There is therefore a pattern that consists of a minimal number of calibration postures to identify the parameters for which the index O 1 is maximal.Because of the conditions on the feasibility of calibration postures (C feasible ), it would be difficult to obtain purely orthogonal patterns.
Block optimization: In the studied case, the over-constrained system of equations provides a rectangular Jacobian Ψ b .Ideally, a novel approach consists in constructing this matrix by adding several near-square block matrices figuring patterns of calibration postures.To do this, we use Function partialOptim to generate the near square matrix blocks (N p = 4) that we concatenate to construct the Jacobian Ψ b .The randomized initialization of the function leads to different solutions for each execution of the function.
3.5.1.Modified approach, N OCP Because 1. the number of rows brought about by each new posture (3N m ) does not allow to obtain a square matrix, 2. feasibility constraints (C feasible ) limit the orthogonality of the Jacobian rows, we use the following algorithm 2 to better assess the interactions between calibration postures.
Algorithm 2 Function q ⋆ = N OCP.Iteratively add N p random feasible postures (line 3,4), optimize O1 over those new postures only (line 5), and call Function COCP.Stop when optimization criterion reaches a plateau.

Random postures and optimization
The generation of feasible random postures (see algorithm 2) was performed using random Cartesian poses of the end-effector respecting inequalities (10).For each pose, a corresponding joint configuration was calculated using a custom QP approach.If a collision was detected the shoulder or hip joints was slightly moved iteratively to get to the next feasible random joint configuration.The proposed non-linear constrained optimization problem was solved using the classical interior-point method [6].

Generation of stable trajectories
Once all the exciting postures were obtained for each kinematic chain, they were sorted out to minimize the Euclidean distance in joint space.Joint trajectories corresponding to the joints of the investigated kinematic chain were interpolated over a fixed time of 5s between two consecutive OCP using 5 th order B-Splines.This allowed us to impose zero condition for initial and final velocity and acceleration thus reducing the robot's oscillations.Five way point values were optimized to minimize the joint velocity while avoiding autocollision.Finally, to avoid any manual intervention during the calibration process, dynamical balance was ensured using a last optimization process on the leg joints that were not being calibrated.Depending on the chain being calibrated the dynamical balance was ensured by either one or two legs.To do so, for each time sample, the deviation of the ZMP with the center of the base of support ZMP 0 was minimized by determining either 6 or 12 stabilizing leg joint angles q S : Find q * S = argmin where α = 10 −3 is a regularisation term.q+ is the vector containing the maximal joint velocities.The constraint µ FX,Y ≤ FZ , with µ = 0.5, guarantees that the friction forces are acting within the friction cone.F X , F Y and F Z being the estimate of the external wrenches under each foot [4].

Experimental Setup
The proposed method was used to calibrate the humanoid robot TALOS shown in Fig. 1 (1.75m, 90kg, PAL Robotics, Barcelona, Spain).It can measure joint positions thanks to 19bits joint encoders.The red cylinders in Fig. 1 indicate the location of the 24 joints equipped with a torque sensor.A SS (20 Miqus M3 cameras, Qualisys) consisting of twenty cameras was used to collect the 3D positions of 60 retro-reflective markers located on the whole structure of the TALOS robot.The complete experimental setup is shown in Fig. 3.a.The longest part of the experiment was positioning the cameras and internal parameters setting.Camera specific gains/thresholds had to be tuned manually to enhance the robustness of marker identification.The TALOS covers reflected the infra-red beams from the cameras as shown in Fig. 3.b.
Depending on the geometry of the robot's cover, markers were located in clusters of three either directly taped onto the robot's cover or using a custom made rigid cluster.The rigid clusters were used for practical purposes to speed up preparation time.They were located on each possible cover to monitor independently as many joints as possible.Double sided tape was used to firmly attach the clusters to the covers and prevented wobbling.No special attention was paid, beside avoiding collision of markers with adjacent links, to locate the markers on the covers.Note that the marker's local positions were identified during the calibration process.Marker positioning took less than five minutes.
As illustrated in Fig. 3.a, 3 clusters were located at the head, trunk and waist, 6 on each arm and 9 on each leg.For the arm, the shoulder, elbow and one DoF of the wrist, joints were fully monitored and one cluster was attached to two wrist DoF.For the leg it was not possible to monitor separately the action of each joint except for the knee joints.For the head-trunk chain, a cluster monitored the trunk joints and another was attached to the head.

Identification Process
The final step in the calibration consists in identifying error parameters.The objective is to identify the model error parameter δ (see eq.7) that minimizes the least-square difference between the vectors corresponding to the estimated y and measured ŷ marker 3D positions.The measurement vector ŷ of dimension 3 × N M ×N S was obtained using all the samples N S of the 3D position measurements of the markers in the global system of reference.For the leg identification, a special constraint has to take into account the fact that the position of the foot supporting the robot weight should remain constant over all the measurements.The following identification problem was then solved: where F(q, δ) ∈ SE(3) is the pose of the support foot when the robot is in configuration q with parameter δ.

Elaboration of experimental data
Raw position data from the SS were low-pass filtered at 5Hz (zero-phase lag 5 th order Butterworth filter) as shown in Fig. 4. Better identification results can be obtained with several samples of the same calibration postures [38].Since the SS was sampled at 100Hz, it should be possible to use hundreds of samples for the same posture.Only those exhibiting the most stable marker positions were selected using a thresholding method based on the magnitude of each marker velocity.To avoid weight bias in the identification process, the same number of samples for all markers of a kinematic chain was selected using the thresholding method.For both legs, the number of selected stable samples were N s = 144 and N s = 168, respectively.For the left and right arms, it was N s = 256 and for the head-trunk chain, it was N s = 72, respectively.Also, owing to the tree kinematic structure of the robot and to the fact that the error parameters are of different types, i.e. geometric or elastic, the measured marker positions obtained during different experiments should be concatenated.Specifically, the arm and head poses are due to the trunk joint configurations.Thus, the marker data of the three experiments related to the head-trunk, left arm and right arm were concatenated in the same vector of marker positions ŷ of dimension 3 × 328.
For leg trials, when one leg is performing OCP, the opposite leg is naturally supporting the robot weight and handling balance.This will apply very large efforts onto the leg joints.This is interesting particularly if one intends to identify the joint and link flexibilities.Thus, we concatenated marker data corresponding to the leg OCP and to the support leg.It boils down to a vector of marker positions ŷ of dimension 3 × 288 for the left leg and of dimension 3 × 336 for the right leg.

Right arm OCP
Head-Trunk OCP Left leg OCP 6. Results

Results of OCP calculation
The calculated OCP are shown in the support video and in Fig. 5.The total number of OCP was empirically determined, as described in Fig. 6, by analyzing the evolution of O 1 with respect to the number of postures.From this figure, it is clear that O 1 value does not increase after 24 postures.Thus, the number of OCP used for leg calibration was set to N q = 24.Based on similar analysis the total number of postures was set to N q = 8 for the head-trunk chain, and N q = 32 for the arms.
Fig. 6 also shows a comparison of O 1 evolution when using the proposed modified version of the IOOPS algorithm and when iteratively applying algorithm 1 as classically done in the literature.Clearly, modified IOOPS leads to better results avoiding local minima which in turn allows to reduce the number of calibration postures required.

Robot repeatability assessment
Prior to performing the calibration of the TALOS robot it is important to assess repeatability as it will define, with the accuracy of the measurement system, the accuracy value that the calibration process can achieve.To do so,  five repetitive motions of the legs and of the arms that are representative of walking and reaching tasks were carried out.Ten samples corresponding to the marker positions of the end-effector and of the waist in the initial and final static postures were averaged.The Euclidean distance between waist and endeffector marker clusters was calculated for each static posture.For the legs, the distance variation between waist cluster and feet was 0.066±0.040mm.For the arms it was 0.041±0.016mm.These values are much smaller than the SS accuracy.Thus, the robot repeatability should not affect the calibration results.

Robot calibration results
The marker tracking residue, obtained during calibration, was relatively low with an average RMSE of 1.5±0.2mm.Specifically, it was 1.3mm for the headarms-trunk and 1.7mm and 1.5mm for the left and right legs, respectively.The identifiable and identified joint parameters are given in Table I.
For clarity, parameters related to local marker positions are not displayed.One can see that most of the geometric parameters are small except for the head and wrist joint offsets.This can be accounted for by the fact that the robot manufacturer does not provide a calibrated version of its robot as is the case with corporations producing industrial serial manipulators.
For the torso and arms, the identified joint stiffness was very large.For the legs, Cartesian stiffness values were diag(K wr ) = [320 120 450 7.2e 4 2.1e4 5.0e 5 ] T Table 1: Identified elasto-geometrical parameters of the joint parameters described in Fig. 1.As expected these values demonstrate that most of the waist deviation appears in the frontal and sagittal planes of the robot were most of the efforts are exerted.Cartesian stiffnesses may seem large.However, when multiplied by the very large efforts expressed in the Cartesian space they result in relatively large displacements.As shown in section VII.4, the waist can be inaccurately positioned by values as high as one centimeter.
Former calibration Current calibration (Table I)

Comparison between right arm calibrations performed at different times
Fig. 7 exemplifies the need for humanoid robots to be regularly calibrated.It shows the results of joint offsets of the right arm estimated from the calibration procedure performed at different times with the proposed method.The so-called former-calibration was performed before the robot was returned to the manufacturer to be updated and re-calibrated following an harmonic drive malfunction.Of course, the first shoulder joint and the wrist joints display very large offsets that were visually observable on the robot.

Robot cross validation results
Cross validation experiments were performed with slow sinusoidal motions at a frequency of 0.5Hz spanning most of the range of motion of each joint.Beside auto-collision and dynamic balance, the end-effector of the legs and arms were not constrained to remain within the calibration workspaces shown in Fig. 2. On the other hand, a series of 20 static random joint postures not used for the calibration process was also used for cross validation.The latter postures comply with the calibration workspaces shown in Fig. 2. Cross validation was also performed with the motions used for the repeatability test performed with the right arm and leg.For each cross validation, the comparison was made with the fully calibrated model, using only the geometrical parameters (without joint and link flexibilities) and with the non calibrated model using only the estimated local marker positions.Table II lists the results of the estimated marker tracking errors for the different cases and chains considered.Head, Arms and Trunk (HAT) errors were reported together as the model base is located at the waist and thus the trunk joints affect the overall accuracy of the head and arms positioning.With respect to all trials the marker tracking average RMSE was 2.2mm for the calibrated model and 7.1mm when the model was not calibrated.The calibration allowed to reduce by 3 the RMSE.A typical representation of the tracking error, when playing separately low frequency sine wave motions, on each joint of the head arms and truck chains, is visible in Fig. 8.a.The motions spanned each joint range of motion, and the zoomed image shows that the calibrated model allows for a much better tracking of the markers specially when the joint reaches its limits.Fig. 8b.shows a similar analysis performed with the right arm only and with static random poses.Note that random postures are generated so that the robot's wrist remains within the calibration workspace shown in Fig. 2. Fig. 8c shows a typical RMSE error distribution for each marker of the right leg during the repeatability motion.One can see on this trial that the RMSE was reduced dramatically for all considered markers after calibration.When no calibration was used some of the RMSE were very large with values up to 10mm.Moreover, the error is distributed over all markers.This highlights the importance of calibrating accurately all link 3D poses to avoid auto-collisions and not only the end-effectors.The RMSE was larger for the sine wave motions.The fact that these trials are continuous motions does not account for the larger RMSE as they were relatively slow.The larger RMSE can likely be explained by the fact that these motions span most of the joint range and thus markers and links can be far away from the calibration workspaces.When no elastic or calibration parameters are used larger RMSE were observed at the leg level.This was expected as the link flexibilities largely impact the pose of the floating base of the robot.A final test was conducted to show the importance of compensating for the elastic deformations.When only the joint geometrical parameters were used in the model the average RMSE for the tracking of the leg markers of all the investigated motions was 4.8mm.It was 2.7mm with the compensation for the elasto-geometrical parameters.

Conclusion
In this paper a new automated and practical method for estimating the whole-body elasto-geometrical parameters of a large-size humanoid robot from a stereophotogrammetric system and joint torque sensors has been proposed.Stereophotogrammetric systems are relatively common in laboratories and joint torque sensors are increasingly embedded in the new humanoid robots.Consequently the proposed method could be routinely used to calibrate humanoid robots.The contribution given by elasto-geometrical and purely geometric models were compared and validated using cross validation trials.Static and slow motions positioning accuracy was improved for each link.Better results were obtained at the leg level when Cartesian stiffnesses were used.In contrast, joint flexibilities did not show improvements for tracking arm markers.Compensation for flexibilities was satisfactory when using a simple linear model.A more realistic modeling using beam geometry [21] for example, could have produced better results.However, these models incur a higher computational cost and are 0.9  more difficult to implement in the control scheme of the robot.Another possible solution could have been to monitor directly the deformation using constraint gauges directly mounted on the parts subject to the largest efforts.However, this would require more cablings and electronics for the robot.During cross experimental validation the average marker tracking error was reduced by a factor 3 between the fully calibrated and non-calibrated robot.Practical decomposition of the problem, addressing the generation of optimal calibration postures of each kinematic chain separately, beside reducing the scale of the optimization problem allows to handle dynamical balance.Thus, the proposed calibration method does not require any manual intervention onto the robot once it has started.To the best of our knowledge this paper is the first to address the complete calibration process of a humanoid robot.We believe that the proposed method can be used for any humanoid robot equipped with joint torque sensors.The proposed method chooses to have the robot standing on its feet but if one had a smaller, more robust, robot it is possible to have the robot laying down or crawling.This would alleviate the burden of having to control dynamical balance and could help in the early stages of the model identification.In case the links and joints are perfectly rigid, the estimated stiffness parameters would simply tend toward infinity.Thus, they will not have any influence on the kinematics.This was exemplified in Table II as the use of elastic parameters for the joints of the head-arms-torso sub-chains did not improve the tracking of the considered markers.However, since the proposed elastic deformation compensation involves joint torque sensors a very dynamic motion might overcompensate for the 3D waist pose estimation.
For OCP generation, we proposed an original numerical algorithm that identifies sets of robot configurations that form calibration patterns.This approach is based on iterative block optimization; each block consisting of a minimal number of postures required to identify the parameters.As shown in Fig. 6, this circumvents local minima and a better optimum is obtained.The good results observed confirm the interest of looking for calibration patterns and rather than isolated configurations [24].Moreover, we observed a localization of these postures at the limit of the workspace, see support video, which confirms previous works [10], in that the excitation of the parameters on the whole workspace provided a better global performance of the robot.Nevertheless, our approach allows, via the constraints on the search space, to limit the calibration to a subspace so as to locally improve the robot performance.Note that criterion 0 1 was chosen from the literature for being invariant under any non-singular linear transformation but the proposed method should give similar results with a different criterion.
For a calibration routine, it could be interesting to always locate the marker clusters in the same exact location.This could reduce the setup time, while remaining robust to occlusions and allowing for tracking possible errors.We believe that new manufactured humanoid robots should include calibration landmarks easily accessible for this purpose.
It is always better to use OCP to improve accuracy and help minimise the experimental time.However, in case one would not like to redevelop the proposed algorithm, we believe that thanks to the large number of available measurements from the stereophotogrammetric system any large set of random feasible postures should provide a correct estimate of the geometrical parameters.The criterion 0 1 can be used to assess the quality of the randomly generated calibration postures.
In the future, we will also extend the proposed calibration method to determine the transformation matrix from the floating base to the IMU [37].Indeed, the latter is used to estimate the robot's state together with the FKM.The results of the calibrated model will also be used for gait pattern generation and whole-body control of the TALOS robot.

Figure 1 :
Figure 1: Mechanical model of the humanoid robot TALOS composed of 30 actuated DoF among which 24 are provided with a joint torque sensor.

Figure 2 :
Figure 2: (a) View of the calibration workspaces of the arms and legs.(b) Position of the 28 spheres used to avoid auto-collisions.

Figure 3 :
Figure 3: (a) Experimental setup and retro-reflective marker locations and (b) infra-red view of a SS camera.
marker vertical position [mm]Stable sample Raw SS dataFiltered SS data

Figure 4 :
Figure 4: Marker data filering and example of stable samples selection.

Figure 5 :
Figure 5: Representative OCP played onto the TALOS robot and simultaneous motion capture measurements.

Figure 6 :
Figure 6: Comparison of the evolution of O 1 criterion for the leg when OCP were calculated using the proposed modified IOOPS (black) relative to a conventional classical approach (gray).

Figure 7 :
Figure 7: Comparison of the estimated joint offsets of the right arm with the proposed calibration method performed at different times.

Figure 8 :
Figure 8: Marker tracking during cross validation experiments with and without calibrated parameters.(a) HAT system's markers estimation during a sinusoidal motion at 0.5Hz.(b) Right arm's markers estimation during random postures generated to be within the calibration workspace.(c) RMSE distribution of marker tracking for the right leg.

Table 2 :
Results of the cross validation obtained with Sinusoidal Motions (SW), Random Postures (RP) and Repeatability Motion (RM) performed with the Full Calibration Model (FCM) , the Geometrically Calibrated Model (GCM) and with the Non Calibrated Model (NCM) using the identified parameters.