• Previous Article
    Atmospheric Ekman flows with uniform density in ellipsoidal coordinates: Explicit solution and dynamical properties
  • JGM Home
  • This Issue
  • Next Article
    Infinite lifting of an action of symplectomorphism group on the set of bi-Lagrangian structures
doi: 10.3934/jgm.2022009
Online First

Online First articles are published articles within a journal that have not yet been assigned to a formal issue. This means they do not yet have a volume number, issue number, or page numbers assigned to them, however, they can still be found and cited using their DOI (Digital Object Identifier). Online First publication benefits the research community by making new scientific discoveries known as quickly as possible.

Readers can access Online First articles via the “Online First” tab for the selected journal.

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

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

Received  February 2021 Revised  March 2022 Early access May 2022

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.

Citation: Martijn Bos, Silvio Traversaro, Daniele Pucci, Alessandro Saccon. Efficient geometric linearization of moving-base rigid robot dynamics. Journal of Geometric Mechanics, doi: 10.3934/jgm.2022009
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. DocquierO. 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. FromV. DuindamK. Y. PettersenJ. 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. GiftthalerM. NeunertM. StäubleM. FrigerioC. 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. KheddarS. CaronP. GergondetA. ComportA. TanguyC. OttB. HenzeG. MesesanJ. EnglsbergerM. A. RoaP. WieberF. ChaumetteF. SpindlerG. OrioloL. LanariA. EscandeK. ChappelletF. 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. LeeM. 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. LuhM. 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. ParkB. KimC. 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. PosaC. 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. PucciS. 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. SacconJ. 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. SacconJ. 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. SacconS. TraversaroF. 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. SanyalT. LeeM. 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.

show all references

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. DocquierO. 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. FromV. DuindamK. Y. PettersenJ. 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. GiftthalerM. NeunertM. StäubleM. FrigerioC. 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. KheddarS. CaronP. GergondetA. ComportA. TanguyC. OttB. HenzeG. MesesanJ. EnglsbergerM. A. RoaP. WieberF. ChaumetteF. SpindlerG. OrioloL. LanariA. EscandeK. ChappelletF. 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. LeeM. 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. LuhM. 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. ParkB. KimC. 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. PosaC. 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. PucciS. 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. SacconJ. 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. SacconJ. 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. SacconS. TraversaroF. 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. SanyalT. LeeM. 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.

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) $
Figure 2.  Joint numbering convention. Joints are numbered according to the successor body
Figure 3.  A general example of body and joint frames
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)
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
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
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
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 $
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 $
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 $
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 $
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] $
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] $
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] $
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] $
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 ] $
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 ] $
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 ] $
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 ] $
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 ] $
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 ] $
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} $
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} $
[1]

Răzvan M. Tudoran, Anania Gîrban. On the Hamiltonian dynamics and geometry of the Rabinovich system. Discrete and Continuous Dynamical Systems - B, 2011, 15 (3) : 789-823. doi: 10.3934/dcdsb.2011.15.789

[2]

Janina Kotus, Mariusz Urbański. The dynamics and geometry of the Fatou functions. Discrete and Continuous Dynamical Systems, 2005, 13 (2) : 291-338. doi: 10.3934/dcds.2005.13.291

[3]

Steven T. Piantadosi. Symbolic dynamics on free groups. Discrete and Continuous Dynamical Systems, 2008, 20 (3) : 725-738. doi: 10.3934/dcds.2008.20.725

[4]

Hua Shi, Xiang Zhang, Yuyan Zhang. Complex planar Hamiltonian systems: Linearization and dynamics. Discrete and Continuous Dynamical Systems, 2021, 41 (7) : 3295-3317. doi: 10.3934/dcds.2020406

[5]

Jan Prüss, Laurent Pujo-Menjouet, G.F. Webb, Rico Zacher. Analysis of a model for the dynamics of prions. Discrete and Continuous Dynamical Systems - B, 2006, 6 (1) : 225-235. doi: 10.3934/dcdsb.2006.6.225

[6]

Laetitia Paoli. A velocity-based time-stepping scheme for multibody dynamics with unilateral constraints. Discrete and Continuous Dynamical Systems - S, 2013, 6 (6) : 1609-1619. doi: 10.3934/dcdss.2013.6.1609

[7]

Qi Wang, Huilian Peng, Fangfang Zhuang. A constraint-stabilized method for multibody dynamics with friction-affected translational joints based on HLCP. Discrete and Continuous Dynamical Systems - B, 2011, 16 (2) : 589-605. doi: 10.3934/dcdsb.2011.16.589

[8]

Michael Hochman. Lectures on dynamics, fractal geometry, and metric number theory. Journal of Modern Dynamics, 2014, 8 (3&4) : 437-497. doi: 10.3934/jmd.2014.8.437

[9]

Janusz Grabowski, Katarzyna Grabowska, Paweł Urbański. Geometry of Lagrangian and Hamiltonian formalisms in the dynamics of strings. Journal of Geometric Mechanics, 2014, 6 (4) : 503-526. doi: 10.3934/jgm.2014.6.503

[10]

Joris Vankerschaver, Eva Kanso, Jerrold E. Marsden. The geometry and dynamics of interacting rigid bodies and point vortices. Journal of Geometric Mechanics, 2009, 1 (2) : 223-266. doi: 10.3934/jgm.2009.1.223

[11]

Xin Zhao, Tao Feng, Liang Wang, Zhipeng Qiu. Threshold dynamics and sensitivity analysis of a stochastic semi-Markov switched SIRS epidemic model with nonlinear incidence and vaccination. Discrete and Continuous Dynamical Systems - B, 2021, 26 (12) : 6131-6154. doi: 10.3934/dcdsb.2021010

[12]

Eugenii Shustin. Dynamics of oscillations in a multi-dimensional delay differential system. Discrete and Continuous Dynamical Systems, 2004, 11 (2&3) : 557-576. doi: 10.3934/dcds.2004.11.557

[13]

Jia-Feng Cao, Wan-Tong Li, Fei-Ying Yang. Dynamics of a nonlocal SIS epidemic model with free boundary. Discrete and Continuous Dynamical Systems - B, 2017, 22 (2) : 247-266. doi: 10.3934/dcdsb.2017013

[14]

Lei Li, Jianping Wang, Mingxin Wang. The dynamics of nonlocal diffusion systems with different free boundaries. Communications on Pure and Applied Analysis, 2020, 19 (7) : 3651-3672. doi: 10.3934/cpaa.2020161

[15]

Lianzhang Bao, Zhengfang Zhou. Traveling wave in backward and forward parabolic equations from population dynamics. Discrete and Continuous Dynamical Systems - B, 2014, 19 (6) : 1507-1522. doi: 10.3934/dcdsb.2014.19.1507

[16]

Xiaoying Han, Peter E. Kloeden. Pullback and forward dynamics of nonautonomous Laplacian lattice systems on weighted spaces. Discrete and Continuous Dynamical Systems - S, 2021  doi: 10.3934/dcdss.2021143

[17]

Claudia Valls. The Boussinesq system:dynamics on the center manifold. Communications on Pure and Applied Analysis, 2005, 4 (4) : 839-860. doi: 10.3934/cpaa.2005.4.839

[18]

Shikun Wang. Dynamics of a chemostat system with two patches. Discrete and Continuous Dynamical Systems - B, 2019, 24 (11) : 6261-6278. doi: 10.3934/dcdsb.2019138

[19]

Jaroslaw Smieja, Marzena Dolbniak. Sensitivity of signaling pathway dynamics to plasmid transfection and its consequences. Mathematical Biosciences & Engineering, 2016, 13 (6) : 1207-1222. doi: 10.3934/mbe.2016039

[20]

Youshan Tao. Global dynamics in a higher-dimensional repulsion chemotaxis model with nonlinear sensitivity. Discrete and Continuous Dynamical Systems - B, 2013, 18 (10) : 2705-2722. doi: 10.3934/dcdsb.2013.18.2705

2021 Impact Factor: 0.737

Metrics

  • PDF downloads (140)
  • HTML views (117)
  • Cited by (0)

[Back to Top]