Efficient geometric linearization of moving-base rigid robot dynamics

Martijn Bos; Silvio Traversaro; Daniele Pucci; Alessandro Saccon

doi:10.3934/jgm.2022009

Article Contents

2022, Volume 14, Issue 4: 507-543. Doi: 10.3934/jgm.2022009

This issue Previous Article Safe trajectory tracking for underactuated vehicles with partially unknown dynamics Next Article Riemannian cubics close to geodesics at the boundaries

Efficient geometric linearization of moving-base rigid robot dynamics

1.
Smart Robotics, De Maas 8, 5684 PL Best, the Netherlands
2.
Artificial Mechanical Intelligence research line, Istituto Italiano di Tecnologia, Via S. Quirico 19D, 16163 Genoa, Italy
3.
Department of Mechanical Engineering, Eindhoven University of Technology, Groene Loper 3, PO Box 513, 5600 MB Eindhoven, the Netherlands

^*Corresponding author: Alessandro Saccon
^*Corresponding author: Alessandro Saccon

Dedicated to Professor Tony Bloch on the occasion of his 65th birthdaybr
¹This work was partly performed while the author was affiliated to the Eindhoven University of Technology.

Received: January 31, 2021

Revised: February 28, 2022

Published: December 2022

Abstract Full Text(HTML) Figure(5) / Table(9) Related Papers Cited by

Abstract

The linearization of the equations of motion of a robotics system about a given state-input trajectory, including a controlled equilibrium state, is a valuable tool for model-based planning, closed-loop control, gain tuning, and state estimation. Contrary to the case of fixed based manipulators with prismatic or revolute joints, the state space of moving-base robotic systems such as humanoids, quadruped robots, or aerial manipulators cannot be globally parametrized by a finite number of independent coordinates. This impossibility is a direct consequence of the fact that the state of these systems includes the system's global orientation, formally described as an element of the special orthogonal group SO(3). As a consequence, obtaining the linearization of the equations of motion for these systems is typically resolved, from a practical perspective, by locally parameterizing the system's attitude by means of, e.g., Euler or Cardan angles. This has the drawback, however, of introducing artificial parameterization singularities and extra derivative computations. In this contribution, we show that it is actually possible to define a notion of linearization that does not require the use of a local parameterization for the system's orientation, obtaining a mathematically elegant, recursive, and singularity-free linearization for moving-based robot systems. Recursiveness, in particular, is obtained by proposing a nontrivial modification of existing recursive algorithms to allow for computations of the geometric derivatives of the inverse dynamics and the inverse of the mass matrix of the robotic system. The correctness of the proposed algorithm is validated by means of a numerical comparison with the result obtained via geometric finite difference.

Keywords:

Mathematics Subject Classification: Primary: 70E55, 22Exx, 93-xx, 65-xx.

Citation:

FullText(HTML)

Figure 1. An example of body numbering for moving-base multibody systems with tree-topology kinematics. The moving base is denoted $ 0 $. In the image, $ j > i $ and $ k > i $. The parent body of body $ i $ is denoted $ \lambda(i) $

Download: Full-size image PowerPoint slide

Figure 2. Joint numbering convention. Joints are numbered according to the successor body

Download: Full-size image PowerPoint slide

Figure 3. A general example of body and joint frames

Download: Full-size image PowerPoint slide

Figure 4. The moving-base multibody system topology selected for numerical validation; (a) reference configuration, showing a branched system with a moving base (yellow) with revolute (green), prismatic (red), and helical (blue) joints; (b) perturbed configuration, where all joint position are equal to $ 0.3 $ (representing meters or radians, depending on the joint type)

Download: Full-size image PowerPoint slide

Figure 5. Parametric study with respect to $ \delta $ of the errors illustrating how the maximal and average error of the finite difference approximation is reducing as $ \delta $ is reduced, up to the point where round off errors become dominant

Download: Full-size image PowerPoint slide

Table 1. Overview of existing approaches and their compliance with the four requirements (MB, SF, RF, ED)

Approach	SF	MB	RF	ED
Geometric linearization [42,4,39,37,12,25,23,44]	√			√
Sensitivity for multibody systems on Lie groups [45,11]	√	√		√
Recursive algorithms [14,26,49]	√		√	√
Finite differences [46,28]	√	√
Lagrangian derivation [16]		√		√
Automatic differentiation [18,31]		√	√	√
Analytical derivation [8,43,32]		√	√	√
This manuscript	√	√	√	√

| Show Table

DownLoad: CSV

Table 2. Introduction of Eindhoven-Genoa (EG) notation

EG	Dimension	Explanation
$ ^A \mathbf H_B $	$ {\rm{SE}}(3) $	Transformation matrix of frame $ B $ w.r.t. frame $ A $
$ ^A \mathbf R_B $	$ {\rm{SO}}(3) $	Rotation matrix of frame $ B $ w.r.t. frame $ A $
$ ^A \mathbf o_B $	$ \mathbb{R}^3 $	Origin of frame $ B $ w.r.t. frame $ A $
$ ^C \mathbf v_{A, B} $	$ \mathbb{R}^6 $	Twist of frame $ B $ w.r.t. frame $ A $ expressed in frame $ C $
$ ^A \mathbf a_{A, B} $	$ \mathbb{R}^6 $	Intrinsic [47,Section 5.1] acceleration of frame $ B $ w.r.t. frame $ A $ expressed in frame $ C $
$ _A \mathbf f $	$ \mathbb{R}^6 $	Wrench w.r.t. frame $ A $ (often written as $ \mathbf b $ for bias wrench)
$ ^A \mathbf X_B $	$ \mathbb{R}^{6 \times 6} $	Velocity transformation of frame $ B $ w.r.t. frame $ A $
$ _A \mathbf X^B $	$ \mathbb{R}^{6 \times 6} $	Wrench transformation of frame $ B $ w.r.t. frame $ A $
$ \mathbf s $	$ \mathbb{R}^{n_J} $	Generalized position vector or system shape
$ \mathbf r $	$ \mathbb{R}^{n_J} $	Generalized velocity vector
$ {\bf{\tau}} $	$ \mathbb{R}^{n_J} $	Joint torques or generalized forces vector
$ ^C \mathbf v_{A, B} \times $	$ \mathbb{R}^{6 \times 6} $	6D twist cross product on $ \mathbb{R}^6 $ (defined in Section 3.1)
$ ^C \mathbf v_{A, B} \bar{\times}^* $	$ \mathbb{R}^{6 \times 6} $	6D twist/wrench cross product on $ \mathbb{R}^6 $

| Show Table

DownLoad: CSV

Table 3. Kinematic, dynamics, and set theoretic quantities used in the recursive algorithms presented in this section

EG	Dim.	Explanation
$ _C^ {\phantom A} \mathbb{M}^{ \mathcal{B} i}_C $	$ \mathbb{R}^{6 \times 6} $	Inertia matrix of body $ i $
$ _C^ {\phantom A} \mathbb{M}^{ \mathcal{B} i, A}_C $	$ \mathbb{R}^{6 \times 6} $	Articulated-body inertia matrix of body $ i $
$ _C^ {\phantom A} \mathbb{M}^{ \mathcal{B} i, a}_C $	$ \mathbb{R}^{6 \times 6} $	Apparent articulated-body inertia matrix of body $ i $
$ _C^ {\phantom A} \mathbb{M}^{ \mathcal{B} i, c}_C $	$ \mathbb{R}^{6 \times 6} $	Composite rigid body inertia matrix of body $ i $
$ ^C \mathbf v_{A, i} $	$ \mathbb{R}^6 $	Twist or spatial velocity of frame $ i $ w.r.t frame $ A $
$ ^C\dot{ \mathbf v}_{A, i} $	$ \mathbb{R}^6 $	Apparent acceleration of frame $ i $ w.r.t frame $ A $
$ ^C \mathbf a_{A, i} $	$ \mathbb{R}^6 $	Intrinsic acceleration of frame $ i $ w.r.t frame $ A $
$ ^C \mathbf a_{grav} $	$ \mathbb{R}^6 $	Intrinsic gravitational acceleration
$ ^C \mathbf a_i^r $	$ \mathbb{R}^6 $	Intrinsic acceleration relative to the moving-base acceleration, plus the gravitational acceleration of body $ i $
$ ^C \mathbf a_i^{vp} $	$ \mathbb{R}^6 $	Intrinsic acceleration that only accounts for the velocity product terms of body $ i $
$ _C \mathbf m_{ \mathcal{B} i} $	$ \mathbb{R}^6 $	Spatial momentum of body $ i $
$ ^C \mathbf \Gamma_{\lambda(i), i} $	$ \mathbb{R}^6 $	Joint velocity subspace matrix of joint $ i $
$ ^i \mathbf v_{\lambda(i), i} $	$ \mathbb{R}^6 $	Velocity of joint $ i $
$ _C \mathbf b_{ \mathcal{B} i} $	$ \mathbb{R}^6 $	Bias wrench acting on body $ i $
$ _C \mathbf b^c_{ \mathcal{B} i} $	$ \mathbb{R}^6 $	Composite rigid body bias wrench acting on body $ i $
$ _C \mathbf b^{vp}_{ \mathcal{B} 0} $	$ \mathbb{R}^6 $	Bias wrench of the moving-base with zero joint acceleration
$ ^C \mathbf X_D $	$ \mathbb{R}^{6 \times 6} $	Velocity transformation from frame $ D $ to frame $ C $
$ _C \mathbf X^D $	$ \mathbb{R}^{6 \times 6} $	Wrench transformation from frame $ D $ to frame $ C $
$ _C \mathbf U_{ \mathcal{B} i} $	$ \mathbb{R}^6 $	Subexpression used in ABA
$ \mathbf D_{ \mathcal{B} i} $	$ \mathbb{R} $	Subexpression used in ABA
$ \mathbf u_{ \mathcal{B} i} $	$ \mathbb{R} $	Subexpression used in ABA
$ _C \mathbf F_{ \mathcal{B} i} $	$ \mathbb{R}^6 $	Required wrench to support unit acceleration of joint $ i $
$ \mathcal{F}_i $	$ \mathbb{R}^{6 \times n_J} $	Wrench set collecting the contributions of the supporting tree rooted at $ i $
$ \mathcal{P}_i $	$ \mathbb{R}^{6 \times n_J} $	Motion set which contains the contributions of all parents of joint $ i $

| Show Table

DownLoad: CSV

Table 4. EIDAmb

Inputs: model, $ \mathbf s, \mathbf r, \dot{ \mathbf r}, \quad ^A \mathbf H_0, \quad ^A \mathbf v_{A, 0}, \quad ^0 \mathbf a_{A, 0}, \quad ^A \mathbf a_{grav} $
Line	EIDAmb
1	$ ^0 \mathbf H_A = ^A \mathbf H_0^{-1} $
2	$ ^0 \mathbf R_A = ^0 \mathbf H_A[1\!\!:\!\!3, 1\!\!:\!\!3] $
3	$ ^0 \mathbf o_A = ^0 \mathbf H_A[1\!\!:\!\!3, 4] $
4	$ ^0 \mathbf X_A = \begin{bmatrix} \quad ^0 \mathbf R_A & \quad ^0 \mathbf o_A^\wedge \quad ^0 \mathbf R_A \\ 0_{3\times3} & \quad ^0 \mathbf R_A \end{bmatrix} $
5	$ \mathbf v_0 = ^0 \mathbf X_A \quad ^A \mathbf v_{A, 0} $
6	$ \mathbf a_0^r = ^0 \mathbf X_A \quad ^A \mathbf a_{grav} $
7*	$ \mathbf a_0^{vp} = \mathbf a_0^r $
8	$ \mathbb{M}_{ \mathcal{B} 0}^c = \mathbb{M}_{ \mathcal{B} 0} $
9	$ \mathbf m_{ \mathcal{B} 0} = \mathbb{M}_{ \mathcal{B} 0} \mathbf v_0 $
10	$ \mathbf b_{ \mathcal{B} 0}^c = \mathbb{M}_{ \mathcal{B} 0} \mathbf a_0^r + \mathbf v_0 \bar{\times}^* \mathbf m_{ \mathcal{B} 0} $
11*	$ \mathbf b_{ \mathcal{B} 0}^{vp} = \mathbf b_{ \mathcal{B} 0}^c $
12	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $
13	$ \quad [^i \mathbf X_{\lambda(i)\|i}, \mathbf \Gamma_{ \mathcal{J} i} ] = $ jcalc(jtype($ i $)$ , \mathbf s_i $)
14	$ \quad \mathbf v_{ \mathcal{J} i} = \mathbf \Gamma_{ \mathcal{J} i} \mathbf r_i $
15	$ \quad \quad ^i \mathbf X_{\lambda(i)} = ^i \mathbf X_{\lambda(i)\|i} \quad ^{\lambda(i)\|i} \mathbf X_{\lambda(i)} $
16	$ \quad \mathbf v_i = ^i \mathbf X_{\lambda(i)} \mathbf v_{\lambda(i)} + \mathbf v_{ \mathcal{J} i} $
17	$ \quad \mathbf a_i^r = ^i \mathbf X_{\lambda(i)} \mathbf a_{\lambda(i)}^r + \mathbf \Gamma_{ \mathcal{J} i} \dot{ \mathbf r}_i + \mathbf v_i \times \mathbf v_{ \mathcal{J} i} $
18*	$ \quad \mathbf a_i^{vp} = ^i \mathbf X_{\lambda(i)} \mathbf a_{\lambda(i)}^{vp} + \mathbf v_i \times \mathbf v_{ \mathcal{J} i} $
19	$ \quad \mathbb{M}_{ \mathcal{B} i}^c = \mathbb{M}_{ \mathcal{B} i} $
20	$ \quad \mathbf m_{ \mathcal{B} i} = \mathbb{M}_{ \mathcal{B} i} \mathbf v_i $
21	$ \quad \mathbf b_{ \mathcal{B} i}^c = \mathbb{M}_{ \mathcal{B} i} \mathbf a_i^r + \mathbf v_i \bar{\times}^* \mathbf m_{ \mathcal{B} i} $
22*	$ \quad \mathbf b_{ \mathcal{B} i}^{vp} = \mathbb{M}_{ \mathcal{B} i} \mathbf a_i^{vp} + \mathbf v_i \bar{\times}^* \mathbf m_{ \mathcal{B} i} $
23	$ \mathbf{end} $
24	$ \mathbf{for}\ i=n_B\ \mathbf{to}\ 1\ \mathbf{do} $
25	$ \quad \mathbb{M}_{ \mathcal{B} \lambda(i)}^c = \mathbb{M}_{ \mathcal{B} \lambda(i)}^c + \quad _{\lambda(i)} \mathbf X^i \mathbb{M}_{ \mathcal{B} i}^c \quad ^i \mathbf X_{\lambda(i)} $
26	$ \quad \mathbf b_{ \mathcal{B} \lambda(i)}^c = \mathbf b_{ \mathcal{B} \lambda(i)}^c + \quad _{\lambda(i)} \mathbf X^i \mathbf b_{ \mathcal{B} i}^c $
27*	$ \quad \mathbf b_{ \mathcal{B} \lambda(i)}^{vp} = \mathbf b_{ \mathcal{B} \lambda(i)}^{vp} + \quad _{\lambda(i)} \mathbf X^i \mathbf b_{ \mathcal{B} i}^{vp} $
28	$ \mathbf{end} $
29	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $
30	$ \quad \quad ^i \mathbf a_{A, 0} = ^i \mathbf X_{\lambda(i)} \quad ^{\lambda(i)} \mathbf a_{A, 0} $
31	$ \quad {\bf{\tau}}_i = \mathbf \Gamma_{ \mathcal{J} i}^T ( \mathbb{M}_{ \mathcal{B} i}^c \quad ^i \mathbf a_{A, 0} + \mathbf b_{ \mathcal{B} i}^c ) $
32**	$ \quad \quad _i \mathbf F_{ \mathcal{B} i} = \mathbb{M}_{ \mathcal{B} i}^c \mathbf \Gamma_{ \mathcal{J} i} $
33**	$ \quad j = i $
34**	$ \quad \mathbf{while}\ \lambda(j) > 0 $
35**	$ \qquad \quad _{\lambda(j)} \mathbf F_{ \mathcal{B} i} = _{\lambda(j)} \mathbf X^j \quad _j \mathbf F_{ \mathcal{B} i} $
36**	$ \qquad j = \lambda(j) $
37**	$ \quad \mathbf{end} $
38**	$ \quad \quad _ 0 \mathbf F_{ \mathcal{B} i} = _0 \mathbf X^j \quad _j \mathbf F_{ \mathcal{B} i} $
39	$ \mathbf{end} $
40***	$ \bar{ {\bf{\tau}}}_b = \mathbb{M}_{ \mathcal{B} 0}^c \quad ^0 \mathbf a_{A, 0} + \quad _0 \mathbf F \dot{ \mathbf r} + \mathbf b_{ \mathcal{B} 0}^{vp} $
Output: $ \overline{ID} = \bar{ {\bf{\tau}}} = [ {\bf{\tau}} ; \bar{ {\bf{\tau}}}_b] $

| Show Table

DownLoad: CSV

Table 5. Left-trivialized derivatives of the extended inverse dynamics with respect to the transformation matrix $ \mathbf H $

Inputs: All outputs and intermediate variables of EIDAmb
Line	Algorithm	Line in EIDAmb
1	$ \dfrac{ \tilde{\partial} \mathbf v_0}{\partial \mathbf H} = \begin{bmatrix} \quad ^0 \mathbf R_A \quad ^A {\bf{\omega}}_{A, 0}^\wedge \quad ^0 \mathbf R_A^T & \quad ^0 \mathbf R_A (^A {{\mathit{\boldsymbol{v}}}}_{A, 0} - \quad ^A \mathbf o_0^\wedge \quad ^A {\bf{\omega}}_{A, 0} )^\wedge \quad ^0 \mathbf R_A^T & 0_{3 \times 3} \\ \quad ^0 \mathbf R_A \quad ^A {\bf{\omega}}_{A, 0}^\wedge \quad ^0 \mathbf R_A^T \end{bmatrix} $	5
2	$ \dfrac{ \tilde{\partial} \mathbf a^r_0}{\partial \mathbf H} = \begin{bmatrix} \quad ^0 \mathbf R_A \quad ^A {\bf{\alpha}}_{grav}^\wedge \quad ^0 \mathbf R_A^T & \quad ^0 \mathbf R_A (^A {{\mathit{\boldsymbol{a}}}}_{grav} - \quad ^A \mathbf o_0^\wedge \quad ^A {\bf{\alpha}}_{grav} )^\wedge \quad ^0 \mathbf R_A^T\\ 0_{3 \times 3} & \quad ^0 \mathbf R_A \quad ^A {\bf{\alpha}}_{grav}^\wedge \quad ^0 \mathbf R_A^T \end{bmatrix} $	6
3	$ \dfrac{ \tilde{\partial} \mathbf m_{ \mathcal{B} 0}}{\partial \mathbf H} = \mathbb{M}_{ \mathcal{B} 0} \dfrac{ \tilde{\partial} \mathbf v_0}{\partial \mathbf H} $	9
4	$ \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} 0}}{\partial \mathbf H} = \mathbb{M}_{ \mathcal{B} 0} \dfrac{ \tilde{\partial} \mathbf a^r_0}{\partial \mathbf H} + \dfrac{ \tilde{\partial} \mathbf v_0}{\partial \mathbf H} {\bar\times^} \mathbf m_{ \mathcal{B} 0} + \mathbf v_0 {\bar\times^} \dfrac{ \tilde{\partial} \mathbf m_{ \mathcal{B} 0}}{\partial \mathbf H} $	10
5	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	12
6	$ \quad \dfrac{ \tilde{\partial} \mathbf v_i}{\partial \mathbf H} = \quad ^i \mathbf X_{\lambda(i)} \dfrac{ \tilde{\partial} \mathbf v_{\lambda(i)}}{\partial \mathbf H} $	16
7	$ \quad \dfrac{ \tilde{\partial} \mathbf a^r_i}{\partial \mathbf H} = \quad ^i \mathbf X_{\lambda(i)} \dfrac{ \tilde{\partial} \mathbf a^r_{\lambda(i)}}{\partial \mathbf H} + \dfrac{ \tilde{\partial} \mathbf v_i}{\partial \mathbf H} \times \mathbf v_{ \mathcal{J} i} $	17
8	$ \quad \dfrac{ \tilde{\partial} \mathbf m_{ \mathcal{B} i}}{\partial \mathbf H} = \mathbb{M}_{ \mathcal{B} i} \dfrac{ \tilde{\partial} \mathbf v_i}{\partial \mathbf H} $	20
9	$ \quad \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf H} = \mathbb{M}_{ \mathcal{B} i} \dfrac{ \tilde{\partial} \mathbf a^r_i}{\partial \mathbf H} + \dfrac{ \tilde{\partial} \mathbf v_i}{\partial \mathbf H} {\bar\times^} \mathbf m_{ \mathcal{B} i} + \mathbf v_i {\bar\times^} \dfrac{ \tilde{\partial} \mathbf m_{ \mathcal{B} i}}{\partial \mathbf H} $	21
10	$ \mathbf{end} $	23
11	$ \mathbf{for}\ i=n_B\ \mathbf{to}\ 1\ \mathbf{do} $	25
12	$ \quad \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf H} = \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf H} + \quad _{\lambda(i)} \mathbf X^i \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf H} $	26
13	$ \mathbf{end} $	28
14	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	29
15	$ \quad \dfrac{ \tilde{\partial} {\bf{\tau}}_i}{\partial \mathbf H} = \mathbf \Gamma_{ \mathcal{J} i}^T \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf H} $	31
16	$ \mathbf{end} $	39
17	$ \dfrac{ \tilde{\partial} \bar{ {\bf{\tau}}}_b}{\partial \mathbf H} = \dfrac{ \tilde{\partial} \mathbf b^c_{ \mathcal{B} 0}}{\partial \mathbf H} $	40
Outputs: $ {\rm D}_1 \overline{ID} \circ {\rm D} L_H (I) = \tilde{\partial} \bar{ {\bf{\tau}}} / \partial \mathbf H = [ \tilde{\partial} {\bf{\tau}} / \partial \mathbf H ; \tilde{\partial} \bar{ {\bf{\tau}}}_b / \partial \mathbf H] $

| Show Table

DownLoad: CSV

Table 6. Derivatives of the extended inverse dynamics with respect to the generalized position vector $ \mathbf s $

Inputs:All outputs and intermediate variables of EIDAmb
Line	Algorithm	Line in EIDAmb
1	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	12
2	$ \quad \dfrac{\partial \hspace{0em}^i \mathbf X_{\lambda(i)\|i}}{\partial \mathbf s_i} = $ jcalcderiv(jtype($ i $)$ , \mathbf s_i $)	13
3	$ \quad \dfrac{\partial \hspace{0em}^i \mathbf X_{\lambda(i)}}{\partial \mathbf s_i} = \dfrac{\partial \hspace{0em}^i \mathbf X_{\lambda(i)\|i}}{\partial \mathbf s_i} \hspace{0em}^{\lambda(i)\|i} \mathbf X_{\lambda(i)} $	15
4	$ \quad \dfrac{\partial \mathbf v_i}{\partial \mathbf s} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf v_{\lambda(i)}}{\partial \mathbf s} $	16
5	$ \quad \dfrac{\partial \mathbf v_i}{\partial \mathbf s_i} = \dfrac{\partial \mathbf v_i}{\partial \mathbf s_i} + \dfrac{\partial \hspace{0em}^i \mathbf X_{\lambda(i)}}{\partial \mathbf s_i} \mathbf v_{\lambda(i)} $	16
6	$ \quad \dfrac{\partial \mathbf a^r_i}{\partial \mathbf s} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf a^r_{\lambda(i)}}{\partial \mathbf s} + \dfrac{\partial \mathbf v_i}{\partial \mathbf s} \times \mathbf v_{ \mathcal{J} i} $	17
7	$ \quad \dfrac{\partial \mathbf a^r_i}{\partial \mathbf s_i} = \dfrac{\partial \mathbf a^r_i}{\partial \mathbf s_i} + \dfrac{\partial \hspace{0em}^i \mathbf X_{\lambda(i)}}{\partial \mathbf s_i} \mathbf a^r_{\lambda(i)} $	17
8	$ \quad \dfrac{\partial \mathbf a^{vp}_i}{\partial \mathbf s} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf a^{vp}_{\lambda(i)}}{\partial \mathbf s} + \dfrac{\partial \mathbf v_i}{\partial \mathbf s} \times \mathbf v_{ \mathcal{J} i} $	18
9	$ \quad \dfrac{\partial \mathbf a^{vp}_i}{\partial \mathbf s_i} = \dfrac{\partial \mathbf a^{vp}_i}{\partial \mathbf s_i} + \dfrac{\partial \hspace{0em}^i \mathbf X_{\lambda(i)}}{\partial \mathbf s_i} \mathbf a^{vp}_{\lambda(i)} $	18
10	$ \quad \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf s} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf v_i}{\partial \mathbf s} $	20
11	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf s} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf a^r_i}{\partial \mathbf s} + \dfrac{\partial \mathbf v_i}{\partial \mathbf s} {\bar\times^} \mathbf m_{ \mathcal{B} i} + \mathbf v_i {\bar\times^} \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf s} $	21
12	$ \quad \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} i}}{\partial \mathbf s} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf a^{vp}_i}{\partial \mathbf s} + \dfrac{\partial \mathbf v_i}{\partial \mathbf s} {\bar\times^} \mathbf m_{ \mathcal{B} i} + \mathbf v_i {\bar\times^} \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf s} $	22
13	$ \mathbf{end} $	23
14	$ \mathbf{for}\ i=n_B\ \mathbf{to}\ 1\ \mathbf{do} $	24
15	$ \quad \mathbf{for}\ k=1\ \mathbf{to}\ n_B\ \mathbf{do} $	25
16	$ \qquad \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_k} = \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_k} + \quad _{\lambda(i)} \mathbf X^i \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} i}}{\partial \mathbf s_k} \quad ^i \mathbf X_{\lambda(i)} $	25
17	$ \quad \mathbf{end} $	25
18	$ \quad \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_i} = \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_i} + \dfrac{\partial \quad _{\lambda(i)} \mathbf X^i}{\partial \mathbf s_i} \mathbb{M}^c_{ \mathcal{B} i} \quad ^i \mathbf X_{\lambda(i)} + _{\lambda(i)} \mathbf X^i \: \mathbb{M}^c_{ \mathcal{B} i} \dfrac{\partial \quad ^i \mathbf X_{\lambda(i)}}{\partial \mathbf s_i} $	25
19	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s} = \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s} + \quad _{\lambda(i)} \mathbf X^i \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf s} $	26
20	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_i} = \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_i} + \dfrac{\partial \quad _{\lambda(i)} \mathbf X^i}{\partial \mathbf s_i} \mathbf b^c_{ \mathcal{B} i} $	26
21	$ \quad \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s} = \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s} + \quad _{\lambda(i)} \mathbf X^i \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} i}}{\partial \mathbf s} $	27
22	$ \quad \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_i} = \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} \lambda(i)}}{\partial \mathbf s_i} + \dfrac{\partial \quad _{\lambda(i)} \mathbf X^i}{\partial \mathbf s_i} \mathbf b^{vp}_{ \mathcal{B} i} $	27
23	$ \mathbf{end} $	28
24	$ \mathbf{for}\ k=1\ \mathbf{to}\ n_B\ \mathbf{do} $	-
25	$ \quad \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} 0} \mathbf a_0}{\partial \mathbf s_k} = \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} 0}}{\partial \mathbf s_k} \mathbf a_0 $	-
26	$ \mathbf{end} $	-
27	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	29
28	$ \quad \dfrac{\partial \quad ^i \mathbf a_{A, 0}}{\partial \mathbf s} = \quad ^i \mathbf X_{\lambda(i)} \dfrac{\partial \quad ^{\lambda(i)} \mathbf a_{A, 0}}{\partial \mathbf s} $	30
29	$ \quad \dfrac{\partial \quad ^i \mathbf a_{A, 0}}{\partial \mathbf s_i} = \dfrac{\partial \quad ^i \mathbf a_{A, 0}}{\partial \mathbf s_i} + \dfrac{\partial \quad ^i \mathbf X_{\lambda(i)}}{\partial \mathbf s_i} \quad ^{\lambda(i)} \mathbf a_{A, 0} $	30
30	$ \quad \mathbf{for}\ k=1\ \mathbf{to}\ n_B\ \mathbf{do} $	-
31	$ \qquad \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} i} \quad ^i \mathbf a_{A, 0}}{\partial \mathbf s_k} = \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} i}}{\partial \mathbf s_k} \quad ^i \mathbf a_{A, 0} $	-
32	$ \qquad \dfrac{\partial \quad _i \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_k} = \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} i}}{\partial \mathbf s_k} \mathbf \Gamma_{ \mathcal{J} i} $	32
33	$ \quad \mathbf{end} $	-
34	$ \quad \dfrac{\partial {\bf{\tau}}_i}{\partial \mathbf s} = \mathbf \Gamma_{ \mathcal{J} i}^T \Big( \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} i}}{\partial \mathbf s} \quad ^i \mathbf a_{A, 0} + \mathbb{M}^c_{ \mathcal{B} i} \dfrac{\partial \quad ^i \mathbf a_{A, 0}}{\partial \mathbf s} + \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf s} \Big) $	32
35	$ \quad j = i $	33
36	$ \quad \mathbf{while}\ \lambda(j) > 0 $	34
37	$ \qquad \mathbf{for}\ k=1\ \mathbf{to}\ n_B\ \mathbf{do} $	35
38	$ \quad \qquad \dfrac{\partial \quad _{\lambda(j)} \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_k} = \quad _{\lambda(j)} \mathbf X^j \dfrac{\partial \quad _j \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_k} $	35
39	$ \qquad \mathbf{end} $	35
40	$ \qquad \dfrac{\partial \quad _{\lambda(j)} \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_j} = \dfrac{\partial \quad _{\lambda(j)} \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_j} + \dfrac{\partial \quad _{\lambda(j)} \mathbf X^j}{\partial \mathbf s_j} \quad _j \mathbf F_{ \mathcal{B} i} $	35
41	$ \qquad j = \lambda(j) $	36
42	$ \quad \mathbf{end} $	37
43	$ \quad \mathbf{for}\ k=1\ \mathbf{to}\ n_B\ \mathbf{do} $	38
44	$ \qquad \dfrac{\partial \quad _0 \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_k} = \quad _0 \mathbf X^j \dfrac{\partial \quad _j \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_k} $	38
45	$ \quad \mathbf{end} $	38
46	$ \quad \dfrac{\partial \quad _0 \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_j} = \dfrac{\partial \quad _0 \mathbf F_{ \mathcal{B} i}}{\partial \mathbf s_j} + \dfrac{\partial \quad _0 \mathbf X^j}{\partial \mathbf s_j} \quad _j \mathbf F_{ \mathcal{B} i} $	38
47	$ \quad \dfrac{\partial \quad _0 \mathbf F \dot{ \mathbf r}}{\partial \mathbf s} = \dfrac{\partial \quad _0 \mathbf F \dot{ \mathbf r}}{\partial \mathbf s} + \dfrac{\partial \quad _0 \mathbf F_{ \mathcal{B} i} }{\partial \mathbf s} \dot{ \mathbf r} $	-
48	$ \mathbf{end} $	39
49	$ \dfrac{\partial \bar{ {\bf{\tau}}}_b}{\partial \mathbf s} = \dfrac{\partial \mathbb{M}^c_{ \mathcal{B} 0} \mathbf a_0 }{\partial \mathbf s} + \dfrac{\partial \quad _0 \mathbf F \dot{ \mathbf r}}{\partial \mathbf s} + \dfrac{\partial \mathbf b^{vp}_{ \mathcal{B} 0}}{\partial \mathbf s} $	40
Outputs: $ {\rm D}_2 \overline{ID} = \partial \bar{ {\bf{\tau}}} / \partial \mathbf s = [ \partial \bar{ {\bf{\tau}}}_b / \partial \mathbf s ; \partial {\bf{\tau}} / \partial \mathbf s ] $

| Show Table

DownLoad: CSV

Table 7. Derivatives of the extended inverse dynamics with respect to the moving-base velocity $ \mathbf v $

Inputs: All outputs and intermediate variables of EIDAmb
Line	Algorithm	Line in EIDAmb
1	$ \dfrac{\partial \mathbf v_0}{\partial \mathbf v} = I_6 $	5
2	$ \dfrac{\partial \mathbf m_{ \mathcal{B} 0}}{\partial \mathbf v} = \mathbb{M}_{ \mathcal{B} 0} $	9
3	$ \dfrac{\partial \mathbf b^c_{ \mathcal{B} 0}}{\partial \mathbf v} = \dfrac{\partial \mathbf v_0}{\partial \mathbf v} {\bar\times^} \mathbf m_{ \mathcal{B} 0} + \mathbf v_0 {\bar\times^} \dfrac{\partial \mathbf m_{ \mathcal{B} 0}}{\partial \mathbf v} $	10
4	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	12
5	$ \quad \dfrac{\partial \mathbf v_i}{\partial \mathbf v} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf v_{\lambda(i)}}{\partial \mathbf v} $	16
6	$ \quad \dfrac{\partial \mathbf a^r_i}{\partial \mathbf v} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf a^r_{\lambda(i)}}{\partial \mathbf v} + \dfrac{\partial \mathbf v_i}{\partial \mathbf v} \times \mathbf v_{ \mathcal{J} i} $	17
7	$ \quad \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf v} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf v_i}{\partial \mathbf v} $	20
8	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf v} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf a^r_i}{\partial \mathbf v} + \dfrac{\partial \mathbf v_i}{\partial \mathbf v} {\bar\times^} \mathbf m_{ \mathcal{B} i} + \mathbf v_i {\bar\times^} \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf v} $	21
9	$ \mathbf{end} $	23
10	$ \mathbf{for}\ i=n_B\ \mathbf{to}\ 1\ \mathbf{do} $	24
11	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf v} = \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf v} + \quad _{\lambda(i)} \mathbf X^i \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf v} $	26
12	$ \mathbf{end} $	28
13	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	29
14	$ \quad \dfrac{\partial {\bf{\tau}}_i}{\partial \mathbf v} = \mathbf \Gamma_{ \mathcal{J} i}^T \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf v} $	31
15	$ \mathbf{end} $	39
16	$ \dfrac{\partial \bar{ {\bf{\tau}}}_b}{\partial \mathbf v} = \dfrac{\partial \mathbf b^c_{ \mathcal{B} 0}}{\partial \mathbf v} $	40
Outputs: $ {\rm D}_3 \overline{ID} = \partial \bar{ {\bf{\tau}}} / \partial \mathbf v = [ \partial \bar{ {\bf{\tau}}}_b / \partial \mathbf v ; \partial {\bf{\tau}} / \partial \mathbf v ] $

| Show Table

DownLoad: CSV

Table 8. Derivatives of the extended inverse dynamics w.r.t. the generalized velocity vector $ \mathbf r $

Inputs: All outputs and intermediate variables of EIDAmb
Line	Algorithm	Line in EIDAmb
1	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	12
2	$ \quad \dfrac{\partial \mathbf v_{ \mathcal{J} i}}{\partial \mathbf r_i} = \mathbf \Gamma_{ \mathcal{J} i} $	14
3	$ \quad \dfrac{\partial \mathbf v_i}{\partial \mathbf r} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf v_{\lambda(i)}}{\partial \mathbf r} $	16
4	$ \quad \dfrac{\partial \mathbf v_i}{\partial \mathbf r_i} = \dfrac{\partial \mathbf v_i}{\partial \mathbf r_i} + \dfrac{\partial \mathbf v_{ \mathcal{J} i}}{\partial \mathbf r_i} $	16
5	$ \quad \dfrac{\partial \mathbf a^r_i}{\partial \mathbf r} = \hspace{0em}^i \mathbf X_{\lambda(i)} \dfrac{\partial \mathbf a^r_{\lambda(i)}}{\partial \mathbf r} + \dfrac{\partial \mathbf v_i}{\partial \mathbf r} \times \mathbf v_{ \mathcal{J} i} $	17
6	$ \quad \dfrac{\partial \mathbf a^r_i}{\partial \mathbf r_i} = \dfrac{\partial \mathbf a^r_i}{\partial \mathbf r_i} + \mathbf v_i \times \dfrac{\partial \mathbf v_{ \mathcal{J} i}}{\partial \mathbf r_i} $	17
7	$ \quad \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf r} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf v_i}{\partial \mathbf r} $	20
8	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf r} = \mathbb{M}_{ \mathcal{B} i} \dfrac{\partial \mathbf a^r_i}{\partial \mathbf r} + \dfrac{\partial \mathbf v_i}{\partial \mathbf r} {\bar\times^} \mathbf m_{ \mathcal{B} i} + \mathbf v_i {\bar\times^} \dfrac{\partial \mathbf m_{ \mathcal{B} i}}{\partial \mathbf r} $	21
9	$ \mathbf{end} $	23
10	$ \mathbf{for}\ i=n_B\ \mathbf{to}\ 1\ \mathbf{do} $	24
11	$ \quad \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf r} = \dfrac{\partial \mathbf b^c_{ \mathcal{B} \lambda(i)}}{\partial \mathbf r} + \quad _{\lambda(i)} \mathbf X^i \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf r} $	26
12	$ \mathbf{end} $	28
13	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $	29
14	$ \quad \dfrac{\partial {\bf{\tau}}_i}{\partial \mathbf r} = \mathbf \Gamma_{ \mathcal{J} i}^T \dfrac{\partial \mathbf b^c_{ \mathcal{B} i}}{\partial \mathbf r} $	31
15	$ \mathbf{end} $	39
16	$ \dfrac{\partial \bar{ {\bf{\tau}}}_b}{\partial \mathbf r} = \dfrac{\partial \mathbf b^c_{ \mathcal{B} 0}}{\partial \mathbf r} $	40
Outputs: $ {\rm D}_4 \overline{ID} = \partial \bar{ {\bf{\tau}}} / \partial \mathbf r = [ \partial \bar{ {\bf{\tau}}}_b / \partial \mathbf r ; \partial {\bf{\tau}} / \partial \mathbf r ] $

| Show Table

DownLoad: CSV

Table 9. Inverse Mass Matrix Algorithm for moving-base systems

Inputs	model, $ \mathbf s $
Line	IMMAmb
1	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $
2	$ \quad [^i \mathbf X_{\lambda(i)\|i}, \mathbf \Gamma_{ \mathcal{J} i} ] = $ jcalc(jtype($ i $)$ , \mathbf s_i $)
3	$ \quad \quad ^i \mathbf X_{\lambda(i)} = ^i \mathbf X_{\lambda(i)\|i} \quad ^{\lambda(i)\|i} \mathbf X_{\lambda(i)} $
4	$ \quad \mathbb{M}_{ \mathcal{B} i}^A = \mathbb{M}_{ \mathcal{B} i} $
5	$ \mathbf{end} $
6	$ \mathbf{for}\ i=n_B\ \mathbf{to}\ 1\ \mathbf{do} $
7	$ \quad \mathbf U_{ \mathcal{B} i} = \mathbb{M}_{ \mathcal{B} i}^A \mathbf \Gamma_{ \mathcal{J} i} $
8	$ \quad \mathbf D_{ \mathcal{B} i} = \mathbf \Gamma_{ \mathcal{J} i}^T \mathbf U_{ \mathcal{B} i} $
9	$ \quad \mathbf M^{inv}[i\!+\!6, i\!+\!6] = \mathbf D_{ \mathcal{B} i}^{-1} $
10	$ \quad \mathbf M^{inv}[i\!+\!6, subtree(i)\!+\!6] = \mathbf M^{inv}[i\!+\!6, subtree(i)\!+\!6] $
	$ \phantom{\quad \mathbf M^{inv}[i\!+\!6, subtree(i)\!+\!6] =} - \mathbf D_{ \mathcal{B} i}^{-1} \mathbf \Gamma_{ \mathcal{J} i}^T \mathcal{F}_i[:, subtree(i)\!+\!6] $
11	$ \quad \mathcal{F}_{\lambda(i)}[:, subtree(i)\!+\!6] = \mathcal{F}_{\lambda(i)}[:, subtree(i)\!+\!6] $
	$ \phantom{\quad \mathcal{F}_{\lambda(i)}[:, subtree(i)\!+\!6] =} + _{\lambda(i)} \mathbf X^i \big( \mathcal{F}_i[:, subtree(i)\!+\!6] $
	$ \phantom{\quad \mathcal{F}_{\lambda(i)}[:, subtree(i)\!+\!6] =} + \mathbf U_{ \mathcal{B} i} \mathbf M^{inv}[i\!+\!6, subtree(i)\!+\!6] \big) $
12	$ \quad \mathbb{M}^a_{ \mathcal{B} i} = \mathbb{M}^A_{ \mathcal{B} i} - \mathbf U_{ \mathcal{B} i} \mathbf D_{ \mathcal{B} i}^{-1} \mathbf U_{ \mathcal{B} i}^T $
13	$ \quad \mathbb{M}^A_{ \mathcal{B} \lambda(i)} = \mathbb{M}^A_{ \mathcal{B} \lambda(i)} + \quad _{\lambda(i)} \mathbf X^i \mathbb{M}^a_{ \mathcal{B} i} \quad ^i \mathbf X_{\lambda(i)} $
14	$ \mathbf{end} $
15	$ \mathcal{P}_0[:, 7\!:] = - ( \mathbb{M}^A_{ \mathcal{B} 0})^{-1} \mathcal{F}_0[:, 7\!:] $
16	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $
17	$ \quad \mathbf M^{inv}[i\!+\!6, i\!+\!6\!:] = \mathbf M^{inv}[i\!+\!6, i\!+\!6\!:] - \mathbf D_{ \mathcal{B} i}^{-1} \mathbf U_{ \mathcal{B} i}^T \quad ^i \mathbf X_{\lambda(i)} \mathcal{P}_{\lambda(i)}[:, i\!+\!6\!:] $
18	$ \quad \mathcal{P}_i[:, i\!+\!6\!:] = \mathbf \Gamma_{ \mathcal{J} i} \mathbf M^{inv}[i\!+\!6, i\!+\!6\!:] + ^i \mathbf X_{\lambda(i)} \mathcal{P}_{\lambda(i)}[i:i\!+\!6\!:] $
19	$ \mathbf{end} $
20	$ \mathbf M^{inv}[1\!:\!6, 7\!:] = \mathcal{P}_0[:, 7\!:] $
21	$ \mathbf{for}\ i=1\ \mathbf{to}\ n_B\ \mathbf{do} $
22	$ \quad \mathbf{for}\ j=i\ \mathbf{to}\ n_B\ \mathbf{do} $
23	$ \qquad \mathbf M^{inv}[j\!+\!6, i\!+\!6] = \mathbf M^{inv}[i\!+\!6, j\!+\!6] $
24	$ \quad \mathbf{end} $
25	$ \mathbf{end} $
26	$ \mathbf M^{inv}[1\!:\!6, 1\!:\!6] = ( \mathbb{M}^A_{ \mathcal{B} 0})^{-1} $
Outputs	$ \mathbf M^{inv} $

| Show Table

DownLoad: CSV

Related Papers

Cited by

References

[1]	K. S. Anderson and Y. Hsu, Analytical fully-recursive sensitivity analysis for multibody dynamic chain systems, Multibody Syst. Dyn., 8 (2002), 1-27. doi: 10.1023/A:1015867515213.
[2]	M. H. Ang and V. D. Tourassis, Singularities of Euler and roll-pitch-yaw representations, IEEE Transactions on Aerospace and Electronic Systems, 23 (1987), 317-324. doi: 10.1109/TAES.1987.310828.
[3]	K. Ayusawa, G. Venture and Y. Nakamura, Identification of humanoid robots dynamics using floating-base motion dynamics, IEEE/RSJ International Conference on Intelligent Robots and Systems, (2008), 2854–2859. doi: 10.1109/IROS.2008.4650614.
[4]	A. Barrau and S. Bonnabel, Intrinsic filtering on lie groups with applications to attitude estimation, IEEE Trans. Automat. Control, 60 (2015), 436-449. doi: 10.1109/TAC.2014.2342911.
[5]	M. P. Bos, Efficient geometric sensitivity analysis of moving-base multibody dynamics, Master's Thesis, Eindhoven University of Technology, 2019.
[6]	O. Brüls and P. Eberhard, Sensitivity analysis for dynamic mechanical systems with finite rotations, Int. J. Numer. Methods Eng., 74 (2008), 1897-1927. doi: 10.1002/nme.2232.
[7]	J. Carpentier, Analytical inverse of the joint space inertia matrix, Available Online at Hal. Laas. Fr, 2018.
[8]	J. Carpentier and N. Mansard, Analytical derivatives of rigid body dynamics algorithms, Robotics: Science and Systems (RSS), 2018. doi: 10.15607/RSS.2018.XIV.038.
[9]	J. Deray and J. Solà, Manif: A micro lie theory library for state estimation in robotics applications, J. Open Source Software, 5 (2020), 1371. doi: 10.21105/joss.01371.
[10]	J. Diebel, Representing attitude: Euler angles, unit quaternions, and rotation vectors, Matrix, 58 (2006), 1-35.
[11]	Q. Docquier, O. Brüls and P. Fisette, Comparison and analysis of multibody dynamics formalisms for solving optimal control problem, IUTAM Symposium on Intelligent Multibody Systems - Dynamics, Control, Simulation, 33 (2019), 55-77. doi: 10.1007/978-3-030-00527-6_3.
[12]	T. Fan and T. Murphey, Structured linearization of discrete mechanical systems on lie groups: A synthesis of analysis and control, In 2015 54th IEEE Conference on Decision and Control (CDC), (2015), 1092–1099. doi: 10.1109/CDC.2015.7402357.
[13]	F. Farshidian, E. Jelavic, A. Satapathy, M. Giftthaler and J. Buchli, Real-time motion planning of legged robots: A model predictive control approach, IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids), (2017), 577–584. doi: 10.1109/HUMANOIDS.2017.8246930.
[14]	R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008.
[15]	P. J. From, V. Duindam, K. Y. Pettersen, J. T. Gravdahl and S. Sastry, Singularity-free dynamic equations of vehicle–manipulator systems, Simulation Modelling Practice and Theory, 18 (2010), 712-731.
[16]	G. Garofalo, C. Ott and A. Albu-Schäffer, On the closed form computation of the dynamic matrices and their differentiations, IEEE/RSJ International Conference on Intelligent Robots and Systems, (2013), 2364–2359. doi: 10.1109/IROS.2013.6696688.
[17]	P. Geoffroy, N. Mansard, M. Raison, S. Achiche and E. Todorov, From inverse kinematics to optimal control, In Advances in Robot Kinematics, Springer, Cham, (2014), 409–418.
[18]	M. Giftthaler, M. Neunert, M. Stäuble, M. Frigerio, C. Semini and J. Buchli, Automatic differentiation of rigid body dynamics for optimal control and estimation, Advanced Robotics, 31 (2017), 1225-1237. doi: 10.1080/01691864.2017.1395361.
[19]	H. K. Khalil, Nonlinear Systems, Prentice hall, 2002.
[20]	A. Kheddar, S. Caron, P. Gergondet, A. Comport, A. Tanguy, C. Ott, B. Henze, G. Mesesan, J. Englsberger, M. A. Roa, P. Wieber, F. Chaumette, F. Spindler, G. Oriolo, L. Lanari, A. Escande, K. Chappellet, F. Kanehiro and P. Rabaté, Humanoid robots in aircraft manufacturing: The airbus use cases., IEEE Robotics Automation Magazine, 26 (2019), 30-45.
[21]	J. Koenemann, A. Del Prete, Y. Tassa, E. Todorov, O. Stasse, M. Bennewitz and N. Mansard, Whole-body model-predictive control applied to the HRP-2 humanoid, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), (2015), 3346–3351. doi: 10.1109/IROS.2015.7353843.
[22]	J. M. Lee, Smooth manifolds, Introduction to Smooth Manifolds, 218 (2013), 1-31. doi: 10.1007/978-1-4419-9982-5_1.
[23]	T. Lee, M. Leok and N. H. McClamroch, Computational geometric optimal control of rigid bodies, Commun. Inf. Syst., 8 (2008), 445-472. doi: 10.4310/CIS.2008.v8.n4.a5.
[24]	T. Lee, M. Leok and N. H. McClamroch, Global Formulations of Lagrangian and Hamiltonian Dynamics on Embedded Manifolds, Springer, 2017. doi: 10.1007/978-3-319-56953-6.
[25]	M. Leok, An overview of lie group variational integrators and their applications to optimal control, In International Conference on Scientific Computation and Differential Equations, The French National Institute for Research in Computer Science and Control, (2007), 1 pp.
[26]	J. Y. S. Luh, M. W. Walker and R. P. C. Paul, On-line computational scheme for mechanical manipulators, J. Dynam. Systems Measurement Control, 102 (1980), 69-76. doi: 10.1115/1.3149599.
[27]	A. Marco, P. Hennig, J. Bohg, S. Schaal and S. Trimpe, Automatic LQR tuning based on Gaussian process global optimization, IEEE International Conference on Robotics and Automation (ICRA), 2016. doi: 10.1109/ICRA.2016.7487144.
[28]	S. Mason, L. Righetti and S. Schaal, Full dynamics LQR control of a humanoid robot: An experimental study on balancing and squatting, IEEE-RAS International Conference on Humanoid Robots, (2014), 374–379. doi: 10.1109/HUMANOIDS.2014.7041387.
[29]	R. M. Murray, A Mathematical Introduction to Robotic Manipulation, CRC press, 2017.
[30]	D. Negrut and J. Ortiz, A practical approach for the linearization of the constrained multibody dynamics equations, J. Comput. Nonlinear Dynam., 1 (2006), 230-239. doi: 10.1115/1.2198876.
[31]	M. Neunert, M. Giftthaler, M. Frigerio, C. Semini and J. Buchli, Fast derivatives of rigid body dynamics for control, optimization and estimation, IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR), (2016), 91–97. doi: 10.1109/SIMPAR.2016.7862380.
[32]	F. C. Park, B. Kim, C. Jang and J. Hong, Geometric algorithms for robot dynamics: A tutorial review, Appl. Mech. Rev., 70 (2018), 010803. doi: 10.1115/1.4039078.
[33]	M. Posa, C. Cantu and R. Tedrake, A direct method for trajectory optimization of rigid bodies through contact, The International Journal of Robotics Research, 33 (2014), 69-81. doi: 10.1177/0278364913506757.
[34]	D. Pucci, G. Nava and F. Nori, Automatic gain tuning of a momentum based balancing controller for humanoid robots, In 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids), IEEE, (2016), 158–164. doi: 10.1109/HUMANOIDS.2016.7803272.
[35]	D. Pucci, S. Traversaro and F. Nori, Momentum control of an underactuated flying humanoid robot, IEEE Robotics and Automation Letters, 3 (2018), 195-202. doi: 10.1109/LRA.2017.2734245.
[36]	W. Rossmann, Lie Groups: An Introduction Through Linear Groups, volume 5., Oxford University Press, 2002.
[37]	A. Saccon, A. P. Aguiar and J. Hauser, Lie group projection operator approach: Optimal control on T SO (3), IEEE Decision and Control and European Control Conference (CDC-ECC), (2011), 6973–6978.
[38]	A. Saccon, J. Hauser and A. P. Aguiar, Optimal control on non-compact lie groups: A projection operator approach, In 49th IEEE Conference on Decision and Control (CDC), (2010), 7111–7116
[39]	A. Saccon, J. Hauser and A. P. Aguiar, Optimal control on Lie groups: The projection operator approach, IEEE Trans. Automat. Control, 58 (2013), 2230-2245. doi: 10.1109/TAC.2013.2258817.
[40]	A. Saccon, J. Hauser and A. Beghi, Trajectory exploration of a rigid motorcycle model, IEEE Transactions on Control Systems Technology, 20 (2012), 424-437. doi: 10.1109/TCST.2011.2116788.
[41]	A. Saccon, S. Traversaro, F. Nori and H. Nijmeijer, On centroidal dynamics and integrability of average angular velocity, IEEE Robotics and Automation Letter, 2 (2017), 943-950. doi: 10.1109/LRA.2017.2655560.
[42]	A. K. Sanyal, T. Lee, M. Leok and N. H. McClamroch, Global optimal attitude estimation using uncertainty ellipsoids, Systems Control Lett., 57 (2008), 236-245. doi: 10.1016/j.sysconle.2007.08.014.
[43]	G. A. Sohl and J. E. Bobrow, A recursive multibody dynamics and sensitivity algorithm for branched kinematic chains, J. Dynamic Systems, Measurement, and Control, 123 (2001), 391-399.
[44]	J. Solà, J. Deray and D. Atchuthan, A micro Lie theory for state estimation in robotics, arXiv preprint, arXiv: 1812.01537, 2018.
[45]	V. Sonneville and O. Brüls, Sensitivity analysis for multibody systems formulated on a Lie group, Multibody Syst. Dyn., 31 (2014), 47-67. doi: 10.1007/s11044-013-9345-z.
[46]	Y. Tassa, T. Erez and E. Todorov, Synthesis and stabilization of complex behaviors through online trajectory optimization, IEEE/RSJ International Conference on Intelligent Robots and Systems, (2012), 4906–4913. doi: 10.1109/IROS.2012.6386025.
[47]	S. Traversaro and A. Saccon, Multibody dynamics notation, revision 2, Available Online at Tue. Research. Nl, 2019.
[48]	V. S. Varadarajan, Lie Groups, Lie Algebras, and Their Representations, Reprint of the 1974 edition. Graduate Texts in Mathematics, 102. Springer-Verlag, New York, 1984. doi: 10.1007/978-1-4612-1126-6.
[49]	M. W. Walker and D. E. Orin, Efficient dynamic computer simulation of robotic mechanisms, J. Dyn. Sys., Meas., Control., 104 (1982), 205-211. doi: 10.1115/1.3139699.