# Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Background

#### 2.1. Notation

- $\mathcal{I}$ denotes an inertial frame of reference.
- ${I}_{n\times n}\in {\mathbb{R}}^{n\times n}$ denotes the identity matrix of size n.
- ${{}^{\mathcal{A}}p}_{\mathcal{B}}\in {\mathbb{R}}^{3}$ is the position of the origin of the frame $\mathcal{B}$ with respect to the frame $\mathcal{A}$.
- ${{}^{\mathcal{A}}R}_{\mathcal{B}}\in SO\left(3\right)$ represents the rotation matrix of the frames $\mathcal{B}$ with respect to $\mathcal{A}$.
- ${{}^{\mathcal{A}}\omega}_{\mathcal{B}}\in {\mathbb{R}}^{3}$ is the angular velocity of the frame $\mathcal{B}$ with respect to $\mathcal{A}$, expressed in $\mathcal{A}$.
- The operator $\mathrm{tr}\left(\phantom{\rule{0.166667em}{0ex}}\xb7\phantom{\rule{0.166667em}{0ex}}\right):{\mathbb{R}}^{3\times 3}\to \mathbb{R}$ denotes the trace of a matrix, such that given $A\in {\mathbb{R}}^{3\times 3}$, it is defined as $\mathrm{tr}\left(A\right):={A}_{1,1}+{A}_{2,2}+{A}_{3,3}$.
- The operator $\mathrm{sk}\left(\phantom{\rule{0.166667em}{0ex}}\xb7\phantom{\rule{0.166667em}{0ex}}\right):{\mathbb{R}}^{3\times 3}\to so\left(3\right)$ denotes skew-symmetric operation of a matrix, such that given $A\in {\mathbb{R}}^{3\times 3}$, it is defined as $\mathrm{sk}\left(A\right):=(A-{A}^{\top})/2$.
- The operator $S\left(\phantom{\rule{0.166667em}{0ex}}\xb7\phantom{\rule{0.166667em}{0ex}}\right):{\mathbb{R}}^{3}\to so\left(3\right)$ denotes skew-symmetric vector operation, such that given two vectors $v,u\in {\mathbb{R}}^{3}$, it is defined as $v\times u=S\left(v\right)u$.
- The vee operator $\phantom{\rule{0.166667em}{0ex}}\xb7{\phantom{\rule{0.166667em}{0ex}}}^{\vee}:so\left(3\right)\to {\mathbb{R}}^{3}$ denotes the inverse of the skew-symmetric vector operator, such that given a matrix $A\in so\left(3\right)$ and a vector $u\in {\mathbb{R}}^{3}$, it is defined as $Au={A}^{\vee}\times u$.
- The operator ${\left(\right)}_{\phantom{\rule{0.166667em}{0ex}}}$ indicates the L2-norm of a vector, such that given a vector $v\in {\mathbb{R}}^{3}$, it is defined as ${\u2225v\u2225}_{2}=\sqrt{{v}_{1}^{2}+{v}_{2}^{2}+{v}_{3}^{2}}$.

#### 2.2. Modeling

#### 2.3. Problem Statement

**Problem**

**1.**

#### 2.4. Dynamical Inverse Kinematics Optimization

- correction of the measured velocity according to the current error,
- inversion of the model differential kinematics to compute the state velocity $\nu \left(t\right)$,
- integration of state velocity to compute the configuration $q\left(t\right)$.

## 3. Method

#### 3.1. Velocity Correction Using Rotation Matrix

**Corollary**

**1.**

**Remark**

**1.**

**Remark**

**2.**

#### 3.2. Constrained Inverse Differential Kinematics

#### 3.2.1. Joint Limit Avoidance

#### 3.2.2. Linear Joint Space Constraints

**Remark**

**3.**

#### 3.3. Numerical Integration

## 4. Experiments

#### 4.1. Motion Data Acquisition

#### 4.2. Models

#### 4.3. Robot Experiments

## 5. Results

#### 5.1. Instantaneous Optimization

#### 5.2. Dynamical Optimization

## 6. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## Appendix A. Proof of Lemma 1

## References

- Dariush, B.; Gienger, M.; Arumbakkam, A.; Goerick, C.; Zhu, Y.; Fujimura, K. Online and markerless motion retargeting with kinematic constraints. In Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 191–198. [Google Scholar] [CrossRef]
- Peng, X.B.; Abbeel, P.; Levine, S.; van de Panne, M. DeepMimic: Example-guided Deep Reinforcement Learning of Physics-based Character Skills. ACM Trans. Graph.
**2018**, 37, 143:1–143:14. [Google Scholar] [CrossRef][Green Version] - Aggarwal, J.K.; Cai, Q. Human motion analysis: A review. Comput. Vis. Image Underst.
**1999**, 73, 428–440. [Google Scholar] [CrossRef] - Zhu, R.; Zhou, Z. A real-time articulated human motion tracking using tri-axis inertial/magnetic sensors package. IEEE Trans. Neural Syst. Rehabil. Eng.
**2004**, 12, 295–302. [Google Scholar] [CrossRef] [PubMed] - Filippeschi, A.; Schmitz, N.; Miezal, M.; Bleser, G.; Ruffaldi, E.; Stricker, D. Survey of motion tracking methods based on inertial sensors: A focus on upper limb human motion. Sensors
**2017**, 17, 1257. [Google Scholar] [CrossRef] [PubMed][Green Version] - Shio, A.; Sklansky, J. Segmentation of people in motion. In Proceedings of the IEEE Workshop on Visual Motion, Princeton, NJ, USA, 7–9 October 1991; pp. 325–332. [Google Scholar] [CrossRef]
- Leung, M.K.; Yang, Y.-H. First Sight: A human body outline labeling system. IEEE Trans. Pattern Anal. Mach. Intell.
**1995**, 17, 359–377. [Google Scholar] [CrossRef] - Niyogi, S.A.; Adelson, E.H. Analyzing and recognizing walking figures in XYT. In Proceedings of the 1994 IEEE Conference on Computer Vision and Pattern Recognition, Seattle, WA, USA, 21–23 June 1994; pp. 469–474. [Google Scholar] [CrossRef]
- Bharatkumar, A.G.; Daigle, K.E.; Pandy, M.G.; Cai, Q.; Aggarwal, J.K. Lower limb kinematics of human walking with the medial axis transformation. In Proceedings of the 1994 IEEE Workshop on Motion of Non-Rigid and Articulated Objects, Austin, TX, USA, 11–12 November 1994; pp. 70–76. [Google Scholar] [CrossRef]
- Wachter, S.; Nagel, H.H. Tracking persons in monocular image sequences. Comput. Vis. Image Underst.
**1999**, 74, 174–192. [Google Scholar] [CrossRef] - Gall, J.; Stoll, C.; De Aguiar, E.; Theobalt, C.; Rosenhahn, B.; Seidel, H.P. Motion capture using joint skeleton tracking and surface estimation. In Proceedings of the 2009 IEEE Conference on Computer Vision and Pattern Recognition, Miami, FL, USA, 20–25 June 2009; pp. 1746–1753. [Google Scholar]
- Monzani, J.S.; Baerlocher, P.; Boulic, R.; Thalmann, D. Using an Intermediate Skeleton and Inverse Kinematics for Motion Retargeting; Computer Graphics Forum; Wiley Online Library: Oxford, UK, 2000; Volume 19, pp. 11–19. [Google Scholar]
- Pons-Moll, G.; Baak, A.; Gall, J.; Leal-Taixe, L.; Mueller, M.; Seidel, H.P.; Rosenhahn, B. Outdoor human motion capture using inverse kinematics and von mises-fisher sampling. In Proceedings of the 2011 International Conference on Computer Vision, Barcelona, Spain, 6–13 November 2011; pp. 1243–1250. [Google Scholar]
- Ganapathi, V.; Plagemann, C.; Koller, D.; Thrun, S. Real time motion capture using a single time-of-flight camera. In Proceedings of the 2010 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, San Francisco, CA, USA, 13–18 June 2010; pp. 755–762. [Google Scholar]
- Aristidou, A.; Lasenby, J. Motion capture with constrained inverse kinematics for real-time hand tracking. In Proceedings of the 2010 4th International Symposium on Communications, Control and Signal Processing (ISCCSP), Limassol, Cyprus, 3–5 March 2010; pp. 1–5. [Google Scholar]
- Traversaro, S.; Saccon, A. Multibody Dynamics Notation; Tech. Rep.; Technische Universiteit Eindhoven: Eindhoven, The Netherlands, 2016. [Google Scholar]
- Goldenberg, A.; Benhabib, B.; Fenton, R. A complete generalized solution to the inverse kinematics of robots. IEEE J. Robot. Autom.
**1985**, 1, 14–20. [Google Scholar] [CrossRef] - Buss, S.R. Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods. IEEE J. Robot. Autom.
**2004**, 17, 16. [Google Scholar] - Kok, M.; Hol, J.; Schön, T. An optimization-based approach to human body motion capture using inertial sensors. In Proceedings of the 19th World Congress of the International Federation of Automatic Control (IFAC), Cape Town, South Africa, 24–29 August 2014; pp. 79–85. [Google Scholar]
- Von Marcard, T.; Rosenhahn, B.; Black, M.J.; Pons-Moll, G. Sparse Inertial Poser: Automatic 3D Human Pose Estimation from Sparse Imus; Computer Graphics Forum; Wiley Online Library: Oxford, UK, 2017; Volume 36, pp. 349–360. [Google Scholar]
- Aristidou, A.; Lasenby, J. FABRIK: A fast, iterative solver for the Inverse Kinematics problem. Graph. Models
**2011**, 73, 243–260. [Google Scholar] [CrossRef] - Grochow, K.; Martin, S.L.; Hertzmann, A.; Popović, Z. Style-based inverse kinematics. ACM Trans. Graph. (TOG)
**2004**, 23, 522–531. [Google Scholar] [CrossRef][Green Version] - Tolani, D.; Goswami, A.; Badler, N.I. Real-Time Inverse Kinematics Techniques for Anthropomorphic Limbs. Graph. Models
**2000**, 62, 353–388. [Google Scholar] [CrossRef][Green Version] - Sciavicco, L.; Siciliano, B. A dynamic solution to the inverse kinematic problem for redundant manipulators. In Proceedings of the 1987 IEEE International Conference on Robotics and Automation, Raleigh, NC, USA, 31 March–3 April 1987; Volume 4, pp. 1081–1087. [Google Scholar]
- Balestrino, A.; De Maria, G.; Sciavicco, L. Robust control of robotic manipulators. IFAC Proc. Vol.
**1984**, 17, 2435–2440. [Google Scholar] [CrossRef] - Wolovich, W.A.; Elliott, H. A computational technique for inverse kinematics. In Proceedings of the 23rd IEEE Conference on Decision and Control, Las Vegas, NV, USA, 12–14 December 1984; pp. 1359–1363. [Google Scholar]
- Whitney, D.E. Resolved motion rate control of manipulators and human prostheses. IEEE Trans. Man-Mach. Syst.
**1969**, 10, 47–53. [Google Scholar] [CrossRef] - Chiacchio, P.; Chiaverini, S.; Sciavicco, L.; Siciliano, B. Closed-loop inverse kinematics schemes for constrained redundant manipulators with task space augmentation and task priority strategy. Int. J. Robot. Res.
**1991**, 10, 410–425. [Google Scholar] [CrossRef] - Andrews, S.; Huerta, I.; Komura, T.; Sigal, L.; Mitchell, K. Real-time physics-based motion capture with sparse sensors. In Proceedings of the 13th European Conference on Visual Media Production (CVMP 2016), London, UK, 12–13 December 2016; pp. 1–10. [Google Scholar]
- Suleiman, W.; Ayusawa, K.; Kanehiro, F.; Yoshida, E. On prioritized inverse kinematics tasks: Time-space decoupling. In Proceedings of the 2018 IEEE 15th International Workshop on Advanced Motion Control (AMC), Tokyo, Japan, 9–11 March 2018; pp. 108–113. [Google Scholar]
- Latella, C.; Lorenzini, M.; Lazzaroni, M.; Romano, F.; Traversaro, S.; Akhras, M.A.; Pucci, D.; Nori, F. Towards real-time whole-body human dynamics estimation through probabilistic sensor fusion algorithms. Auton. Robots
**2019**, 43, 1591–1603. [Google Scholar] [CrossRef][Green Version] - Kok, M.; Hol, J.D.; Schön, T.B. Using inertial sensors for position and orientation estimation. arXiv
**2017**, arXiv:1704.06053. [Google Scholar] - Olfati-Saber, R. Nonlinear Control of Underactuated Mechanical Systems with Application to Robotics and Aerospace Vehicles. Ph.D. Thesis, Massachusetts Institute of Technology, Cambridge, MA, USA, 2001. [Google Scholar]
- Ogata, K. Discrete-Time Control Systems; Prentice Hall: Englewood Cliffs, NJ, USA, 1995; Volume 2. [Google Scholar]
- Sciavicco, L.; Siciliano, B. A solution algorithm to the inverse kinematic problem for redundant manipulators. IEEE J. Robot. Autom.
**1988**, 4, 403–410. [Google Scholar] [CrossRef] - Kanoun, O.; Lamiraux, F.; Wieber, P. Kinematic Control of Redundant Manipulators: Generalizing the Task-Priority Framework to Inequality Task. IEEE Trans. Robot.
**2011**, 27, 785–792. [Google Scholar] [CrossRef][Green Version] - Rao, C.R.; Mitra, S.K. Generalized inverse of a matrix and its applications. In Proceedings of the Sixth Berkeley Symposium on Mathematical Statistics and Probability, Volume 1: Theory of Statistics; The Regents of the University of California: Berkeley, CA, USA, 1972. [Google Scholar]
- Stellato, B.; Banjac, G.; Goulart, P.; Bemporad, A.; Boyd, S. OSQP: An Operator Splitting Solver for Quadratic Programs. Math. Program. Comput.
**2020**. [Google Scholar] [CrossRef][Green Version] - Davis, P.J.; Rabinowitz, P. Methods of Numerical Integration; Courier Corporation: North Chelmsford, MA, USA, 2007. [Google Scholar]
- Boyle, M. The integration of angular velocity. Adv. Appl. Clifford Algebr.
**2017**, 27, 2345–2374. [Google Scholar] [CrossRef][Green Version] - Gros, S.; Zanon, M.; Diehl, M. Baumgarte stabilisation over the SO(3) rotation group for control. In Proceedings of the 2015 54th IEEE Conference on Decision and Control (CDC), Osaka, Japan, 15–18 December 2015; pp. 620–625. [Google Scholar] [CrossRef]
- Roetenberg, D.; Luinge, H.; Slycke, P. Xsens MVN: Full 6DOF Human Motion Tracking Using Miniature Inertial Sensors; Xsens Technologies B.V.: Enschede, The Netherlands, 2009. [Google Scholar]
- Metta, G.; Fitzpatrick, P.; Natale, L. YARP: yet another robot platform. Int. J. Adv. Robot. Syst.
**2006**, 3, 8. [Google Scholar] [CrossRef][Green Version] - Moll, J.; Wright, V. Normal range of spinal mobility. An objective clinical study. Ann. Rheum. Dis.
**1971**, 30, 381. [Google Scholar] [CrossRef] [PubMed][Green Version] - Gerhardt, J. Clinical measurements of joint motion and position in the neutral-zero method and SFTR recording: Basic principles. Int. Rehabil. Med.
**1983**, 5, 161–164. [Google Scholar] [CrossRef] [PubMed] - Alison Middleditch, M.; Jean Oliver, M. Functional Anatomy of the Spine; Elsevier Health Sciences: Oxford, UK, 2005; ISBN 0-7506-2717-4. [Google Scholar]
- Natale, L.; Bartolozzi, C.; Pucci, D.; Wykowska, A.; Metta, G. icub: The not-yet-finished story of building a robot child. Sci. Robot.
**2017**, 2, eaaq1026. [Google Scholar] [CrossRef] - Barnes, J. An algorithm for solving non-linear equations based on the secant method. Comput. J.
**1965**, 8, 66–72. [Google Scholar] [CrossRef][Green Version] - Fletcher, R. Generalized Inverse Methods for the Best Least Squares Solution of Systems of Non-Linear Equations. Comput. J.
**1968**, 10, 392–399. [Google Scholar] [CrossRef][Green Version] - Blanchini, F.; Fenu, G.; Giordano, G.; Pellegrino, F.A. A convex programming approach to the inverse kinematics problem for manipulators under constraints. Eur. J. Control
**2017**, 33, 11–23. [Google Scholar] [CrossRef][Green Version] - Nori, F.; Traversaro, S.; Eljaik, J.; Romano, F.; Del Prete, A.; Pucci, D. iCub Whole-body Control through Force Regulation on Rigid Noncoplanar Contacts. Front. Robot. AI
**2015**, 2, 6. [Google Scholar] [CrossRef][Green Version] - Wächter, A.; Biegler, L.T. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Math. Program.
**2006**, 106, 25–57. [Google Scholar] [CrossRef]

**Figure 1.**Motion tracking of a human running on a treadmill using an human model, on the center, and a humanoid model, on the right.

**Figure 3.**Constrained configuration space for the elbow joints of the iCub model. Blue and red lines represent respectively upper and lower joint velocity limits, depending on joint angle. The yellow area represents the joint configuration space. According to limit avoidance 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). Yellow lines represent the joint configuration trajectory computed via inverse kinematics algorithm, while the purple line represents the trajectory tracked by a real robot.

**Figure 4.**Model of the human (

**left**) and iCub (

**right**) in T-pose, with the corresponding links frame definition.

**Figure 6.**A floating base model with eight links and four orientation targets (${{}^{\mathcal{I}}R}_{\mathcal{B}}$, ${{}^{\mathcal{I}}R}_{\mathcal{C}}$, ${{}^{\mathcal{I}}R}_{\mathcal{D}}$, ${{}^{\mathcal{I}}R}_{\mathcal{E}}$) can be divided into three subsystems, with a pair of orientation targets each, in order to solve the inverse kinematics problem as pair-wised instantaneous optimization.

**Figure 7.**Comparison of the performance of inverse kinematics methods (whole-body, pair-wised, and dynamical) for three models (two humans, and iCub humanoid) in three different scenarios (T-pose, Walking, and Running). Each line contains the boxplots for a different performance evaluation metric, on the top the overall error for the orientation targets as base 10 logarithm of mean normalized trace error, in the middle line the overall error for the angular velocities as base 10 logarithm of root mean squared error, and at the bottom the computational time. Logarithmic metrics allows to compare metrics characterized by different order of magnitude in the different scenarios.

**Figure 8.**Convergence of the dynamical inverse kinematics optimization for static T-pose using a 66 degrees of freedom (DoF) model, starting from zero configuration. Convergence rate depends on the magnitude of the gain K.

**Figure 9.**Joint configuration of the iCub model obtained from human motion data using dynamical inverse kinematics. The plots show both the desired joint configuration computed by inverse kinematics, and the joint configuration measured from the robot. Dashed lines represent the joint limits.

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Rapetti, L.; Tirupachuri, Y.; Darvish, K.; Dafarra, S.; Nava, G.; Latella, C.; Pucci, D.
Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics. *Algorithms* **2020**, *13*, 266.
https://doi.org/10.3390/a13100266

**AMA Style**

Rapetti L, Tirupachuri Y, Darvish K, Dafarra S, Nava G, Latella C, Pucci D.
Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics. *Algorithms*. 2020; 13(10):266.
https://doi.org/10.3390/a13100266

**Chicago/Turabian Style**

Rapetti, Lorenzo, Yeshasvi Tirupachuri, Kourosh Darvish, Stefano Dafarra, Gabriele Nava, Claudia Latella, and Daniele Pucci.
2020. "Model-Based Real-Time Motion Tracking Using Dynamical Inverse Kinematics" *Algorithms* 13, no. 10: 266.
https://doi.org/10.3390/a13100266