Pith. sign in

REVIEW 3 major objections 5 minor 32 references

An $O(n$)-Algorithm for the Higher-Order Kinematics and Inverse Dynamics of Serial Manipulators using Spatial Representation of Twists

T0 review · 3 major / 5 minor · reviewed 2026-08-07 · deepseek-v4-flash

Pith's one-line read A spatial-twist recursion computes a serial arm's joint torques and their first two time derivatives in O(n) time.

desk verdict A clean spatial-representation variant of the author's own body-fixed higher-order inverse dynamics algorithm; the O(n) claim holds for the recursions but not for the Jacobian-based inverse kinematics step. read the letter →

arxiv 2506.10686 v1 pith:RZFWFKAN submitted 2025-06-12 cs.RO cs.SCmath.GRmath.OCphysics.class-ph

classification cs.ROcs.SCmath.GRmath.OCphysics.class-ph MSC 70B1570E60
keywords second-orderinversedynamicshigher-orderforwardkinematicsspatialrepresentationoftwistsrecursiveO(n)algorithmscrewtheoryLiegroupformulationserialmanipulatorflatness-basedcontrol
verification ladder T0 review T1 audit T2 compute T3 formal

The pith

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

The reading

Optimal control and flatness-based control of robotic arms require not just the joint torques that produce a desired motion, but their first and second time derivatives. This paper claims a recursive O(n) algorithm, built on the spatial representation of twists, that computes a serial manipulator's joint torques/forces $Q$ together with $\dot Q$ and $\ddot Q$, given the desired joint motion and its derivatives up to fourth order. The claim matters because these are exactly the quantities that flatness-based controllers for robots with elastic joints must evaluate repeatedly, and the cost grows only linearly with the number of joints. The algorithm is worked out in full (fourth-order forward kinematics, combined inverse kinematics, and the dynamics recursion) and demonstrated on the 7-DOF Franka Emika Panda robot, where the outputs are validated against analytic derivatives of the closed-form equations of motion.

What carries the argument

The central object is the spatial twist $V_i^s = (\omega_i^s, v_i^s)$ of each link together with the instantaneous joint screw $S_i = \mathrm{Ad}_{f_i(q)} Y_i$, which propagates by the purely additive recurrence $V_i^s = V_{i-1}^s + S_i \dot q_i$ with no frame transformation of twists. The load-bearing identity is Eq. (14), $\dot M^s = -M^s \mathrm{ad}_{V^s} - \mathrm{ad}_{V^s}^T M^s$, the time derivative of the configuration-dependent spatial mass matrix $M^s = \mathrm{Ad}_C^{-T} M^b \mathrm{Ad}_C^{-1}$; it is what converts the second and third derivatives of the momentum screw, Eqs. (19) and (22), into closed expressions reusable in the backward recursion. Jourdain's principle of virtual power then expresses the generalized forces as the reciprocal product $Q_i = S_i^T \bar W_i$ of the accumulated inter-body wrenches, and the product rule with the precomputed $\dot S_i$ and $\ddot S_i$ yields $\dot Q_i$ and $\ddot Q_i$ without further kinematics.

What would settle it

Take a 2- or 3-DOF serial arm with known link parameters, run Algorithms 1 and 3 along a smooth trajectory, and compare $Q$, $\dot Q$, $\ddot Q$ against symbolic time derivatives of the closed-form equations of motion: they should agree to machine precision, since both are exact algebraic expansions. A more local check is to finite-difference the momentum $\Pi_i^s = M_i^s V_i^s$ at each body and compare with $\dot\Pi_i^s$ produced by Algorithm 3, or to test Eq. (14) directly by finite-differencing $M_i^s$ from Eq. (13); any mismatch isolates the failing recursion step.

Watch

Extended reading notes

Core claim

The central claim is that second-order inverse dynamics — the joint torques/forces $Q(t)$ together with their first and second time derivatives $\dot Q(t)$ and $\ddot Q(t)$ — can be computed for an $n$-joint serial manipulator in $O(n)$ time using the spatial representation of twists. The algorithm is a forward kinematics pass (Algorithm 1) that distributes the fourth-order motion state $(q, \dot q, \ddot q, \dddot q, \ddddot q)$ among the links and computes the instantaneous joint screws $S_i$ and their first three derivatives, followed by a backward dynamics pass (Algorithm 3) that propagates each body's spatial momentum screw $\Pi_i^s = M_i^s V_i^s$ and its derivatives into inter-body wrenches, then projects them onto the joint screws to obtain $Q_i$, $\dot Q_i$, and $\ddot Q_i$. The algebraic hinge that makes the momentum derivatives tractable is the identity $\dot M^s = -M^s \mathrm{ad}_{V^s} - \mathrm{ad}_{V^s}^T M^s$ for the time derivative of the spatial mass matrix. A combined recursion (Algorithm 2) handles the case where only the end-effector motion is prescribed, solving the inverse kinematics through fourth order.

Load-bearing premise

The load-bearing premise is the derivative identity for the spatial mass matrix, $\dot M^s = -M^s \mathrm{ad}_{V^s} - \mathrm{ad}_{V^s}^T M^s$, through which the second and third derivatives of the momentum screw, and hence $\dot Q$ and $\ddot Q$, are obtained; if that identity is wrong or is used outside its domain of validity, every higher-order output of the recursion fails.

Editorial extensions

If this is right

  • Flatness-based controllers for robots with serial-elastic or variable-stiffness actuators can evaluate the required torque rates $\dot Q$ and $\ddot Q$ on line, because each evaluation costs $O(n)$ in the number of joints.
  • The spatial algorithm completes the set of Lie-group formulations: recursive second-order inverse dynamics now exists in spatial, body-fixed, and hybrid representations, so the representations can be compared directly.
  • The fourth-order forward kinematics pass distributes any prescribed motion state, including jerk and jounce, to every link, which is what time-optimal trajectory planning with torque-rate limits demands.
  • Because the formulation is parameterized by vectorial data (joint axes, position vectors, inertias), the algorithm can be assembled directly from standard robot-description data without a separate parameterization step.

Reading between the lines

Editorial extensions of the paper, not claims the author makes directly.

  • The same ad-matrix pattern that produces Eqs. (19) and (22) accumulates by a regular rule, so in principle a fifth-order forward kinematics pass would yield $\dddot Q$ at the same $O(n)$ cost, which would matter if inverter current-rate limits required even smoother command profiles.
  • The paper's preliminary timing, in which the spatial recursion ran about 15% faster than the body-fixed version in Matlab, hints that the classical first-order conclusion that body-fixed is slightly cheaper may not carry over to higher orders, because the reused joint-screw and mass-matrix derivatives avoid frame transformations; only a compiled stand-alone benchmark could settle this.
  • Eq. (14) doubles as a cheap unit test for any implementation: finite-differencing $M^s$ computed from Eq. (13) and comparing with the identity would isolate algebraic slip-ups in the momentum-derivative chain before a full trajectory validation.
Share X Bluesky LinkedIn Reddit HN

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, and a circularity audit.

Referee Report

3 major / 5 minor

Summary. The manuscript develops a spatial-representation Lie-group formulation of recursive higher-order kinematics and inverse dynamics for serial manipulators. Algorithm 1 computes the twist and its first three time derivatives for every link, together with the joint-screw vectors and their derivatives, in O(n) time; Algorithm 3 computes the generalized forces Q and their first two time derivatives through a backward momentum recursion, also in O(n) time. Algorithm 2 combines the forward kinematics with a velocity-level inverse kinematics step. The derivation is based on the spatial mass matrix (11), its time derivative (14), and the Newton-Euler equations (15)-(22). A Matlab demonstration on the 7-DOF Franka Emika Panda is reported, and the code is provided as supplementary material.

Significance. If the two main recursions are correct, the paper is a useful complement to the existing body-fixed and hybrid Lie-group algorithms [21], since it supplies the spatially represented analogues and reuses the joint-screw and mass-matrix derivatives. The derivation is self-contained and rests on the standard identity (14); no ad-hoc parameters or circular assumptions enter. The release of the Matlab implementation is a concrete strength. However, the O(n) claim is currently overstated because it is attached to Algorithm 2, which contains a dense Jacobian inversion, and that algorithm has an indexing gap in the terminal-link screw derivatives. With those points repaired, the contribution would be a solid O(n) inverse-dynamics recursion and O(n) forward-kinematics recursion, plus a higher-order inverse-kinematics scheme whose complexity must be stated honestly.

major comments (3)
  1. [Section IV, Algorithm 2, step 2.a] The headline O(n) claim does not cover Algorithm 2 as written. In step 2.a, qdot = J^{-1} V_E^s requires solving a dense n-by-n linear system for a non-redundant manipulator; standard dense factorization or inversion costs Theta(n^3), and J is configuration-dependent so the inverse must be recomputed as q changes along a trajectory. The statement that J^{-1}(q) is computed only once in step 2.a does not remove this cost from a single evaluation, and no use is made of the serial-chain structure to solve the system faster. Moreover, for the demonstrated 7-DOF Panda, J is 6-by-7 and the pseudocode's J^{-1} is not defined; Remark 2 refers to a joint-space decomposition but supplies no algorithm or complexity analysis. The O(n) assertion should therefore be restricted to Algorithms 1 and 3, or Algorithm 2 should be replaced by an inverse-kinematics method whose complexity is actually analyzed.
  2. [Section IV, Algorithm 2, steps 2.b-5.b] The forward-kinematics loops are written for i=1,...,n-1, but the inverse-kinematics steps 3.a, 4.a, and 5.a require dotS_n, ddotS_n, and dddotS_n in the sums over i<=n. Those quantities are never assigned in the pseudocode, so Algorithm 2 is not executable as printed. The terminal-link screw derivatives should be computed from the given V_E^s, dotV_E^s, ddotV_E^s before the corresponding inverse-kinematics step, or the loops should run through n. This is a local but load-bearing fix for the claimed fourth-order inverse kinematics.
  3. [Section VI] The validation sentence ('The results were validated against the solution computed from the closed form analytic expressions of the EOM and their analytic derivatives') is not backed by any quantitative comparison in the manuscript. Figure 2 shows only the computed torque traces, not the error between the recursive and closed-form derivatives, so the reader cannot assess the agreement, especially for ddotQ_i whose magnitudes reach about 1000. Please add error norms and/or overlays for Q_i, dotQ_i, and ddotQ_i, and state the accuracy target.
minor comments (5)
  1. [Section VI] In the definition of the spatial screw coordinates, the text reads Y_j = (e_j, y_j)^T, but the numerical values correspond to Y_j = (e_j, y_j x e_j)^T; please correct the formula.
  2. [Eq. (11) and Appendix A] The notation e_d and e_r in Eq. (11) is never defined; please state explicitly that e_x denotes the skew-symmetric matrix of x.
  3. [Section VI, Figure 2] It would help to show the validation trajectory q(t) and to include an overlay of the recursive and closed-form results, since the current axis scales make the claimed agreement difficult to judge.
  4. [Section VI] The timing comparison is appropriately labeled preliminary, but because it includes Matlab overhead, a more informative comparison would report per-call operation counts or a compiled benchmark.
  5. [Algorithm 3 input] Algorithm 3 uses barW_i^{s,app}, dotbarW_i^{s,app}, and ddotbarW_i^{s,app}, while Section V defines W_i^{s,app}; please clarify whether gravity wrenches are folded into these terms.

Circularity Check

0 steps flagged · score 0.0 of 10

No significant circularity: the higher-order kinematics and inverse dynamics recursions are derived from standard screw-theoretic identities and validated externally, not assumed from the target claim.

full rationale

The paper's derivation chain does not reduce to its own inputs. Algorithm 1 is obtained by differentiating the recursive twist relation and the instantaneous-screw relation, with the derivative formulas for S_i stated explicitly in Eq. (9); no fitted parameter or target result is assumed. Algorithm 3 follows from the momentum definition, the spatial mass-matrix relation, the standard derivative identity (14), the time-differentiated Newton-Euler equations (15)-(22), and Jourdain's principle (27)-(28). The recursions for Q_i, Qdot_i, and Qddot_i are explicit algebraic consequences of those equations, not fitted quantities renamed as predictions. Dynamic parameters and the validation trajectory come from the external Gaz et al. reference [12], and the results are checked against closed-form analytic derivatives of the EOM, so the central inverse-dynamics claim has independent content. The author self-citations [19]-[22] provide standard background identities that are restated or directly derivable within the paper; none is invoked as an unverified uniqueness theorem or as the sole proof of the target algorithm. One non-circular caveat: the O(n) characterization of Algorithm 2 includes a dense n x n Jacobian inversion step qdot = J^{-1} V_E^s whose cost is not analyzed, and the paper itself defers detailed complexity analysis to future work; this is a complexity-correctness issue, not a circularity, so it does not raise the circularity score.

Assumptions & free parameters 0 free parameters · 5 assumptions · 0 invented entities

No free parameters are fitted; the dynamic parameters (mass, COM, inertia) are taken from external identification [12] and the trajectory from [12] as well. The derivation is a parameter-free use of standard Lie group mechanics. No new physical entities are introduced.

assumptions (5)
  • standard math The product of exponentials formula (4) with constant reference screw coordinates Y_j describes the configuration of a serial chain.
    Section II.A; foundational result from Brockett [1] and Murray et al. [23].
  • standard math The time derivative of the adjoint map is \dot{Ad}_C = ad_{V^s} Ad_C (Eq. 38), used to derive the joint screw derivatives (9).
    Section III and Appendix A; standard Lie group identity.
  • standard math The spatial mass matrix transforms as M^s = Ad_C^{-T} M^b Ad_C^{-1} and has the time derivative (14).
    Section V.A, Eqs. (13)-(14); standard rigid body dynamics result.
  • domain assumption The manipulator is an open chain of rigid bodies connected by 1-DOF joints; friction is neglected.
    Section II and Section VI; this sets the scope of the algorithm.
  • domain assumption Gravity is modeled by assigning the base body a gravitational acceleration G_0^s = (0,-g)^T.
    Section III; an alternative to explicit gravity wrenches, used in Algorithms 1-3.

how reviews work

0 comments
Cite this review

Pith. "Pith review of An $O(n$)-Algorithm for the Higher-Order Kinematics and Inverse Dynamics of Serial Manipulators using Spatial Representation of Twists." pith.science (2026). https://pith.science/paper/RZFWFKAN

@misc{pith2026250610686,
  author       = {Pith},
  title        = {Pith review of: An $O(n$)-Algorithm for the Higher-Order Kinematics and Inverse Dynamics of Serial Manipulators using Spatial Representation of Twists},
  year         = {2026},
  howpublished = {\url{https://pith.science/paper/RZFWFKAN}},
  note         = {Machine review of arXiv:2506.10686}
}
abstract

Optimal control in general, and flatness-based control in particular, of robotic arms necessitate to compute the first and second time derivatives of the joint torques/forces required to achieve a desired motion. In view of the required computational efficiency, recursive $O(n)$-algorithms were proposed to this end. Aiming at compact yet efficient formulations, a Lie group formulation was recently proposed, making use of body-fixed and hybrid representation of twists and wrenches. In this paper a formulation is introduced using the spatial representation. The second-order inverse dynamics algorithm is accompanied by a fourth-order forward and inverse kinematics algorithm. An advantage of all Lie group formulations is that they can be parameterized in terms of vectorial quantities that are readily available. The method is demonstrated for the 7 DOF Franka Emika Panda robot.

Figures

Figures reproduced from arXiv: 2506.10686 by the authors.

Figure 1
Figure 1. Industrial robot regarded as a manipulator equipped with SEA [PITH_FULL_IMAGE:figures/full_fig_p001_1.png] view at source ↗
Figure 2
Figure 2. Joint torques Qi and their first and second derivatives, Q˙ i and Q¨ i, for the validation trajectory according to equation (31) in [12]. allow for implementation of computationally efficient O (n) algorithms. The presented algorithm in spatial representation complements the Lie group formulation in body-fixed and hybrid representation reported in [21]. Therewith, second order inverse-dynamics O (n) algorithms are n… view at source ↗

Discussion (0). Continue with ORCID to comment.

Reference graph

Works this paper leans on

32 extracted references · 31 canonical work pages

  1. [21]

    M ¨uller: Recursive Second-Order Inverse Dynamics for Serial Manip- ulators, IEEE Int

    A. M ¨uller: Recursive Second-Order Inverse Dynamics for Serial Manip- ulators, IEEE Int. Conf. Robotics Automations (ICRA), May 29-June 3, 2017, Singapore

  2. [1]

    R. W. Brockett: Robotic manipulators and the product of exponentials formula, Mathematical Theory of Networks and Systems, Lecture Notes in Control and Information Sciences, 58, 1984, pp. 120-129a

  3. [2]

    Buondonno, A

    G. Buondonno, A. De Luca: A recursive Newton-Euler algorithm for robots with elastic joints and its application to control, 2015 IEEE/RSJ IROS, 5526-5532

  4. [3]

    Buondonno, A

    G. Buondonno, A. De Luca: Efficient Computation of Inverse Dynamics and Feedback Linearization for VSA-Based Robots, IEEE Rob. Aut. Letters, 1(2), 2016, 908-915

  5. [4]

    Carpentier, N

    J. Carpentier, N. Mansard: Analytical Derivatives of Rigid Body Dy- namics Algorithms, in: Robotics: Science and Systems (RSS 2018), Jun 2018, Pittsburgh, US

  6. [5]

    J. Carpentier et al.: The Pinocchio C++ library : A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives, IEEE/SICE International Symposium on System Integration (SII), Paris, France, 2019, pp. 614-619

  7. [6]

    Colom ´e and C

    A. Colom ´e and C. Torras: Closed-Loop Inverse Kinematics for Re- dundant Robots: Comparative Assessment and Two Enhancements, IEEE/ASME Transactions on Mechatronics, V ol. 20, No. 2, pp. 944- 955, 2015

  8. [7]

    De Luca: Decoupling and feedback linearization of robots with mixed rigid/elastic joints, Int

    A. De Luca: Decoupling and feedback linearization of robots with mixed rigid/elastic joints, Int. J. Rob. Nonlin. Cont., 8, 1998, 965-977 flexible joint manipulators, Multibody Syst Dyn. 32, 2014, 117-131

Show all 32 references
  1. [8]

    Featherstone: The Calculation of Robot Dynamics using Articulated- Body Inertias, Int

    R. Featherstone: The Calculation of Robot Dynamics using Articulated- Body Inertias, Int. J. Robotics Research, V ol. 2, No. 1, 1983, pp. 13-30

  2. [9]

    Featherstone: Rigid Body Dynamics Algorithms, Springer, 2008

    R. Featherstone: Rigid Body Dynamics Algorithms, Springer, 2008

  3. [10]

    Galicki: Time-optimal controls of kinematically redundant manipu- lators with geometric constraints, IEEE Trans

    M. Galicki: Time-optimal controls of kinematically redundant manipu- lators with geometric constraints, IEEE Trans. Robot. Autom., V ol. 16, no. 1, 2000, pp. 89-93

  4. [11]

    Gattringer, et al.: Recursive methods in control of flexible joint manipulators, Multibody Syst Dyn

    H. Gattringer, et al.: Recursive methods in control of flexible joint manipulators, Multibody Syst Dyn. 32, 2014, 117-131

  5. [12]

    C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano and A. De Luca, Dynamic Identification of the Franka Emika Panda Robot With Retrieval of Feasible Parameters Using Penalty-Based Optimization, IEEE Rob. Aut. Letters, V ol. 4, No. 4, pp. 4147-4154, 2019

  6. [13]

    Giftthaler et al.: Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation, Advanced Robotics, V ol

    M. Giftthaler et al.: Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation, Advanced Robotics, V ol. 31, No. 22, 2017, pp. 1225-1237

  7. [14]

    Giusti, J

    A. Giusti, J. Malzahn, N. G. Tsagarakis, M. Althoff: On the Combined Inverse-Dynamics/Passivity-Based Control of Elastic-Joint Robots, IEEE Tran. Robotics, vol. 34, no. 6, 2018, pp. 1461-1471

  8. [15]

    Guarino Lo Bianco, E

    C. Guarino Lo Bianco, E. Fantini: A recursive Newton-Euler approach for the evaluation of generalized forces derivatives, 12th IEEE Int. Conf. Methods Models Autom. Robot., 2006, 739-744

  9. [16]

    Guarino Lo Bianco: Evaluation of Generalized Force Derivatives by Means of a Recursive Newton–Euler Approach, IEEE Trans

    C. Guarino Lo Bianco: Evaluation of Generalized Force Derivatives by Means of a Recursive Newton–Euler Approach, IEEE Trans. Rob., 25(4), 2009, 954-959

  10. [17]

    Lynch, F.C

    K.M. Lynch, F.C. Park: Modern Robotics, Cambridge, 2017

  11. [18]

    S. Ma, M. Watanabe: Time optimal path-tracking control of kinemati- cally redundant manipulators, JSME Int. Journal, V ol. 47, No. 2, 2004, pp. 582-590

  12. [19]

    M ¨uller: Screw and Lie group theory in multibody dynamics – Motion representation and recursive kinematics of tree-topology systems, Multib Syst Dyn, V ol

    A. M ¨uller: Screw and Lie group theory in multibody dynamics – Motion representation and recursive kinematics of tree-topology systems, Multib Syst Dyn, V ol. 43, No. 1, pp. 1-34

  13. [20]

    M ¨uller: Screw and Lie group theory in multibody dynamics – Recursive algorithms and equations of motion of tree-topology systems, Multib Syst Dyn, V ol

    A. M ¨uller: Screw and Lie group theory in multibody dynamics – Recursive algorithms and equations of motion of tree-topology systems, Multib Syst Dyn, V ol. 42, No. 2, 2018, pp. 219-248

  14. [22]

    M ¨uller: An overview of formulae for the higher-order kinematics of lower-pair chains with applications in robotics and mechanism theory, Mech

    A. M ¨uller: An overview of formulae for the higher-order kinematics of lower-pair chains with applications in robotics and mechanism theory, Mech. Mach. Theory, V ol. 142, 2019

  15. [23]

    Murray, Z

    R.M. Murray, Z. Li, and S.S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press BocaRaton, 1994

  16. [24]

    Palli, C

    G. Palli, C. Melchiorri, A. De Luca: On the Feedback Linearization of Robots with Variable Joint Stiffness, IEEE Int. Conf. Rob. Aut. (IROS), Pasadena, CA, USA, May 19-23, 2008

  17. [25]

    F. C. Park, J. E. Bobrow, S. R. Ploen: A Lie group formulation of robot dynamics, Int. J. Rob. Research, V ol. 14, No. 6, 1995, pp. 609-618

  18. [26]

    Reiter, A

    A. Reiter, A. M ¨uller, H. Gattringer: On Higher-Order Inverse Kinematics Methods in Time-Optimal Trajectory Planning for Kinematically Redun- dant Manipulators, IEEE Trans. Industrial Informatics, V ol. 14, No. 4, 2018, pp. 1681 - 1690

  19. [27]

    Selig: Geometric Fundamentals of Robotics (Monographs in Computer Science Series), Springer-Verlag New York, 2005

    J. Selig: Geometric Fundamentals of Robotics (Monographs in Computer Science Series), Springer-Verlag New York, 2005

  20. [28]

    Siciliano, B., Sciavicco, L., Villani, L., Oriolo, G.: Robotics, Springer, London, 2009

  21. [29]

    Spong: Modeling and Control of Elastic Joint Robots, ASME J

    M.W. Spong: Modeling and Control of Elastic Joint Robots, ASME J. Dyn. Sys., Meas., Control, V ol. 109, No. 4, 1987, pp. 310-318

  22. [30]

    J. J. Uicker, B. Ravani, P. N. Sheth: Matrix Methods in the Design Analysis of Mechanisms and Multibody Systems, Cambridge University Press, 2013

  23. [31]

    Wampler: Inverse kinematic functions for redundant manipulators, IEEE Int

    C. Wampler: Inverse kinematic functions for redundant manipulators, IEEE Int. Conf. Robot. Autom., 1987, pp. 610-617

  24. [32]

    2020, https://wiki.ros.org/urdf

    Robot Operating System (ROS), Open Source Robotics Foundation, accessed 1 Nov. 2020, https://wiki.ros.org/urdf

Pith tools

Reviewed August 7, 2026 · model on record in the stance chip above.