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

Vincent Bonnet, Joseph Mirabel, David Daney, Florent Lamiraux, Maxime Gautier, Olivier Stasse

<figure><img src="image_1.png" /></figure>

## To cite this version:

Vincent Bonnet, Joseph Mirabel, David Daney, Florent Lamiraux, Maxime Gautier, et al. *Practical whole-body elasto-geometric calibration of a humanoid robot: Application to the TALOS robot*. Robotics and Autonomous Systems, 2023, 164, pp.104365. 10.1016/j.robot.2023.104365. hal-04222944

HAL Id: hal-04222944

https://hal.science/hal-04222944v1

Submitted on 29 Sep 2023

HAL is a multi-disciplinary open access archive for the deposit and dissemination of scientific research documents, whether they are published or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

<figure><img src="image_2.png" /></figure>
Practical Whole-Body Elasto-Geometric Calibration of
a Humanoid Robot: Application to the TALOS Robot.

Vincent Bonnet<sup>a,*</sup>, Joseph Mirabel<sup>a</sup>, David Daney<sup>b</sup>, Florent Lamiraux<sup>b</sup>,
Maxime Gautier<sup>c</sup>, Olivier Stasse<sup>a</sup>

<sup>a</sup>LAAS-CNRS, 7 Av. du Colonel Roche, 31400 Toulouse, France

<sup>b</sup>INRIA-Bordeaux, 200 Av. de la Vieille Tour, 33405 Talence, France

<sup>c</sup>LS2N, 1 rue de la Noë, Nantes, France

**Abstract**

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.

**Keywords:** Calibration, Humanoid Robot, Elasto-geometric calibration, Stereophotogrammetric system.

<footnote>*Corresponding author</footnote>

<footnote>Email address: vincent.bonnet@laas.fr (Vincent Bonnet)</footnote>
# 1. 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]. Non-kinematics 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.

## 1.1. 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.

## 1.2. 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.

## 2. Mechanical and Error Models

### 2.1. Geometrical Model

<figure><img src="image_3.png" /><figcaption>Figure 1: Mechanical model of the humanoid robot TALOS composed of 30 actuated DoF among which 24 are provided with a joint torque sensor.</figcaption></figure>

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 $\theta_i$, the offset distance $d_i$, the link length $r_i$, and the so-called twist angle $\alpha_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 $\beta_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:

$$i^{-1}\mathbf{T}_i = \text{Roty}(\beta_i)\text{Rotx}(\alpha_i)\text{Trx}(d_i)\text{Rotz}(\theta_i)\text{Trz}(r_i) \quad (1)$$

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:

$$\begin{gathered} ^0\mathbf{T}_w = ^0\mathbf{T}_{M_w} \text{Rotz}(\gamma_0) \text{Trz}(b_0) \text{Rotx}(\alpha_0) \text{Trx}(d_0) \\ \text{Rotz}(\theta_0) \text{Trz}, d_0 \end{gathered} \quad (2)$$

where $^0\mathbf{T}_{M_w}$ 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 $\delta_{Mk} = [\delta_{Mdk} \delta_{M\beta k} \delta_{Mrk}]^T$. To estimate the 3D position vector $\mathbf{y}$ ($3 \times 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:

$$\mathbf{y} = \text{FKM}(\mathbf{q}, \mathbf{L}, \delta_{\mathbf{P}}, ^0\mathbf{T}_w, \delta_{\mathbf{M}}) \quad (3)$$

where $\mathbf{q}$ and $\mathbf{L}$ are the joint positions and nominal segment length vectors, respectively. $\delta_{\mathbf{P}} = [\delta_{\theta_i} \delta_{r_i} \delta_{d_i} \delta_{\alpha_i} \delta_{\beta_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 $\Psi_{\theta}$, $\Psi_{\alpha}$, $\Psi_{d}$, $\Psi_{r}$, and $\Psi_{\beta}$ (($3 \times N_M \times N_S) \times N_J$)) to the resultant marker position errors $\Delta\mathbf{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:

$$\begin{gathered} \Delta\mathbf{y} = \Psi\delta_{\mathbf{g}} \\ \text{where } \delta_{\mathbf{g}} = [\delta_{\mathbf{P}} \delta_{\mathbf{T}_0} \delta_{\mathbf{M}}]^T \\ \Psi = [\Psi_{\theta} \Psi_{\alpha} \Psi_{d} \Psi_{r} \Psi_{\beta} \Psi_{\mathbf{T}_0} \Psi_{\mathbf{M}}] \end{gathered} \quad (4)$$

where $\Psi$ (($3 \times N_M \times N_S) \times (N_J \times 5)$) is the total generalized Jacobian matrix. $\Psi_{\mathbf{T}_0}$ (($3 \times N_M \times N_S) \times 6$) and $\Psi_{\mathbf{M}}$ (($3 \times N_M \times N_S) \times (3 \times N_M)$) are the generalized Jacobian matrices related to the robot's base and local marker positions.
The generalized Jacobian matrix $\mathbf{\Psi}$ 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 $\mathbf{\Psi}$ 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 $\mathbf{\Psi}$ matrix was organized by placing first the joint offsets, then parameters $r_i$ and $d_i$, the angles $\alpha_i$ and finally the parameters related to the robot's base matrix $\mathbf{T}_0$ and the local marker poses $\mathbf{\delta}_M$. Once the redundant parameters had been identified, their corresponding columns in $\mathbf{\Psi}$ 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 $\mathbf{\Psi}_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.

## 2.2. 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]:

$$ \delta_{\theta_K} = K_{\theta}^{-1} \tau \qquad (5) $$

where $\delta\theta_K$ is the vector of the small joint deflections due to the loads applied onto the joints. $K_\theta$, $(N_J \times N_J)$ and $\tau$ $(N_J \times 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 $\delta 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 \times 6)$:

$$ \delta_{T_0} = K_w^{-1} (J^T)^{-1} \tau \qquad (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:

$$ \delta = [\delta_g \ diag(K_\theta) \ diag(K_w)]^T \qquad (7) $$

### 3. 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.

### 3.1. Constraints

Obviously, calibration postures should be feasible for the robot. Thus, a set of feasible postures $C_{feasible}$ was defined by the following inequalities:

$$q_j^- \le q_j \le q_j^+, \quad \text{with } j = 1, \dots, N_J \qquad (8)$$

$$|\tau_j(\mathbf{q})| \le \tau_j^+ \qquad (9)$$

$$\mathbf{P}_E^- \le \mathbf{P}_E(\mathbf{q}) \le \mathbf{P}_E^+ \qquad (10)$$

$$\mathbf{0} \le \mathbf{d}_{\text{spheres}}(\mathbf{q}) \qquad (11)$$

(8) means that the joint angles should remain within their boundaries, $\tau_j(\mathbf{q})$ in (9) is the motor torque of joint j necessary to compensate for gravity in configuration $\mathbf{q}$, $\tau_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.

<figure><img src="image_4.png" /><figcaption>Figure 2: (a) View of the calibration workspaces of the arms and legs. (b) Position of the 28 spheres used to avoid auto-collisions.</figcaption></figure>

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 \times 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$.

## 3.2. 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 $\Psi_b$ corresponding to each kinematic chain.

## 3.3. 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 $\sigma$ of $\Psi_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 $\Psi_b^T \Psi_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 $\Psi_b^T \Psi_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]:

$$O_1 = \frac{\sqrt[N_q]{\sigma_1 \sigma_2 \dots \sigma_{N_b}}}{\sqrt{(N_q)}} \quad (12)$$

where $N_q$ is the number of considered postures, and $\sigma_i$ with $i = 1, \dots, N_b$ are the singular values of $\Psi_b$. The singular values were obtained using a classical SVD decomposition.

## 3.4. Feasible OCP generation

Determination of a vector of OCP $q^* = (q_1^*, \dots, q_{N_q}^*) \in C_{feasible}^{N_q}$ 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.
### 3.4.1. Definitions

We define by

$$ \mathcal{F} = \bigcup_{i=1}^{\infty} C_{\text{feasible}}^{i} $$

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 $\mathcal{F}$ to $\mathbb{R}$.

Let $N$ be a positive integer, and $\mathbf{q} = (q_1, \cdots, q_N)$ be a vector of $N$ feasible postures. We denote by

$$ \mathbf{q}_{k}^{-} = (q_{1}, \cdots, q_{k-1}, q_{k+1}, \cdots, q_{N}) $$

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

$$ k = \text{worstIndex}(\mathbf{q}) = \underset{k \in \{1, \cdots, N\}}{\operatorname{argmin}} O_1(\mathbf{q}_k^{-}). $$

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 $\mathbf{q}_0 = (q_{01}, \cdots, q_{0N}) \in C_{\text{feasible}}^N$ a vector of $N$ feasible postures. Function $\text{partialOptim}(I, \mathbf{q}_0)$ was defined as that that optimizes $O_1$ over postures of indices in $I$, starting from $\mathbf{q}_0$:

$$ \begin{array}{l} \text{partialOptim}(I, \mathbf{q}_0) = \underset{q_j = q_{0j} \quad \text{for any } j \notin I}{\operatorname{argmax}}_{q_i, i \in I} O_1(q_1, \cdots, q_N) \\ q_j = q_{0j} \quad \text{for any } j \notin I \end{array} $$

For example,

$$ \text{partialOptim}(\{1, 3\}, \mathbf{q}_0) = \underset{q_1, q_3}{\operatorname{argmax}} O_1(q_1, q_{02}, q_3, q_{04}, \cdots, 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(\mathbf{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 $\mathbf{q}^*$ [27, 9].
**Algorithm 1** Function $\mathcal{C}OCP[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: $k_{prev} = k$

2: $k = \text{worstIndex}(q)$

3: $q = \text{partialOptim}(\{k\}, q)$

until $k_{prev} == k$;

return $q$

The method consists in fixing a set of $N > N_p$ random feasible calibration postures denoted by $q_0 = (q_{01}, \dots, 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 $\Psi_b$.

### 3.5. *Our proposed algorithm*

We propose an alternative inspired by a geometrical observation of the structure of the Jacobian $\Psi_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 $\Psi_b$.

In an ideal case, where $\Psi_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 $\Psi_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 $\Psi_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 $\Psi_b$. The randomized initialization of the function leads to different solutions for each execution of the function.

### 3.5.1. Modified approach, NOCP

Because

1. the number of rows brought about by each new posture ($3N_p$) 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 $\mathbf{q}^* = \mathcal{NOCP}$. Iteratively add $N_p$ random feasible postures (line 3,4), optimize $O1$ over those new postures only (line 5), and call Function $\mathcal{C}OCP$. Stop when optimization criterion reaches a plateau.

**Result:** a set of $(k+1)N_p$ ($N_p = 4$) calibration postures $\mathbf{q}^*$

0: $k = 0$

1: $\mathbf{q}_0 \in \mathbb{R}^0$ (empty vector)

2: **Repeat**

3: $\mathbf{r}$ = vector of $N_p$ feasible postures

4: $\mathbf{q}_{k+1} = (\mathbf{q}_k, \mathbf{r})$

5: $\mathbf{q}_{k+1} = \text{partialOptim}(\{kN_p + 1, \dots, (k+1)N_p\}, \mathbf{q}_k)$

6: $\mathbf{q}_{k+1} = \mathcal{C}OCP(\mathbf{q}_{k+1})$

7: $k = k + 1$

**until** $|O_1(\mathbf{q}_{k-1}) - O_1(\mathbf{q}_k)| < \epsilon$

### 3.5.2. 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].

## 3.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 autocollission. 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₀ was minimized by determining either 6 or 12 stabilizing leg joint angles qS:

$$ \begin{array}{ll} \text{Find } \mathbf{q}_S^* = \underset{\mathbf{q}_S \in \mathbb{R}^{N_J}}{\operatorname{argmin}} & \| \mathbf{ZMP} - \mathbf{ZMP}_0 \|_2^2 + \alpha \| \dot{\mathbf{q}} \|^2 \\ \text{subject to} & \mathbf{q}^- \le \mathbf{q}_S \le \mathbf{q}^+ \\ & |\dot{\mathbf{q}}| \le \dot{\mathbf{q}}^+ \\ & |\mathbf{\Gamma}| \le \mathbf{\Gamma}^+ \\ & \mu F_{X,Y} \le F_Z \\ & 0 \le \mathbf{d}_{vertex} \end{array} \qquad (13) $$

where $\alpha = 10^{-3}$ is a regularisation term. $\dot{\mathbf{q}}^+$ is the vector containing the maximal joint velocities. The constraint $\mu \bar{F}_{X,Y} \le \bar{F}_Z$, with $\mu = 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].

## 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 Miqu 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.

<figure><img src="image_5.png" /><figcaption>Figure 3: (a) Experimental setup and retro-reflective marker locations and (b) infra-red view of a SS camera.</figcaption></figure>

## 5. Identification Process

The final step in the calibration consists in identifying error parameters. The objective is to identify the model error parameter $\delta$ (see eq.7) that minimizes the least-square difference between the vectors corresponding to the estimated $\mathbf{y}$ and measured $\hat{\mathbf{y}}$ marker 3D positions. The measurement vector $\hat{\mathbf{y}}$ of dimension $3 \times N_M \times 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:

$$ \begin{array}{ll} \text{Find } \delta^* = \min_{\delta \in \mathbb{R}^{N_b}} & \| \hat{\mathbf{y}} - \mathbf{y}(\delta) \|^2 \\ \text{subject to } & \mathbf{F}(\mathbf{q}_i, \delta) = \mathbf{F}(\mathbf{q}_1, \delta) \quad \forall i \in \{2, \dots, N_S\} \end{array} \tag{14} $$

where $\mathbf{F}(\mathbf{q}, \delta) \in SE(3)$ is the pose of the support foot when the robot is in configuration $\mathbf{q}$ with parameter $\delta$.
5.1. Elaboration of experimental data

Raw position data from the SS were low-pass filtered at 5Hz (zero-phase lag
5th 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 Ns = 144
and Ns = 168, respectively. For the left and right arms, it was Ns = 256 and
for the head-trunk chain, it was Ns = 72, respectively.

<figure><img src="image_6.png" /><figcaption>Figure 4: Marker data filtering and example of stable samples selection.</figcaption></figure>

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 mea-
sured marker positions obtained during different experiments should be con-
catenated. 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.

<figure><img src="image_7.png" /><figcaption>Figure 5: Representative OCP played onto the TALOS robot and simultaneous motion capture measurements.</figcaption></figure>

**6. Results**

6.1. *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.

6.1.1. *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,
<figure><img src="image_8.png" /><figcaption>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).</figcaption></figure>

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 end-effector 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.

### 6.1.2. 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 head-arms-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.1e^4\ 5.0e^5]^T$
<table><caption>Table 1: Identified elasto-geometrical parameters of the joint parameters described in Fig.1.</caption><thead><tr><th>Joint name</th><th>δ<sub>θ</sub><br>[deg]</th><th>δ<sub>d</sub><br>[mm]</th><th>δ<sub>r</sub><br>[deg]</th><th>δ<sub>α</sub><br>[mm]</th><th>δ<sub>β</sub><br>[deg]</th><th>K<sub>θ</sub><br>[N.m/rad]</th></tr></thead><tbody><tr><td>LLEG<sub>1</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>LLEG<sub>2</sub></td><td>-0.09</td><td>-0.00</td><td>-0.47</td><td>0.00</td><td>-</td><td>-</td></tr><tr><td>LLEG<sub>3</sub></td><td>0.02</td><td>-1.48</td><td>0.56</td><td>-0.21</td><td>-</td><td>-</td></tr><tr><td>LLEG<sub>4</sub></td><td>0.12</td><td>-3.33</td><td>0.56</td><td>-0.14</td><td>0.00</td><td>-</td></tr><tr><td>LLEG<sub>5</sub></td><td>-1.09</td><td>0.99</td><td>0.55</td><td>-0.23</td><td>-</td><td>-</td></tr><tr><td>LLEG<sub>6</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>RLEG<sub>1</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>RLEG<sub>2</sub></td><td>0.00</td><td>-0.03</td><td>0.5</td><td>0.00</td><td>-</td><td>-</td></tr><tr><td>RLEG<sub>3</sub></td><td>-0.05</td><td>-0.98</td><td>-0.68</td><td>0.50</td><td>-</td><td>-</td></tr><tr><td>RLEG<sub>4</sub></td><td>-0.12</td><td>-2.45</td><td>-0.73</td><td>0.41</td><td>0.01</td><td>-</td></tr><tr><td>RLEG<sub>5</sub></td><td>-1.19</td><td>0.64</td><td>-0.72</td><td>0.13</td><td>-</td><td>-</td></tr><tr><td>RLEG<sub>6</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>TORSO<sub>1</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>0.50e<sup>5</sup></td></tr><tr><td>TORSO<sub>2</sub></td><td>-0.47</td><td>1.17</td><td>-0.02</td><td>-0.15</td><td>-</td><td>0.33<sup>5</sup></td></tr><tr><td>HEAD<sub>1</sub></td><td>-2.75</td><td>-0.80</td><td>-2.77</td><td>-0.11</td><td>0.00</td><td>-</td></tr><tr><td>HEAD<sub>2</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>LARM<sub>1</sub></td><td>1.73</td><td>-0.24</td><td>0.75</td><td>0.25</td><td>-</td><td>2.26e<sup>5</sup></td></tr><tr><td>LARM<sub>2</sub></td><td>1.13</td><td>-0.75</td><td>0.35</td><td>0.13</td><td>-</td><td>5.10e<sup>5</sup></td></tr><tr><td>LARM<sub>3</sub></td><td>-1.04</td><td>-0.24</td><td>-0.23</td><td>-0.04</td><td>-</td><td>2.95e<sup>5</sup></td></tr><tr><td>LARM<sub>4</sub></td><td>-0.02</td><td>-0.05</td><td>-0.36</td><td>0.35</td><td>-</td><td>0.48e<sup>5</sup></td></tr><tr><td>LARM<sub>5</sub></td><td>1.24</td><td>-1.7</td><td>1.14</td><td>0.18</td><td>-</td><td>-</td></tr><tr><td>LARM<sub>6</sub></td><td>-0.61</td><td>0.24</td><td>1.33</td><td>-0.22</td><td>-</td><td>-</td></tr><tr><td>LARM<sub>7</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr><tr><td>RARM<sub>1</sub></td><td>-1.38</td><td>-1.67</td><td>0.62</td><td>0.34</td><td>-</td><td>-</td></tr><tr><td>RARM<sub>2</sub></td><td>-0.99</td><td>-0.62</td><td>-0.05</td><td>0.12</td><td>-</td><td>1.24e<sup>5</sup></td></tr><tr><td>RARM<sub>3</sub></td><td>0.21</td><td>0.07</td><td>-1.65</td><td>-0.03</td><td>-</td><td>0.44e<sup>5</sup></td></tr><tr><td>RARM<sub>4</sub></td><td>-1.83</td><td>-0.67</td><td>-0.31</td><td>0.72</td><td>-</td><td>0.61e<sup>5</sup></td></tr><tr><td>RARM<sub>5</sub></td><td>1.76</td><td>-0.66</td><td>-0.13</td><td>0.16</td><td>-</td><td>-</td></tr><tr><td>RARM<sub>6</sub></td><td>1.86</td><td>0.19</td><td>-0.39</td><td>0.32</td><td>-</td><td>-</td></tr><tr><td>LARM<sub>7</sub></td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td><td>-</td></tr></tbody></table>

and diag(K<sub>w1</sub>) = [350 110 300 5.9e<sup>4</sup> 9.2e<sup>4</sup> 3.6e<sup>5</sup>]<sup>T</sup> for the right and left legs, respectively.

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.
<figure><img src="image_9.png" /><figcaption>Figure 7: Comparison of the estimated joint offsets of the right arm with the proposed calibration method performed at different times.</figcaption></figure>

### 6.1.3. 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.

### 6.1.4. 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.

## 7. 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
<figure><img src="image_10.png" /><figcaption>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.</figcaption></figure>
<table><caption>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.</caption><thead><tr><th rowspan="2">Chain name</th><th rowspan="2">Case</th><th>FCM</th><th>GCM</th><th>NCM</th></tr><tr><th>RMSE [mm]</th><th>RMSE [mm]</th><th>RMSE [mm]</th></tr></thead><tbody><tr><td rowspan="3">HAT</td><td>SW</td><td>2.9</td><td>2.9</td><td>6.6</td></tr><tr><td>RP</td><td>1.9</td><td>1.9</td><td>6.6</td></tr><tr><td>RM</td><td>1.9</td><td>1.9</td><td>5.7</td></tr><tr><td rowspan="2">Left Leg</td><td>SW</td><td>2.4</td><td>3.8</td><td>8.4</td></tr><tr><td>RP</td><td>1.9</td><td>4.8</td><td>7.6</td></tr><tr><td rowspan="3">Right Leg</td><td>SW</td><td>2.5</td><td>4.8</td><td>7.8</td></tr><tr><td>RP</td><td>1.8</td><td>5.4</td><td>8.4</td></tr><tr><td>RM</td><td>2.2</td><td>5.5</td><td>7.8</td></tr></tbody></table>

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 cabilities 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 positions 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 $O_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 $O_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.

## Acknowledgment

This research was partly funded by the research agreement ROB4FAM.

## References

[1] Aurand, A., Dufour, J., Marras, W., 2017. Accuracy map of an optical motion capture system with 42 or 21 cameras in a large measurement volume. J. Biomech. 58, 237-240.

[2] Benallegue, M., Lamiraux, F., 2015. Estimation and Stabilization of Humanoid Flexibility Deformation Using Only Inertial Measurement Units and Contact Information. International Journal of Humanoid Robotics 12, 15500255.

[3] Birbach, O., Frese, U., Bauml, B., 2015. Rapid calibration of a multisensorial humanoid's upper body: An automatic and self-contained approach. The International Journal of Robotics Research 34, 420-436.
[4] Bonnet, V., Fraisse, P., Crosnier, A., Gautier, M., González, A., Venture, G., 2016. Optimal exciting dance for identifying inertial parameters of an anthropomorphic structure. IEEE Transactions on Robotics 32, 823–836.

[5] Borm, J.H., Menq, C.H., 1989. Experimental study of observability of parameter errors in robot calibration, in: 1989 IEEE International Conference on Robotics and Automation, IEEE Computer Society. pp. 587–588.

[6] Byrd, R., Hribar, E.M., Nocedal, J., 1999. An interior point algorithm for large-scale nonlinear programming. SIAM Journal on Optimization 9, 877–900.

[7] Carrillo, H., Birbach, O., Taubig, H., Bäuml, B., Frese, U., Castellanos, J.A., 2013. On task-oriented criteria for configurations selection in robot calibration, in: 2013 IEEE International Conference on Robotics and Automation, pp. 3653–3659.

[8] Conconi, M., Pompili, A., Sancisi, N., Parenti-Castelli, V., 2021. Quantification of the errors associated with marker occlusion in stereophotogrammetric systems and implications on gait analysis. J. Biomech. 114, 110162.

[9] Daney, D., 2002. Optimal measurement configurations for gough platform calibration, in: Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292), pp. 147–152 vol.1.

[10] Daney, D., Papegay, Y., Madeline, B., 2005. Choosing measurement poses for robot calibration with the local convergence method and tabu search. The International Journal of Robotics Research 6, 501–518.

[11] Driels, M.R., Pathre, U.S., 1990. Significance of observation strategy on the design of robot calibration experiments. Journal of Robotic Systems 7, 197–223.

[12] Gautier, M., 1990. Numerical calculation of the base inertial parameters of robots, in: Proceedings., IEEE International Conference on Robotics and Automation, pp. 1020–1025 vol.2.

[13] Hayati, S., M.Mirmirani, 1985. Improving the absolute positioning accuracy of robot manipulators. Journal of Robotic Systems 2, 397–413.

[14] Hollerbach, J., Khalil, W., Gautier, M., 2008. Model Identification. Springer Berlin Heidelberg, Berlin, Heidelberg. pp. 321–344.

[15] Hollerbach, J.M., Wampler, C.W., 1996. The calibration index and taxonomy for robot kinematic calibration methods. The International Journal of Robotics Research 15, 573–591.

[16] Joubair, A., Bonev, I.A., 2013. Comparison of the efficiency of five observability indices for robot calibration. Mechanism and Machine Theory 70, 254–265.
[17] Kamali, K., Bonev, I.A., 2019. Optimal experiment design for elasto-geometrical calibration of industrial robots. IEEE/ASME Transactions on Mechatronics 24, 2733-2744.

[18] Kastner, T., Rofer, T., Laue, T., 2015. Automatic robot calibration for the nao, in: RoboCup 2014: Robot World Cup XVIII, pp. 233-244.

[19] Khalil, W., Besnard, S., 2002. Geometric calibration of robots with flexible joints and links. Journal of Intelligent and Robotic Systems 34, 357-379.

[20] Khalil, W., Caenen, J.L., Enguehard, C.H., 1989. Identification and calibration of the geometric parameters of robots, in: Proc. of the 1st International Symposium on Experimental Robotics.

[21] Khalil, W., Dombre, E., 2002. Modeling, identification & control of robots. London : Hermes Penton Ltd.

[22] Khalil, W., Gautier, M., Enguehard, C., 1991. Identifiable parameters and optimum configurations for robots calibration. Robotica 9, 63-70.

[23] Khusainov, R., Klimchik, A., Magid, E., 2017. Humanoid robot kinematic calibration using industrial manipulator, in: 2017 International Conference on Mechanical, System and Control Engineering (ICMSC), pp. 184-189.

[24] Klimchik, A., Caro, S., Pashkevich, A., 2015. Optimal pose selection for calibration of planar anthropomorphic manipulators. Precision Engineering 40, 214-229.

[25] Lightcap, C., Hamner, S., Schmitz, T., Banks, S., 2008. Improved positioning accuracy of the pa10-6ce robot with geometric and flexibility calibration. IEEE Trans. Robot. 24, 452-456.

[26] Marie, S., Courteille, E., Maurine, P., 2013. Elasto-geometrical modeling and calibration of robot manipulators: Application to machining and forming applications. Mechanism and Machine Theory 69, 13-43.

[27] Mitchell, T.J., 2000. An algorithm for the construction of "d-optimal" experimental designs. Technometrics 42, 48-54.

[28] Morozov, M., Riise, J., Summan, R., Pierce, S., Mineo, C., Macleod, C., Brown, R.H., 2016. Assessing the accuracy of industrial robots through metrology for the enhancement of automated non-destructive testing. 2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), 335-340.

[29] Nahvi, A., Hollerbach, J.M., 1996. The noise amplification index for optimal pose selection in robot calibration, in: Proceedings of IEEE international conference on robotics and automation, IEEE. pp. 647-654.

[30] Nubiola, A., Bonev, I.A., 2004. Absolute calibration of an abb irb 1600 robot using a laser tracker. Robot. Comput.-Integr. Manuf. 29, 236-245.
[31] Rotella, N., Bloesch, M., Righetti, L., Schaal, S., 2014. State estimation for a humanoid robot, in: Proceedings., IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 952–958.

[32] Rozlivek, J., Rustler, L., Stepanova, K., Hoffmann, M., 2021. Multisensorial robot calibration framework and toolbox, 459–466.

[33] Stepanova, K., Pajdla, T., Hoffmann, M., 2019. Robot self-calibration using multiple kinematic chains—a simulation study on the icub humanoid robot. IEEE Robotics and Automation Letters 4, 1900–1907.

[34] Sun, Y., Hollerbach, J.M., 2008. Observability index selection for robot calibration, in: 2008 IEEE International Conference on Robotics and Automation, pp. 831–836.

[35] Takeda, Y., Shen, G., Funabashi, H., 2004. A dbb-based kinematic calibration method for in-parallel actuated mechanisms using a fourier series. J. Mech. Des. 126, 856–865.

[36] Tanguy, A., Kheddar, A., Comport, A.I., 2018. Online eye-robot self-calibration, in: 2018 IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR), pp. 68–73.

[37] Watanabe, K., Strong, M., West, M., Escobedo, C., Aramburu, A., Chaitana Kodur, K., Roncone, A., 2021. Self-contained kinematic calibration of a novel whole-body artificial skin for human-robot collaboration, 1778–1785.

[38] Wu, Y., Klimchik, A., Caro, S., Furet, B., Pashkevich, A., 2015. Geometric calibration of industrial robots using enhanced partial pose measurements and design of experiments. Robotics and Computer-Integrated Manufacturing 35, 151–168.

[39] Zhuang, H., Wang, K., Roth, Z., 1994. Optimal selection of measurement configurations for robot calibration using simulated annealing, in: Proceedings of the 1994 IEEE International Conference on Robotics and Automation, pp. 393–398 vol.1.

[40] Zhuang, H., Wu, J., Huang, W., 1997. Optimal planning of robot calibration experiments by genetic algorithms. Journal of Robotic Systems 14, 741–752.