Model-Based Real-Time Motion Tracking using Dynamical Inverse Kinematics on SO(3)

Model-Based Real-T ime Motion T racking using Dynamical In v erse Kinematics on SO(3) Lorenzo Rapetti 1 , 2 , Y eshasvi T irupachuri 1 , 3 , K ourosh Darvish 1 , Claudia Latella 1 , Daniele Pucci 1 Abstract —This paper contributes to wards the development of motion tracking algorithms for time-critical applications, propos- ing an infrastructure for solving dynamically the inv erse kine- matics of highly articulate systems such as humans. W e present a method based on the integration of differential kinematics using distance measurement on SO(3) for which the con vergence is proved using L yapunov analysis. An experimental scenario, where the motion of a human subject is tracked in static and dynamic configurations, is used to validate the in verse kinematics method perf ormance on human and humanoid models. Mor eover , the method is tested on a human-humanoid r etargeting scenario, verifying the usability of the computed solution for real-time robotics applications. Our approach is e valuated both in terms of accuracy and computational load, and compared to iterative optimization algorithms. I . I N T RO D U C T I O N Now adays, real-time motion tracking has man y established applications in different fields such as medicine, virtual reality , and computer gaming. Moreov er , in the field of robotics there is a growing interest in human motion retargeting and imitation [1][2]. Dif ferent tracking technologies and algo- rithms are currently av ailable. Among these, optical tracking techniques are more spread and have been av ailable since the eighties [3]. Inertial/magnetic tracking technologies have been av ailable only with the advent of micromachined sensors, and ensure higher frequency of data and lower latenc y , that makes them suited for demanding real-time applications [4]. The objectiv e of motion tracking algorithm is to find the human configuration from a set of measurement. T racking algorithms can use human body representations with dif ferent level of complexity spacing from contours [5][6], stick figure [7][8], and v olumes [9][10]. F or some techniques it is not required to know a priori the shape of the model, and its identification is part of the algorithm [5][7]. When the human is modeled as a kinematic chain, the solution of the model inv erse kinematics has a major role in the algorithm [11][12][13][14], hence, strategies to solve it ef ficiently are needed. In the field of robotics, a common inv erse kinematics problem consists in finding the mapping between the end- effector of a manipulator (task space) and the corresponding joints angle (configuration space). Compared to an industrial manipulator , solving the in verse kinematics for a human kine- matic model can be demanding as it is a highly articulate 1 Dynamic Interaction Control at Istituto Italiano di T ecnolo- gia, Center for Robotics T echnologies, Genov a, Italy . (email: name.surname@iit.it ) 2 Machine Learning and Optimisation, The University of Manchester , Manchester , United Kingdom. 3 DIBRIS, University of Genova, Genov a, Italy . Fig. 1: Motion tracking of a human running on a treadmill using an human model, on the center, and a humanoid model, on the right. kinematic chain. Human kinematics is redundant, it generally has a high number of degrees of freedom (DoF), and may also take into account muskoloskeletal constraints in order to ensure realistic motion. Moreover , a human moving in the space is a floating base system, and its configuration space lies on a differentiable manifold [15]. Since finding an analytical closed-form solution for the in verse kinematics of a human model is not always either possible or efficient, a numerical solution is often preferred. One of the most common approaches for solving in verse kinematics is to formulate the problem as a non-linear opti- mization that is solved via iterati ve algorithms [16][17]. This class of algorithms, referred to as instantaneous optimization since the y aim to con verge to a stable solution for each time step. Although instantaneous optimization algorithms con verge fast to a solution for common robotics applications, finding the solution for a human model at a suf ficient rate becomes demanding for time-critical applications. In some cases, better performances are achiev ed using heuristic iter- ativ e algorithms [18], learning algorithms [19], or combining analytical and numerical methods [20]. An alternative to the instantaneous optimization approach consists of rephrasing the in verse kinematics problem as a control problem [21]. This class of algorithms is referred to as dynamical optimization since the model configuration is controlled in order to dynam- ically con verge ov er time to the sensors measurement. From a computational point of view , the main adv antage of this approach is that the solution at each time-step is computed directly with a single iteration. The absence of iterations makes the dynamical optimization approach faster , therefore suitable for solving whole-body inv erse kinematics of complex models in time-critical motion tracking applications. This article presents a scheme for real-time motion tracking of highly articulate human, or humanoid, models. The tracking is achiev ed at a high frequency by using dynamic inv erse kinematics optimization approach. The main contribution of this work is the application of dynamical in verse kinemat- ics strategies to floating-base models using rotation matrix parametrization, proving the con vergence of the method using L yapunov theory , and considering model kinematic constraints by enforcing them into the dif ferential kinematics solution. The implementation of the proposed scheme is tested for both human and humanoid models, during dif ferent tasks in volving static and dynamic motions. The performance is compared to the results obtained using instantaneous optimization. The paper is or ganized as following: Section II introduces the notation, human modeling, formulation of the motion tracking as in verse kinematics problem, and the dynamical optimization scheme. Section III presents our proposed dynamical in verse kinematics approach with rotation matrix parametrization and constrained inv erse differential kinematics. Section IV lays the experimental details, and in Section V the results are discussed and compared to instantaneous optimization. Conclusions fol- low in Section VI. I I . B A C K G RO U N D A. Notation • I denotes an inertial frame of reference. • I n × n ∈ R n × n denotes the identity matrix of size n . • A p B ∈ R 3 is the the position of the origin of the frame B with respect to the frame A . • A R B ∈ S O (3) represents the rotation matrix of the frames B with respect to A . • A ω B ∈ R 3 is the angular velocity of the frame B with respect to A , expressed in A . • The operator tr ( · ) : R 3 × 3 → R denotes the trace of a matrix, such that given A ∈ R 3 × 3 , it is defined as tr ( A ) := A 1 , 1 + A 2 , 2 + A 3 , 3 . • The operator sk ( · ) : R 3 × 3 → so (3) denotes ske w- symmetric operation of a matrix, such that given A ∈ R 3 × 3 , it is defined as sk ( A ) := ( A − A > ) / 2 . • The operator S ( · ) : R 3 → so (3) denotes sk ew-symmetric vector operation, such that giv en two vectors v, u ∈ R 3 , it is defined as v × u = S ( v ) u . • The vee operator · ∨ : so (3) → R 3 denotes the inv erse of the sk ew-symmetric vector operator , such that gi ven a matrix A ∈ so (3) and a vector u ∈ R 3 , it is defined as Au = A ∨ × u . • The operator ◦ indicates the element-wise multiplication , such that gi ven tw o vectors with same length, a = [ a 1 , a 2 , ..., a n ] and b = [ b 1 , b 2 , ..., b n ] , it is defined as a ◦ b = [ a 1 b 1 , a 2 b 2 , ..., a n b n ] . • The operator tanh ( · ) indicates the hyperbolic tangent function of a scalar , or the element-wise hyperbolic tangent of a vector of scalars. • The operator k·k 2 indicates the L2-norm of a vector , such that giv en a vector v ∈ R 3 , it is defined as k v k 2 = p v 2 1 + v 2 2 + v 2 3 . B. Modeling The human is modeled as a multi-body mechanical system composed of n + 1 rigid bodies, called links that are connected by n joints with one degr ee of fr eedom (DoF) each [22]. Additionally , the system is assumed to be floating base , i.e., none of the links has an a priori constant pose with respect to the inertial frame I . Hence, a specific frame, attached to a link of the system, is referred to as the base frame , and denoted by B . The model configuration is characterized by the position and the orientation of the base frame along with the joint positions . Accordingly , the configuration space lies on the Lie group Q = R 3 × S O (3) × R n . An element of the configuration space q ∈ Q is defined as the triplet q = ( I p B , I R B , s ) where I p B ∈ R 3 and I R B ∈ S O (3) denote the position and the orientation of the base frame , and s ∈ R n denotes the joints configuration that represents the topology , i.e. shape, of the mechanical system. The position and the orientation of a frame A attached to the model can be obtained via geometrical forward kinematic map h A ( · ) : Q → ( S O (3) × R 3 ) from the model configuration . The forward kinematics can be decom- posed into position, i.e., I p A = h p A ( q ) , and orientation, i.e., I R A = h o A ( q ) , maps. The model velocity is characterized by the linear and the angular velocity of the base frame along with the joint velocities . Accordingly , the configuration velocity space lies on the group V = R 3 × R 3 × R n . An element of the configuration velocity space ν ∈ V is defined as ν = ( I v B , ˙ s ) where I v B = ( I ˙ p B , I ω B ) ∈ R 3 × R 3 denotes the linear and angular velocity of the base frame , and ˙ s denotes the joint v elocities. The velocity of a frame A attached to the model is denoted by I v A = ( I ˙ p A , I ω A ) with the linear and the angular velocity components respecti vely . The mapping between frame velocity I v A and configuration velocity ν is achieved through the J acobian J A = J A ( q ) ∈ R 6 × ( n +6) , i.e., I v A = J A ( q ) ν . The J acobian is composed of the linear part J ` A ( q ) and the angular part J a A ( q ) that maps the linear and the angular velocities respectiv ely , i.e., I ˙ p A = J ` A ( q ) ν and I ω A = J a A ( q ) ν . C. Pr oblem Statement Motion tracking algorithms aims to find the human con- figuration giv en a set of targets describing the kinematics of its links. The targets are the measurements of the link pose and velocity expressed in a world reference frame, and are retriev ed from v arious sensors measurements, e.g., IMUs or image processing. The process of estimating the configuration of a mechanical system from task space measurements is gen- erally referred to as in verse kinematics, and can be formulated as follows: Problem 1. Given a set of n p frames P = {P 1 , P 2 , .... P n p } with the associated tar get position I p P i ( t ) ∈ R 3 and tar get linear velocity measur ements I ˙ p P i ( t ) ∈ R 3 , and given a set of n o frames O = {O 1 , O 2 , .... O n o } with the associated tar get orientation I R O j ( t ) ∈ S O (3) and tar get angular velocity measur ements I ω O j ( t ) ∈ R 3 , find the state configuration ( q ( t ) , ν ( t ) ) of a model suc h that:                      I p P i ( t ) = h p P i ( q ( t )) , ∀ i = 1 , . . . , n p I R O j ( t ) = h o O j ( q ( t )) , ∀ j = 1 , . . . , n o I ˙ p P i ( t ) = J ` P i ( q ( t )) ν ( t ) , ∀ i = 1 , . . . , n p I ω O j ( t ) = J a O j ( q ( t )) ν ( t ) , ∀ j = 1 , . . . , n o A q s ( t ) ≤ b q , A ν ˙ s ( t ) ≤ b ν , (1) wher e A q and b q ar e two constant parameters that r epr esent the limits for the joint configuration of the model, and A ν and b ν constant parameter s that r epresent the limits for the joint velocity . The following quantities are defined in order to have a compact representation of Problem 1 . The targets are collected in a pose targ et vector x ( t ) and velocity targ et vector v( t ) : x ( t ) :=          I p P 1 ( t ) . . . I p P n p ( t ) I R O 1 ( t ) . . . I R O n o ( t )          , v( t ) :=          I ˙ p P 1 ( t ) . . . I ˙ p P n p ( t ) I ω O 1 ( t ) . . . I ω O n o ( t )          , (2) and, forward geometrical kinematics is expressed as a single vector h ( q ( t )) , and Jacobians as single matrix J ( q ( t )) : h ( q ( t )) :=          h p P 1 ( q ( t )) . . . h p P n p ( q ( t )) h o O 1 ( q ( t )) . . . h o O n o ( q ( t ))          , J ( q ( t )) :=          J ` P 1 ( q ( t )) . . . J ` P n p ( q ( t )) J a O 1 ( q ( t )) . . . J a O n o ( q ( t ))          , (3) the set of equations in (1) can then be reduced, using the definitions of (2) and (3), to the follo wing two equations describing respectively the forwar d kinematics and differ ential kinematics for all the tar get frames: x ( t ) = h ( q ( t )) , (4a) v( t ) = J ( q ( t )) ν ( t ) . (4b) As mentioned in Section I, in the case of highly articulate systems, like humans, finding an analytical solution to Equa- tions (4a) and (4b) is often v ain, and a numerical optimization solution is preferred. Hence, the optimization problem has to be properly identified, such that the minimization of the cost function implies the estimated state configuration ( ˆ q , ˆ ν ) follo w their ground truth. D. Dynamical In verse Kinematics Optimization The dynamical optimization method aims to control the state configuration ( q ( t ) , ν ( t ) ) to con ver ge to the giv en target measurements, i.e., to control the distance between the model and the measurements to con ver ge to zero [21]. The block diagram for dynamical in verse kinematics implementation is Fig. 2: Dynamical optimization scheme for real-time in verse kine- matics solution. presented in Figure 2. The scheme is composed by three main parts: a) correction of the target v elocity measurements according to the residual feedback ( v ∗ ( t ) = v( t ) + K r ( t ) ), b) in version of the model differential kinematics to obtain the state velocity ν ( t ) , and c) inte gration of state velocity to obtain the configuration q ( t ) . The implementation of these three parts and the definition of the residual error are not unique and depends on different design choices. T raditionally , Euler angles or unit quaternion parametrization are used in literature for modeling floating-base systems and for defining the orientation distances. The implementation presented in the next section exploits rotation matrix parametrization for targets orientation and systems’ floating-base modeling. T o the best of authors’ knowledge, this parametrization has not been presented in literature before. I I I . M E T H O D A. V elocity Corr ection using Rotation Matrix The distances between the gi ven state q ( t ) and the pose tar- get vector x ( t ) are collected in a residual vector r ( q ( t ) , x ( t )) defined as follo ws: r ( q ( t ) , x ( t )) =            I p P 1 ( t ) − h p P 1 ( q ( t )) . . . I p P n p ( t ) − h p P n p ( q ( t )) sk ( h o O 1 ( q ( t )) T I R O 1 ( t )) ∨ . . . sk ( h o O n o ( q ( t )) T I R O n o ( t )) ∨            . (5) According to the chosen rotation matrix parametrization, the distance between two orientation measurements is computed on SO(3) with the sk ( . ) ∨ operator . The distances from the target velocities v( t ) are collected in the velocity residual vector u ( q ( t ) , ν ( t ) , v( t )) defined as: u ( q ( t ) , ν ( t ) , v( t )) = v( t ) − J ( q ( t )) ν ( t ) . (6) At this stage, we assume the state velocity ν ( t ) being the input of a dynamical system, and we want to control the system in order to dri ve the residual v ectors to wards zero. Lemma 1. Assume r ( q ( t ) , x ( t )) defined as in (5) , u ( q ( t ) , ν ( t ) , v( t )) defined as in (6) , and the system u ( q ( t ) , ν ( t ) , v( t )) + K r ( q ( t ) , x ( t )) = 0 , (7) wher e K ∈ R (3 n p +3 n o ) × R (3 n p +3 n o ) is a positive definite diagonal matrix. Then, ( r , u ) = (0 , 0) denotes an (almost) globally asymptotically stable equilibrium point for the sys- tem. The proof is provided in appendix Lemma 1 shows that we can control the system input ν ( t ) so that r ( t ) and u ( t ) con- ver ge to zero for (almost) any initialization q ( t 0 ) . Replacing the expression of u ( q ( t ) , ν ( t )) , presented in (6), in the system (7), it can be observed that the expression is linear in state velocity ν ( t ) . J ( q ( t )) ν ( t ) = v( t ) + K r ( q ( t ) , x ( t )) . (8) The rate of con ver gence depends on the magnitude of the elements of matrix K . Higher values of K imply faster con vergence of the system (7) to wards zero. Howe ver , the implementation of discrete time solution bounds the values of K depending on the sampling time [23]. In fact, Equation (8) is solved for a discrete control input ν ( t k ) from the following equation: J ( q ( t k − 1 )) ν ( t k ) = v( t k ) + K r ( q ( t k − 1 ) , x ( t k )) . (9) Hence, according to the scheme in Figure 2, the corrected velocity term is v ∗ ( t k ) = v( t k )+ K r ( q ( t k − 1 ) , x ( t k )) . The way the discrete control input ν ( t k ) is obtained will be discussed in Section III-B. B. Constrained Inver se Dif fer ential Kinematics The inv erse dif ferential kinematics is the problem of in- verting the dif ferential kinematics presented in Equation (4b) in order to find the configuration state velocity ν ( t ) for a giv en set of task space velocities. In order to compute the control input ν ( t k ) from (9), it is required to solve the in verse differential kinematics for the corrected target velocity v ∗ ( t k ) . The solution depends on the rank of the Jacobian matrix J ( q ( t k )) , and in most of the cases it can be found only numerically as an optimization. Different strategies to solve the inv erse dif ferential kinematics can be found in litera- ture [16][17][21][24], among the possible solutions, the most common approach is to use Jacobian generalized inv erse [25]. In order to take into account also the model constraints, howe ver , a Quadratic Progamming (QP) solver is preferred since it allo ws to introduce a set of constraints to the problem [26]. Hence, the in verse differential kinematics solution can be defined as the follo wing QP optimization problem: minimize ν ( t k ) k v ∗ ( t k ) − J ( q ( t k − 1 )) ν ( t k ) k 2 (10a) subject to Gν ( t k ) ≤ g . (10b) The constraints of the optimization problem defined in (10b) are linear with respect to the v elocity configuration, and indeed can be used to directly enforce constraints on joint velocities ˙ s . In order to enforce system of linear constraints on joints configuration: Aq ( t k ) ≤ b q , (11) instead, the proposed joint limit a voidance strategy consists of translating (11) to (10b) such that the dynamical constraint bounds depend on the joints configuration, i.e., g = g ( q ) . The Fig. 3: Constrained configuration space for the elbo w joints of the iCub model. Blue and red lines represent respecti vely upper and lower joint velocity limits, depending on joint angle. The yellow area represents the joint configuration space. According to limit av oidance strategy , the joint velocity is bounded to be ≥ 0 when lower angle limit is reached, and ≤ 0 when upper limit is reached (angle limits are represented by dashed lines). Y ellow lines represent the joint configuration trajectory computed by the in verse kinematics algorithm, while the purple line represents the trajectory tracked by a real robot. idea is to limit the joints velocity as the joints get closer to their limits, modeling this behaviour using hyperbolic tangent function. It is assumed that for each configuration constraint, there e xists a one-to-one projection to the velocity constraint space, i.e., A q = A ν = A (in case there are no constraints in a projected space, b q i → ∞ and b ν i → ∞ can be approximated numerically). Therefore, the QP constraints v ariable used in (10b) are defined as follo ws: G = A (12a) g = tanh ( K g ( b q − Aq )) ◦ b ν (12b) where K g is a positive definite diagonal matrix regulating the slope of the hyperbolic tangent and b ν is a constant v ector . Figure 3 sho ws the ef fect of the proposed limit a voidance approach in constraining the joints configuration space. C. Numerical Inte gration Giv en the configuration velocity solution ν ( t k ) , it is possible to compute the state configuration q ( t k ) by defining an initial configuration q ( t 0 ) and integrating ov er time. Base position I p B and joints configuration s lie in vector space o ver R for which most of the numerical inte grations methods proposed in literature can be used [27]. The integration of the base angular velocity I ω B ( t k ) is not tri vial, and numerical inte gration errors can lead to the violation of the orthonormality condition [28] for the base orientation I R B . A possible approach presented in literature is the Baumgarte stabilization [28], the conv er- gence of I R B ( t k ) over S O (3) is ensured computing the base orientation matrix dynamics I ˙ R B ( t k ) ∈ R 3 × 3 as follow: A ( t k − 1 ) = ρ 2 (( I R B ( t k − 1 ) T I R B ( t k − 1 )) − 1 − I 3 × 3 ) , (13a) I ˙ R B ( t k ) = I R B ( t k − 1 )( S ( I ω B ( t k )) + A ( t k − 1 )) , (13b) where ρ ∈ R + is the gain regulating the con vergence tow ards the orthonormality condition, and ∆ t k = t k − t k − 1 is the integration time step. The adv antage of obtaining the config- uration q ( t k ) through integration of velocity ν ( t k ) is that the two estimated quantities are directly related, and continuity of the state configuration is ensured. I V . E X P E R I M E N T S A. Motion Data Acquisition The proposed method has been implemented and tested using motion data acquired with the Xsens A winda wearable suit [29] pro viding pose and velocity of a 23 links human model, computed from a set of distributed Inertial Measure- ment Units (IMUs). The motion data is streamed through Y ARP middle ware [30] that facilitates recording and real- time playback of data. The motion data is acquired for three scenarios with dif ferent le vels of dynamicity: t-pose where the subject stands on two feet with the arms parallel to the ground, walking where the subject walks on a treadmill at a constant speed of 4 km / h , and running where the subject runs on a treadmill at a constant speed of 10 km / h . B. Models The motion tracking is performed by using two different human models defined as in II-B. Both the models are com- posed by 23 physical links representing se gments of the human body . Each physical link is attached to the next one through a certain number of rotational joint connected through dummy links , i.e., links with dimension zero, in order to model human joints with multiple DoFs. In one human model ( Human66 ) all the physical links are connected through spherical joints ( 3 rotational joints), i.e., a total of 66 DoFs and 67 links. The second model ( Human48 ) is based on the modeling of the human musculoskeletal system as described in clinical studies [31][32][33], it has a reduced number of joint, i.e., 48DoFs, and tak es into account human joint limits. Additionally , we consider experiments with a model of the iCub humanoid robot [34]. The motiv ation behind this is to highlight the performance in achieving motion tracking, and motion retargeting from the human to a humanoid. The iCub model is composed of 15 physical links connected through 34 rotational joints. Model joint limits are defined according to the real robot mechanic constraints, including linear system of constraints in volving multiple set of joints due to coupled joints mechanics. The human 1 and humanoid 2 models sho wn in Figure 1 are open-source resources. Considering that all the models hav e only rotational joints, the in verse kinematics problem is defined with a rotational and angular v elocity target for each physical link, i.e., n o = 23 for the human model and n o = 15 for the humanoid model. Additionally , as both the models are floating base, a position and linear velocity target is used for the base frame, i.e., n p = 1 . C. Robot Experiments The computed joint data for the iCub model has been tested on the real robot in order to v erify the feasibility 1 https://github .com/robotology/human-gazebo 2 https://github .com/robotology/icub-models of the computed robot configuration. The experimental setup in volv es an iCub robot fixed on a pole , as shown in Figure 4, controlled in position through lo w-lev el PID running at 1 kHz . The desired joint configuration is sent to the robot at 50 Hz , and it is computed real-time using the iCub model via in verse kinematics starting from human measurement data. V . R E S U LT S The performance of dynamical optimization in verse kine- matics solv er is compared to instantaneous optimization imple- mentations. The ev aluation is done in terms of computational load and accuracy on a 2 . 3 GHz Intel Core i7 processor with 16 GB of RAM. The accuracy is measured with two metrics, one for the orientation tar gets and one for the velocity targets. The mean normalized trace err or ( M N T E ) is a dimensionless metric measuring the ov erall accuracy of the orientation targets: M N T E = 1 n o n o X j =1 tr ( I 3 × 3 − I ˆ R O j ( q ) T I R O j ) 2 , (14) where I ˆ R O j ( q ) is the estimated frame orientation given the state q , and the 1 2 factor normalize the value of the trace between 0 and 1. For the angular velocities, the ov erall error is ev aluated as r oot mean squar ed err or ( RM S E ): RM S E = v u u u t 1 n o n o X j =1    I ω O j − I ˆ ω O j ( q , ν )    2 2 3 , (15) where ˆ ω O j ( q , ν ) is the estimated frame v elocity gi ven the configuration ( q , ν ) . The computational load is ev aluated as the time for computing the state ( q , ν ) . The statistics hav e been collected discarding the transient of 2 second from the initial time t 0 . A. Instantaneous Optimization As mentioned in Section I, the instantaneous optimization methods solve the in verse kinematics at each time-step t k through non-linear optimization. A general formulation of the optimization problem is defined as follo wing: minimize q ( t k ) k K r r ( q ( t k ) , x ( t k )) k 2 (16a) subject to As ( t k ) ≤ b (16b) where K r is a weight matrix that matches the unit measure- ments of targets distances, and e ventually assigns a weight to each of the target. A common approach for solving the non-linear optimization problem is to consider the linear approximation of the system by recalling the Jacobian matrix definition, and solve the problem iterati vely [35][36]. Ho wever , in order to enforce state configuration constraints, recent approaches make also use of con ve x optimization [24][37]. As benchmark, we hav e implemented instantaneous in verse kinematics optimization using iDynT ree [38] multibody kine- matics library , and the IPOPT software library for non-linear optimization [39]. The stopping criteria for the optimization Fig. 4: Real-time retargeting of the human motion to iCub humanoid robot configuration. is the pose err or accuracy , and it has been tuned in order to find a solution in a time comparable to the dynamical optimization. T wo dif ferent implementations have been tested. The former , referred to as whole-body optimization , solves a single optimization problem instantiated for the whole-system. Instead, the latter instantiates the optimization process di viding the model into multiple subsystems, each consisting of e xactly a pair of targets, and solves the subproblems in parallel. W e refer to this implementation as pair-wise optimization . Looking at Figure 7, it can be observ ed that the performance of instantaneous optimization approaches decreases as the task gets more dynamic. This is particularly evident for the com- putational time. In the whole-body optimization, the av erage computational time is higher, and it is characterized by a large variance, reaching peaks above 25 ms during the running task. Concerning the pair-wised optimization, the increase of time between walking and running is less evident. Howe ver , the pair-wised optimization takes longer for finding a solution for the iCub model because of the local difference between the human and the robot kinematics. B. Dynamical Optimization The dynamical optimization inv erse kinematics has been implemented using iDynTree multibody kinematics library [38], and the in verse differential kinematics is solved using OSQP [26]. Figure 5 highlights the rate of con ver gence of the error from a gi ven initial zero configuration ( s (0) = 0 ) to wards a tar get static pose. When the gain is zero, there is no velocity correction and the error remains constant. Howe ver , increasing the magnitude of K , the error con verges to its steady state value in less then one second. A large value of the gain K leads to system instability as mentioned in Section III-A. From Figure 7, the dynamical optimization orientation error is mostly comparable with the results achie ved with instantaneous optimization . The only case in which it shows worst orientation accuracy is with the Human66 model dur- ing running task. Concerning the angular v elocity error , the performances are again comparable during dynamic motion, while it is higher for the t-pose with the constrained models. This angular velocity error may be due to the fact that a corrected angular velocity is used in place of the measured link angular velocities, and the joint constraints may intro- duce a constant constraint error because of an unfeasible configuration. Concerning the computational load, this method seems to outperform the others not only in terms of a mean computational time, having an a verage always below 3 ms , but also for its consistency in dif ferent scenario. Experiments with real-robot sho w the feasibility of the in verse kinematics solution. The robot is able to track real- time the motion of the human by follo wing the computed joint configuration with a delay less then 300 ms , as sho wn in Figure 6 (e valuation of the controller is out of the scope of this work). The joint limit av oidance strategy successfully constraints the joint angles within the limits, and more in detail from Figure 3 it can be observed how the full ( s, ˙ s ) configuration is constrained. Fig. 5: Con vergence of the dynamical in verse kinematics optimization for static T -pose using 66 DoF model, starting from zero configura- tion. Con vergence rate depends on the magnitude of the gain K . Fig. 6: Joints configuration of the iCub model obtained from human motion data using dynamical inv erse kinematics. The plots sho w both the desired joint configuration computed by in verse kinematics and sent to the robot, and the configuration measured from the robot. Dashed lines represent the joint limits. V I . C O N C L U S I O N S This paper presents an infrastructure for whole-body in verse kinematics of highly articulate floating-base models in real- time motion tracking applications. The theory is presented Fig. 7: Comparison of the performance of inv erse kinematics methods ( whole-body , pair-wised , and dynamical ) for three models (two humans, and iCub humanoid) in three different scenarios ( T -pose , W alking , and Running ). Each line contains the boxplots for a dif ferent performance ev aluation metric, on the top the ov erall error for the orientation tar gets as base 10 logarithm of mean normalized trace err or , in the middle line the ov erall error for the angular velocities as base 10 logarithm of r oot mean squar ed error , and at the bottom the computational time. Logarithmic metrics allows to compare metrics characterized by different order of magnitude in the different scenarios. using rotation matrix parametrization of orientations, together with the proof of conv ergence through L yapunov analysis. The proposed method has been implemented and the performances tested in an experimental scenario with different conditions. Differently from iterati ve algorithms, the dynamical optimiza- tion requires a single iteration at each time step keeping the computational time constant, and ensures fast con ver gence of the error o ver time. Furthermore, the integration of ve- locities ensures obtaining a continuous and smooth solution. The method has been tested in a human-robot retar geting application to verify its usability . Its characteristics make it suitable for time-critical motion tracking applications with highly dynamic motions, where iterativ e algorithms may not con verge in a sufficient time. As a future work, the ev aluation may be extended to a wider number of in verse kinematics algorithms, models, and experimental scenarios. Another interesting future work is to extend our method by considering the dynamics of the system. A C K N O W L E D G M E N T S This work is supported by Honda R&D Co., Ltd and by EU An.Dy Project that recei ved funding from the European Unions Horizon 2020 research and innov ation p r ogramme under grant agreement No. 731540 . The content of this publi- cation is the sole responsibility of the authors. The European Commission or its services cannot be held responsible for any use that may be made of the information it contains. A P P E N D I X : P RO O F O F L E M M A 1 . The system in (7) can be written as follow:          u p P i ( t ) . . . u p P n p ( t ) u o O 1 ( t ) . . . u o O n o ( t )          +          K p 1 r p P 1 ( t ) . . . K p n p r p P n p ( t ) K o 1 r o O 1 ( t ) . . . K o n o r o O n o ( t )          = 0 , (17) where r p P i ( t ) = I p P i ( t ) − h p P i ( q ( t )) , r o O j ( t ) = sk ( h o O j ( q ( t )) T I R O j ( t )) ∨ , u p P i ( t ) = I ˙ p P i ( t ) − J ` P i ( q ( t )) ν ( t ) , u o O j ( t ) = I ω O j ( t ) − J a O j ( q ( t )) ν ( t ) , K p i and K o j are R 3 × R 3 blocks on the diagonal of K . This system can be decomposed in to a set of n p + n o independent systems, one for each target, depending on the type of target, each subsystem is described by one of the follo wing two equations: u p P i ( t ) + K p i r p P i ( t ) = 0 , (18a) u o O j ( t ) + K o j r o O j ( t ) = 0 . (18b) The system (18a) is a linear first order autonomous system, and for K i positiv e definite the equilibrium point ( r p P i , u p P i ) = (0 , 0) is globally asymptotically stable. F or the system (18b) it can be prov ed that the equilibrium ( r o O j , u o O j ) = (0 , 0) is an almost globally asymptotically stable equilibrium point [40]. The almost global asymptotically stability of all the subsys- tems is indeed pro ved for the point ( r , u ) = (0 , 0) , thus the almost globally asymptotically stability of the equilibrium ( r , u ) = (0 , 0) for the system (7) is pro ved. R E F E R E N C E S [1] B. Dariush, M. Gienger, A. Arumbakkam, C. Goerick, Y ouding Zhu, and K. Fujimura, “Online and markerless motion retargeting with kinematic constraints, ” in 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems , Sep. 2008, pp. 191–198. [2] X. B. Peng, P . Abbeel, S. Levine, and M. v an de Panne, “Deepmimic: Example-guided deep reinforcement learning of physics-based character skills, ” ACM T rans. Graph. , v ol. 37, no. 4, pp. 143:1–143:14, Jul. 2018. [Online]. A vailable: http://doi.acm.org/10.1145/3197517.3201311 [3] J. K. Aggarwal and Q. Cai, “Human motion analysis: A revie w , ” Computer vision and image understanding , vol. 73, no. 3, pp. 428–440, 1999. [4] R. Zhu and Z. Zhou, “ A real-time articulated human motion tracking using tri-axis inertial/magnetic sensors package, ” IEEE T ransactions on Neural systems and rehabilitation engineering , vol. 12, no. 2, pp. 295– 302, 2004. [5] A. Shio and J. Sklansky , “Segmentation of people in motion, ” in Pr oceedings of the IEEE W orkshop on V isual Motion , Oct 1991, pp. 325–332. [6] M. K. Leung and Y ee-Hong Y ang, “First sight: A human body outline labeling system, ” IEEE T ransactions on P attern Analysis and Machine Intelligence , vol. 17, no. 4, pp. 359–377, April 1995. [7] Niyogi and Adelson, “ Analyzing and recognizing walking figures in xyt, ” in 1994 Proceedings of IEEE Conference on Computer V ision and P attern Recognition , June 1994, pp. 469–474. [8] A. G. Bharatkumar, K. E. Daigle, M. G. Pandy, Qin Cai, and J. K. Aggarwal, “Lower limb kinematics of human walking with the medial axis transformation, ” in Pr oceedings of 1994 IEEE W orkshop on Motion of Non-rigid and Articulated Objects , Nov 1994, pp. 70–76. [9] S. W achter and H.-H. Nagel, “Tracking persons in monocular image sequences, ” Computer V ision and Image Understanding , vol. 74, no. 3, pp. 174–192, 1999. [10] J. Gall, C. Stoll, E. De Aguiar, C. Theobalt, B. Rosenhahn, and H.- P . Seidel, “Motion capture using joint skeleton tracking and surface estimation, ” in 2009 IEEE Confer ence on Computer V ision and P attern Recognition . IEEE, 2009, pp. 1746–1753. [11] J.-S. Monzani, P . Baerlocher, R. Boulic, and D. Thalmann, “Using an intermediate skeleton and inv erse kinematics for motion retargeting, ” in Computer Graphics F orum , vol. 19, no. 3. W iley Online Library , 2000, pp. 11–19. [12] G. Pons-Moll, A. Baak, J. Gall, L. Leal-T aixe, M. Mueller, H.-P . Seidel, and B. Rosenhahn, “Outdoor human motion capture using inv erse kine- matics and v on mises-fisher sampling, ” in 2011 International Conference on Computer V ision . IEEE, 2011, pp. 1243–1250. [13] V . Ganapathi, C. Plagemann, D. Koller , and S. Thrun, “Real time motion capture using a single time-of-flight camera, ” in 2010 IEEE Computer Society Confer ence on Computer V ision and P attern Recognition . IEEE, 2010, pp. 755–762. [14] A. Aristidou and J. Lasenby , “Motion capture with constrained in verse kinematics for real-time hand tracking, ” in 2010 4th International Sym- posium on Communications, Contr ol and Signal Processing (ISCCSP) . IEEE, 2010, pp. 1–5. [15] S. Tra versaro and A. Saccon, “Multibody dynamics notation, ” T echnis- che Universiteit Eindhoven, T ech. Rep , 2016. [16] A. Goldenberg, B. Benhabib, and R. Fenton, “ A complete generalized solution to the in verse kinematics of robots, ” IEEE J ournal on Robotics and Automation , vol. 1, no. 1, pp. 14–20, March 1985. [17] S. R. Buss, “Introduction to inverse kinematics with jacobian transpose, pseudoin verse and damped least squares methods, ” IEEE Journal of Robotics and A utomation , v ol. 17, no. 1-19, p. 16, 2004. [18] A. Aristidou and J. Lasenby , “Fabrik: A fast, iterative solver for the in verse kinematics problem, ” Graphical Models , vol. 73, no. 5, pp. 243– 260, 2011. [19] K. Grochow , S. L. Martin, A. Hertzmann, and Z. Popovi ´ c, “Style-based in verse kinematics, ” in A CM transactions on graphics (TOG) , v ol. 23, no. 3. ACM, 2004, pp. 522–531. [20] D. T olani, A. Goswami, and N. I. Badler , “Real-time inverse kinematics techniques for anthropomorphic limbs, ” Graphical Models , vol. 62, no. 5, pp. 353 – 388, 2000. [Online]. A vailable: http://www .sciencedirect.com/science/article/pii/S1524070300905289 [21] L. Sciavicco and B. Siciliano, “ A solution algorithm to the in verse kine- matic problem for redundant manipulators, ” IEEE Journal on Robotics and Automation , vol. 4, no. 4, pp. 403–410, 1988. [22] C. Latella, M. Lorenzini, M. Lazzaroni, F . Romano, S. Tra versaro, M. A. Akhras, D. Pucci, and F . Nori, “T owards real-time whole-body human dynamics estimation through probabilistic sensor fusion algorithms, ” Autonomous Robots , pp. 1–13, 2018. [23] K. Ogata et al. , Discrete-time contr ol systems . Prentice Hall Englew ood Cliffs, NJ, 1995, v ol. 2. [24] O. Kanoun, F . Lamiraux, and P . W ieber, “Kinematic control of redundant manipulators: Generalizing the task-priority framew ork to inequality task, ” IEEE T ransactions on Robotics , v ol. 27, no. 4, pp. 785–792, Aug 2011. [25] C. R. Rao, S. K. Mitra et al. , “Generalized in verse of a matrix and its applications, ” in Pr oceedings of the Sixth Berkeley Symposium on Mathematical Statistics and Probability , V olume 1: Theory of Statistics . The Regents of the University of California, 1972. [26] B. Stellato, G. Banjac, P . Goulart, A. Bemporad, and S. Boyd, “OSQP: An operator splitting solver for quadratic programs, ” ArXiv e-prints , Nov . 2017. [27] P . J. Davis and P . Rabinowitz, Methods of numerical integr ation . Courier Corporation, 2007. [28] S. Gros, M. Zanon, and M. Diehl, “Baumgarte stabilisation over the so(3) rotation group for control, ” in 2015 54th IEEE Conference on Decision and Contr ol (CDC) , Dec 2015, pp. 620–625. [29] D. Roetenber g, H. Luinge, and P . Slyck e, “Xsens MVN: Full 6dof human motion tracking using miniature inertial sensors. ” [Online]. A vailable: http://human.kyst.com.tw/upload/do wnloadfs46130703234558070.pdf [30] G. Metta, P . Fitzpatrick, and L. Natale, “Y arp: yet another robot platform, ” International Journal of Advanced Robotic Systems , vol. 3, no. 1, p. 8, 2006. [31] J. Moll and V . Wright, “Normal range of spinal mobility . an objectiv e clinical study . ” Annals of the rheumatic diseases , vol. 30, no. 4, p. 381, 1971. [32] J. Gerhardt, “Clinical measurements of joint motion and position in the neutral-zero method and sftr recording: basic principles, ” International r ehabilitation medicine , vol. 5, no. 4, pp. 161–164, 1983. [33] M. Alison Middleditch, M. Jean Oliv er et al. , Functional anatomy of the spine . Elsevier Health Sciences, 2005. [34] L. Natale, C. Bartolozzi, D. Pucci, A. W yko wska, and G. Metta, “icub: The not-yet-finished story of b uilding a robot child, ” Science Robotics , vol. 2, no. 13, p. eaaq1026, 2017. [35] J. Barnes, “ An algorithm for solving non-linear equations based on the secant method, ” The Computer J ournal , vol. 8, no. 1, pp. 66–72, 1965. [36] R. Fletcher , “Generalized Inverse Methods for the Best Least Squares Solution of Systems of Non-Linear Equations, ” The Computer J ournal , vol. 10, no. 4, pp. 392–399, 02 1968. [37] F . Blanchini, G. Fenu, G. Giordano, and F . A. Pelle grino, “ A conv ex programming approach to the inv erse kinematics problem for manipula- tors under constraints, ” European Journal of Contr ol , vol. 33, pp. 11–23, 2017. [38] F . Nori, S. Tra versaro, J. Eljaik, F . Romano, A. Del Prete, and D. Pucci, “icub whole-body control through force regulation on rigid noncoplanar contacts, ” F rontier s in Robotics and AI , vol. 2, no. 6, 2015. [39] A. W ¨ achter and L. T . Biegler , “On the implementation of an interior- point filter line-search algorithm for large-scale nonlinear programming, ” Mathematical Pro gramming , vol. 106, no. 1, pp. 25–57, Mar 2006. [40] R. Olfati-Saber , “Nonlinear control of underactuated mechanical systems with application to robotics and aerospace vehicles, ” Ph.D. dissertation, Massachusetts Institute of T echnology , 2001.