Pith. sign in

REVIEW 4 major objections 4 minor 3 cited by

Riemannian Direct Trajectory Optimization of Rigid Bodies on Matrix Lie Groups

T0 review · 4 major / 4 minor · reviewed 2026-08-16 · deepseek-v4-flash

Challenge this review Re-run · record.json Download PDF Read on arXiv ↗ Review JSON

Pith's one-line read The paper shows that directly optimizing rigid-body trajectories on the matrix Lie group $(SO(3)\times\mathbb{R}^3)^{N_b}$, with closed-form Riemannian derivatives, yields a singularity-free optimizer that is an order of magnitude faster…

desk verdict Genuine new derivation of closed-form Riemannian derivatives for LGVI dynamics on SO(3)xR^3, but the headline speedup claim overreaches the paper's own benchmark data. read the letter →

arxiv 2505.02323 v1 pith:DHD4TTKV submitted 2025-05-05 cs.RO cs.SYeess.SY

Sangli Teng , Tzu-Yuan Lin , William A Clark , Ram Vasudevan , Maani Ghaffari This is my paper · ORCID
classification cs.ROcs.SYeess.SY
keywords trajectoryoptimizationrigidbodydynamicsmatrixLiegroupsgroupvariationalintegratorRiemannianinteriorpointmethodSO(3)motionplanning
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

This paper proposes a direct trajectory optimization method for rigid-body robots that keeps rotations on the matrix Lie group $SO(3)\times\mathbb{R}^3$ instead of flattening them into coordinates such as Euler angles or three-parameter quaternions. It models discrete rigid-body dynamics with a Lie group variational integrator and derives closed-form first- and second-order Riemannian derivatives of those dynamics on the product manifold $(SO(3)\times\mathbb{R}^3)^{N_b}$. These derivatives feed a line-search Riemannian interior point method that handles general nonlinear constraints. If the claims hold, motion planning for drones and manipulators would avoid gimbal-lock-type singularities by construction, evaluate derivatives in time linear in the number of bodies and horizon, and run roughly an order of magnitude faster than conventional solvers in the reported tasks.

What carries the argument

The machinery is the pair consisting of a Lie group variational integrator and a second-order retraction, which is a map that moves a tangent direction back onto the manifold. The first ingredient discretizes rigid-body dynamics directly on $SO(3)\times\mathbb{R}^3$ as $R_{k+1}=R_kF_k$ and $p_{k+1}=p_k+v_k\Delta t$, preserving the group structure and the energy behavior of the continuous system. The second ingredient is the closed-form second-order retraction of the constraint map around the identity: each product of exponentials is vectorized with the logarithmic map and expanded with the Baker–Campbell–Hausdorff formula, so the first- and second-order terms in the perturbation parameter are exactly the Riemannian gradient and Hessian pieces needed for a Newton step. These pieces are assembled into the Karush–Kuhn–Tucker optimality system of a log-barrier problem and solved with a line-search Riemannian interior point method, whose key update is $x_{k+1}=R_{x_k}(d_k)$ for a tangent search direction $d_k$.

What would settle it

Compute the Riemannian exponential of $SE(3)$ under an inertia-weighted, non-bi-invariant metric at a configuration with nonzero angular velocity and compare it with the Lie exponential used in the paper's retractions; if the two disagree at second order, Assumption 1 fails for that group. Separately, run the Newton iteration on a long horizon or large perturbation and check whether the truncated Baker–Campbell–Hausdorff expansion stays inside its convergence radius; a diverging logarithmic map would show that the 'singularity-free' claim is only local.

Watch

Extended reading notes

Core claim

The paper's central discovery is that coupling the Lie group variational integrator with second-order retraction formulas turns constrained rigid-body trajectory optimization into a Riemannian Newton-type problem whose data are available in closed form. Under the assumption that the group admits a bi-invariant metric, the Riemannian exponential agrees with the Lie exponential, so the Hessian of the discrete dynamics and of holonomic constraints can be read off from a Baker–Campbell–Hausdorff expansion of products of exponentials; Table I lists the resulting second-order expansions for rotational and translational kinematics, rotational and translational dynamics, and pivot and axis constraints. Solving the KKT system of the log-barrier problem with a line-search Riemannian interior point method then respects the manifold structure at every iterate and requires no projection back onto $SO(3)$. The paper reports linear complexity in derivative evaluation and Newton steps, and demonstrates faster convergence than quaternion-based ambient-space optimization in drone docking and a 7-DOF manipulator task.

Load-bearing premise

Everything rests on the assumption that the configuration group has a bi-invariant metric, so that geodesics are exactly the matrix exponentials used in every retraction; this is true for $SO(3)\times\mathbb{R}^3$ but not for general groups such as $SE(3)$ with an inertia-weighted metric.

Editorial extensions

If this is right

  • Rigid-body motion planners can optimize on the true configuration space $(SO(3)\times\mathbb{R}^3)^{N_b}$ without ever leaving the manifold, so no projection or renormalization step is needed between Newton iterations.
  • Both derivative evaluation and Newton steps scale linearly with the number of rigid bodies and the planning horizon, which the paper verifies on a kinematic chain of increasing depth.
  • The method converges at a superlinear rate to very tight tolerances ($10^{-14}$) on unconstrained cases and handles nonconvex inequality constraints such as obstacle avoidance.
  • Compared with quaternion-based maximal-coordinate direct methods and generalized-coordinate solvers, the reported iterations-to-convergence and per-iteration times give an order-of-magnitude speedup in the tested drone and manipulator tasks.

Reading between the lines

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

  • A natural extension is to drop Assumption 1 and derive retractions for $SE(3)$ with an inertia-weighted metric, replacing the Lie exponential by the true Riemannian exponential or a modified connection; the closed-form Hessian would then pick up connection terms the paper omits.
  • The Baker–Campbell–Hausdorff truncation suggests a testable domain condition: one could bound the norm of iterated adjoint actions on Newton perturbations so that the series remains convergent, making the locality of the singularity-free claim quantitative.
  • Because per-iteration cost is dominated by solving the KKT system in maximal coordinates, coupling this Lie-group formulation with null-space reduction of joint constraints could preserve the topological benefits while shrinking the system size.
  • The same second-order retraction tables could be embedded in other constrained on-manifold solvers, such as augmented-Lagrangian or sequential quadratic programming methods, to test whether the speedup comes from the derivatives themselves or from the interior-point scheme.
Share X Bluesky LinkedIn Reddit HN

Signed reviews

No signed human review yet.

Editorial analysis

A structured set of objections, weighed in public.

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

Referee Report

4 major / 4 minor

Summary. The paper proposes a Riemannian direct trajectory optimization framework for rigid bodies formulated on the matrix Lie group product (SO(3) x R^3)^Nb. It uses a Lie group variational integrator for the discrete rigid-body dynamics, derives closed-form first- and second-order Riemannian derivatives for the dynamics and for common joint constraints via the BCH formula under the assumption of a bi-invariant metric, and solves the resulting constrained optimization with a line-search Riemannian interior point method. The paper claims linear complexity in the planning horizon and system degrees of freedom for both derivative evaluations and Newton steps, and claims an order-of-magnitude speed improvement over conventional solvers. The experiments include single-rigid-body drone docking with input limits and obstacle avoidance, and a 7-DOF KUKA manipulator benchmark comparing the proposed method with IPOPT in generalized coordinates.

Significance. If the technical claims hold, the paper offers a useful alternative formulation for rigid-body trajectory optimization: it preserves the rotation-group topology by construction, provides closed-form second-order expansions in Table I, and makes the source code publicly available. The linear-complexity derivative evaluation in maximal coordinates (Figure 6) is a concrete and reproducible contribution. However, the headline practical claim of an order-of-magnitude speedup is contradicted by the paper's own multi-rigid-body benchmark, and the generality of the 'singularity-free' statement is limited by Assumption 1. The derivative derivations and the constrained Riemannian interior-point formulation are plausible and worth publishing after the claims are aligned with the evidence.

major comments (4)
  1. [§VIII.C, Table III and Abstract] The unqualified claim in the abstract and conclusion that the method is 'faster than conventional methods by an order of magnitude' is not supported by the reported data. In Table III, the proposed RIPM on M_RB has a per-iteration time of 0.1159 s versus 0.0319 s for IPOPT with infeasibility restoration, i.e., it is about 3.6x slower per iteration, and it converges in only 21/100 cases versus 64/100 for IPOPT. Even in the single-rigid-body results of Table II, the per-iteration advantage is roughly 1.7x to 2.9x, not an order of magnitude. The authors should either supply total-solver-time comparisons at matched convergence rates or remove/qualify the order-of-magnitude claim.
  2. [§IX and §X] The limitations section explicitly states that the proposed solver is 'not as robust as IPOPT' and lacks infeasibility restoration, inertial correction, and second-order correction, yet the conclusion repeats the order-of-magnitude claim. These statements are internally inconsistent. The abstract and conclusion should be revised to reflect the measured convergence rates and per-iteration costs reported in Tables II and III.
  3. [§III.B, Assumption 1 and §VI.A] The closed-form BCH-based derivatives in Table I and Appendix A rely on Assumption 1, which requires that the group admits a bi-invariant metric and that the Riemannian exponential coincides with the Lie exponential. This holds for the product SO(3) x R^3 used in the experiments, but it is a genuine restriction for other Lie-group models such as SE(3) with an inertia-weighted left-invariant metric. Moreover, the truncated BCH expansion requires each Newton perturbation to stay in a neighborhood where the logarithm is single-valued, and the paper provides no domain-of-convergence analysis. The 'singularity-free' claim should therefore be stated as local and limited to the product structure used here.
  4. [§VI.C, §VIII.C, and Algorithm 1] The claim that the Newton steps required by the RIPM exhibit linear complexity is not demonstrated. Figure 6 measures only the time to evaluate zeroth-, first-, and second-order derivatives of the dynamics. Algorithm 1 invokes generic MATLAB linear system solvers for the KKT system in (53), and the paper does not describe a structure-exploiting factorization of that system or report a timing experiment for the Newton step itself. A linear-complexity statement about Newton steps needs either an algorithmic description of the linear solve exploiting the block structure or a corresponding measurement.
minor comments (4)
  1. [§VI.B, Eq. (44)] In the second-order retraction for the pivot constraints, both position terms appear with the subscript 2 on the tangent vector (xi_p^2); the first term should presumably be xi_p^1 and the second xi_p^2.
  2. [§II.B and §VIII.A.2] There are small typographical errors: 'Rodrigueze formula' should be 'Rodrigues formula', and 'Y ALMIP' should be 'YALMIP'.
  3. [§VI.A and Table I] The notation for the adjoint action in Eq. (40) and Table I is dense; a short definition of Ad_{X_{i+1,n}^{-1}} with the indexing convention would improve readability.
  4. [§VIII.B] The sentence 'The proposed method also takes only about 30% of the time as in IPOPT for the unconstrained cases in each iteration' is awkward; the comparison basis should be stated more precisely.

Circularity Check

0 steps flagged · score 0.0 of 10

No circularity found: the dynamics, derivative expansions, and solver are independent ingredients, and the central derivation does not reduce to its own inputs.

full rationale

The paper's claimed derivation chain is self-contained. The discrete rigid-body dynamics (Section V, Eqs. 30-35 and 60) are the standard Lie Group Variational Integrator equations from Marsden-West and Lee-McClamroch-Leok, not a quantity fitted to the optimization results. The first- and second-order expansions in Table I and Eq. (40) are obtained by substituting retraction curves and applying the standard BCH formula with adjoint actions, as shown in Appendix A; these are Taylor expansions of the constraint and dynamics functions, not restatements of the assumed dynamics. The RIPM solver in Section VII is a standard line-search interior-point method adapted from IPOPT and from Lai-Yoshise, with no uniqueness theorem or prior result by the authors invoked to force the formulation. Self-citations [12] and [17] appear only as pointers to related moment-relaxation work and as initialization for baseline solvers; they are not load-bearing for the derivation. Assumption 1 (bi-invariant metric and Lie exponential equal to Riemannian exponential) is a declared assumption, not an imported conclusion; it restricts the class of groups considered and the local validity of the truncated BCH expansion is a genuine correctness risk, but it is not a circular step. Section IX's acknowledgment that the solver is 'not as robust as IPOPT' and lacks infeasibility restoration is weighed here: it undercuts the abstract's unqualified 'order of magnitude faster' empirical claim, but an unsupported performance claim is an evidence problem, not circularity. No fitted parameter is renamed as a prediction, and no central claim is defined in terms of its own output.

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

The derivations depend on no fitted constants; user-selected cost weights, time step, and solver tolerances are problem data, not free parameters. The framework imports the LGVI dynamics and RIPM from prior work and uses standard Lie group mathematics. No new physical or mathematical entities are postulated.

assumptions (5)
  • domain assumption G admits a bi-invariant metric and the Riemannian exponential equals the Lie exponential (Assumption 1, Section III.B).
    All the closed-form second-order retraction and Hessian formulas in Section VI and Table I are derived under this assumption; it holds for SO(3)xR^3 but is restrictive for general SE(3) with inertia-weighted metrics.
  • domain assumption The discrete dynamics in Eq. (34)-(35) from the LGVI are a correct representation of rigid body motion (from [41], [12]).
    The trajectory optimization enforces these equations as equality constraints; if the discretization were dynamically inconsistent, the planned trajectories would be infeasible.
  • standard math BCH formula and adjoint action identities used in the expansions (Section VI, Appendix A).
    Standard Lie group machinery; assumed valid in the neighborhood where the series converges.
  • domain assumption Near each operating point, perturbations stay in the domain where the logarithmic map is single-valued and differentiable (Appendix A).
    The BCH-based second-order expansion and the vectorized constraints in Eq. (38) require that the group elements remain away from the cut locus (rotation angle pi); the paper does not analyze this domain.
  • domain assumption The KKT system (53) has block-sparse structure that can be solved in time linear in horizon and DOF (Section VII, Fig. 7).
    This follows from the graph structure of maximal coordinates, but the paper provides only a sparsity plot, not a solver-scaling test.

how reviews work

0 comments
Cite this review

Pith. "Pith review of Riemannian Direct Trajectory Optimization of Rigid Bodies on Matrix Lie Groups." pith.science (2026). https://pith.science/paper/DHD4TTKV

@misc{pith2026250502323,
  author       = {Pith},
  title        = {Pith review of: Riemannian Direct Trajectory Optimization of Rigid Bodies on Matrix Lie Groups},
  year         = {2026},
  howpublished = {\url{https://pith.science/paper/DHD4TTKV}},
  note         = {Machine review of arXiv:2505.02323}
}
read the original abstract

Designing dynamically feasible trajectories for rigid bodies is a fundamental problem in robotics. Although direct trajectory optimization is widely applied to solve this problem, inappropriate parameterizations of rigid body dynamics often result in slow convergence and violations of the intrinsic topological structure of the rotation group. This paper introduces a Riemannian optimization framework for direct trajectory optimization of rigid bodies. We first use the Lie Group Variational Integrator to formulate the discrete rigid body dynamics on matrix Lie groups. We then derive the closed-form first- and second-order Riemannian derivatives of the dynamics. Finally, this work applies a line-search Riemannian Interior Point Method (RIPM) to perform trajectory optimization with general nonlinear constraints. As the optimization is performed on matrix Lie groups, it is correct-by-construction to respect the topological structure of the rotation group and be free of singularities. The paper demonstrates that both the derivative evaluations and Newton steps required to solve the RIPM exhibit linear complexity with respect to the planning horizon and system degrees of freedom. Simulation results illustrate that the proposed method is faster than conventional methods by an order of magnitude in challenging robotics tasks.

Figures

Figures reproduced from arXiv: 2505.02323 by the authors.

Figure 1
Figure 1. Constrained Riemannian direct trajectory optimization of rigid bodies formulated on matrix Lie groups. The rigid body dynamics can be formulated [PITH_FULL_IMAGE:figures/full_fig_p001_1.png] view at source ↗
Figure 2
Figure 2. Example of a rigid body system in maximal coordinates and its Jacobian. The blue block indicates the Jacobians with nonzero elements. The lighter [PITH_FULL_IMAGE:figures/full_fig_p008_2.png] view at source ↗
Figure 3
Figure 3. Testing cases for drone docking from randomly generated initial poses [PITH_FULL_IMAGE:figures/full_fig_p009_3.png] view at source ↗
Figures from the paper (5 more)
Figure 5
Figure 5. Figure 5: We consider nonconvex norm constraints (x−xc) 2+ (y − yc) 2 ≥ r 2 for the drones to avoid the cylindrical regions. As the proposed RIPM considers general nonlinear constraints, future work can also incorporate more sophisticated represen￾tations of collision-free regio…
Figure 4
Figure 4. Figure 4: Convergence of RIPM on drone on SO(3) × R3 with full dynamics with 100 different initial conditions. The tolerance for convergence is set to E0 = 1e−14. The convergence rate of unconstrained cases in (a) is much faster than that of constrained cases in (b). The dashed …
Figure 5
Figure 5. Figure 5: Trajectories optimization for drone traversing complex environments considering general nonlinear constraints. [PITH_FULL_IMAGE:figures/full_fig_p011_5.png]
Figure 6
Figure 6. Figure 6: Average time to evaluate the zeroth-, first-, and second-order derivative [PITH_FULL_IMAGE:figures/full_fig_p011_6.png]
Figure 7
Figure 7. Figure 7: Illustration of the Hessian of the Lagrangian and Jacobians of the rigid body dynamics of the manipulator example. The block diagonal structure shows [PITH_FULL_IMAGE:figures/full_fig_p012_7.png]

Discussion (0). Continue with ORCID to comment.

Forward citations

Cited by 3 Pith papers

Reviewed papers in the Pith corpus that reference this work. Sorted by Pith novelty score. Full citation record

  1. A globally and superlinearly convergent QO-free method for nonlinear optimization on Riemannian manifolds

    math.OC 2026-08 conditional novelty 6.0 of 10

    A new QO-free method for constrained Riemannian optimization is proven globally, strongly, and superlinearly convergent, overcoming the Maratos effect for the first time in this setting.

  2. Learning Smooth SE(3) Trajectories under Left-Invariant Riemannian Metrics

    cs.RO 2026-08 conditional novelty 6.0 of 10

    A metric-conditioned network with analytic boundary completion generates smooth SE(3) trajectories in about a millisecond, approximating variational optima under left-invariant Riemannian metrics.

  3. Max Entropy Moment Kalman Filter for Polynomial Systems with Arbitrary Noise

    cs.RO 2025-06 conditional novelty 6.0 of 10

    MEM-KF approximates the Bayes filter for polynomial systems by propagating moments and recovering max-entropy distributions, with point estimates extracted via semidefinite relaxation.

Reference graph

Works this paper leans on

86 extracted references · 63 canonical work pages · cited by 3 Pith papers

  1. [54]

    Discrete-time differential dynamic programming on Lie groups: Derivation, convergence analysis, and numerical results

    George I Boutselis and Evangelos Theodorou. Discrete-time differential dynamic programming on Lie groups: Derivation, convergence analysis, and numerical results. IEEE Transactions on Automatic Control , 66(10):4636–4651, 2020

    work page 2020

  2. [8]

    MPC on manifolds with an application to the control of spacecraft attitude on SO(3)

    Uroš V Kalabi ´c, Rohit Gupta, Stefano Di Cairano, Anthony M Bloch, and Ilya V Kolmanovsky. MPC on manifolds with an application to the control of spacecraft attitude on SO(3). Automatica, 76:293–300, 2017

    work page 2017

  3. [1]

    Direct trajectory optimization using nonlinear programming and collocation

    Charles R Hargraves and Stephen W Paris. Direct trajectory optimization using nonlinear programming and collocation. Journal of guidance, control, and dynamics , 10(4):338–342, 1987

    1987

  4. [2]

    Survey of numerical methods for trajectory optimization

    John T Betts. Survey of numerical methods for trajectory optimization. Journal of guidance, control, and dynamics , 21 (2):193–207, 1998

    1998

  5. [3]

    A direct method for trajectory optimization of rigid bodies through contact

    Michael Posa, Cecilia Cantu, and Russ Tedrake. A direct method for trajectory optimization of rigid bodies through contact. International Journal of Robotics Research , 33(1):69– 81, 2014

    2014

  6. [4]

    FROST: Fast robot optimization and simulation toolkit

    Ayonga Hereid and Aaron D Ames. FROST: Fast robot optimization and simulation toolkit. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 719–726. IEEE, 2017. 12

    2017

  7. [5]

    Tracking for fully actu- ated mechanical systems: a geometric framework

    Francesco Bullo and Richard M Murray. Tracking for fully actu- ated mechanical systems: a geometric framework. Automatica, 35(1):17–34, 1999

    1999

  8. [6]

    John M. Lee. Smooth Manifolds , pages 1–29. Springer New York, New York, NY , 2003. ISBN 978-0-387-21752-9. doi: 10.1007/978-0-387-21752-9_1

    doi:10.1007/978-0-387-21752-9_1 2003

Show all 86 references
  1. [7]

    Linear-time variational integrators in maximal coordinates

    Jan Brüdigam and Zachary Manchester. Linear-time variational integrators in maximal coordinates. In International Workshop on the Algorithmic Foundations of Robotics , pages 194–209. Springer, 2021

    2021

  2. [9]

    Introduction to mechanics and symmetry

    Jerrold E Marsden and Tudor S Ratiu. Introduction to mechanics and symmetry. 1998

    1998

  3. [10]

    Nonholonomic mechanics

    Anthony M Bloch. Nonholonomic mechanics. In Nonholonomic mechanics and control , pages 207–276. Springer, 2003

    2003

  4. [11]

    Input influence matrix design for mimo discrete-time ultra-local model

    Sangli Teng, Amit K Sanyal, Ram Vasudevan, Anthony Bloch, and Maani Ghaffari. Input influence matrix design for mimo discrete-time ultra-local model. In 2022 American Control Conference (ACC), pages 2730–2735. IEEE, 2022

    2022

  5. [12]

    Convex geometric motion planning of multi-body systems on Lie groups via variational integrators and sparse moment relaxation

    Sangli Teng, Ashkan Jasour, Ram Vasudevan, and Maani Ghaf- fari. Convex geometric motion planning of multi-body systems on Lie groups via variational integrators and sparse moment relaxation. International Journal of Robotics Research , page 02783649241296160, 2024

    2024

  6. [13]

    Convex geomet- ric trajectory tracking using lie algebraic mpc for autonomous marine vehicles

    Junwoo Jang, Sangli Teng, and Maani Ghaffari. Convex geomet- ric trajectory tracking using lie algebraic mpc for autonomous marine vehicles. IEEE Robotics and Automation Letters , 8(12): 8374–8381, 2023. doi: 10.1109/LRA.2023.3328450

    2023

  7. [14]

    Toward safety-aware informative motion planning for legged robots

    Sangli Teng, Yukai Gong, Jessy W Grizzle, and Maani Ghaffari. Toward safety-aware informative motion planning for legged robots. arXiv preprint arXiv:2103.14252 , 2021

    2021 arXiv

  8. [15]

    Discrete-time hybrid automata learning: Legged loco- motion meets skateboarding

    Hang Liu, Sangli Teng, Ben Liu, Wei Zhang, and Maani Ghaffari. Discrete-time hybrid automata learning: Legged loco- motion meets skateboarding. arXiv preprint arXiv:2503.01842, 2025

    2025 arXiv

  9. [16]

    A general- ized metriplectic system via free energy and system˜ iden- tification via bilevel convex optimization

    Sangli Teng, Kaito Iwasaki, William Clark, Xihang Yu, Anthony Bloch, Ram Vasudevan, and Maani Ghaffari. A general- ized metriplectic system via free energy and system˜ iden- tification via bilevel convex optimization. arXiv preprint arXiv:2410.06233, 2024

    arXiv

  10. [17]

    Convex Geometric Motion Planning on Lie Groups via Moment Relaxation

    Sangli Teng, Ashkan Jasour, Ram Vasudevan, and Maani Ghaf- fari Jadidi. Convex Geometric Motion Planning on Lie Groups via Moment Relaxation. In Proceedings of the Robotics: Science and Systems Conference, Daegu, Republic of Korea, July 2023. doi: 10.15607/RSS.2023.XIX.058

    2023 doi

  11. [18]

    Progress in symmetry preserving robot perception and control through geometry and learning

    Maani Ghaffari, Ray Zhang, Minghan Zhu, Chien Erh Lin, Tzu- Yuan Lin, Sangli Teng, Tingjun Li, Tianyi Liu, and Jingwei Song. Progress in symmetry preserving robot perception and control through geometry and learning. Frontiers in Robotics and AI, 9:232, 2022

    2022

  12. [19]

    Lie algebraic cost function design for control on Lie groups

    Sangli Teng, William Clark, Anthony Bloch, Ram Vasudevan, and Maani Ghaffari. Lie algebraic cost function design for control on Lie groups. In Proceedings of the IEEE Conference on Decision and Control , pages 1867–1874. IEEE, 2022

    2022

  13. [20]

    An error-state model predictive control on connected matrix Lie groups for legged robot control

    Sangli Teng, Dianhao Chen, William Clark, and Maani Ghaffari. An error-state model predictive control on connected matrix Lie groups for legged robot control. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 8850–8857. IEEE, 2022

    2022

  14. [21]

    Legged robot state estimation in slippery environments using invariant extended kalman filter with velocity update

    Sangli Teng, Mark Wilfried Mueller, and Koushil Sreenath. Legged robot state estimation in slippery environments using invariant extended kalman filter with velocity update. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3104–3110. IEEE, 2021

  15. [22]

    Fully proprioceptive slip- velocity-aware state estimation for mobile robots via invariant kalman filtering and disturbance observer

    Xihang Yu, Sangli Teng, Theodor Chakhachiro, Wenzhe Tong, Tingjun Li, Tzu-Yuan Lin, Sarah Koehler, Manuel Ahumada, Jeffrey M Walls, and Maani Ghaffari. Fully proprioceptive slip- velocity-aware state estimation for mobile robots via invariant kalman filtering and disturbance o...

    2023

  16. [23]

    Legged robot state estimation within non-inertial environments

    Zijian He, Sangli Teng, Tzu-Yuan Lin, Maani Ghaffari, and Yan Gu. Legged robot state estimation within non-inertial environments. arXiv preprint arXiv:2403.16252 , 2024

    2024 arXiv

  17. [24]

    GMKF: Generalized moment kalman filter for polynomial systems with arbitrary noise

    Sangli Teng, Harry Zhang, David Jin, Ashkan Jasour, Maani Ghaffari, and Luca Carlone. GMKF: Generalized moment kalman filter for polynomial systems with arbitrary noise. arXiv preprint arXiv:2403.04712, 2024

    2024 arXiv

  18. [25]

    An introduction to optimization on smooth manifolds

    Nicolas Boumal. An introduction to optimization on smooth manifolds. Cambridge University Press, 2023

    2023

  19. [26]

    On-manifold preintegration for real-time visual– inertial odometry

    Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. On-manifold preintegration for real-time visual– inertial odometry. IEEE Transactions on Robotics , 33(1):1–21, 2016

    2016

  20. [27]

    Non- parametric continuous sensor registration

    William Clark, Maani Ghaffari, and Anthony Bloch. Non- parametric continuous sensor registration. Journal of Machine Learning Research, 22(271):1–50, 2021

    2021

  21. [28]

    SE-Sync: A certifiably correct algorithm for synchro- nization over the special euclidean group

    David M Rosen, Luca Carlone, Afonso S Bandeira, and John J Leonard. SE-Sync: A certifiably correct algorithm for synchro- nization over the special euclidean group. International Journal of Robotics Research , 38(2-3):95–125, 2019

    2019

  22. [29]

    Building rome with convex optimization

    Haoyu Han and Heng Yang. Building rome with convex optimization. arXiv preprint arXiv:2502.04640 , 2025

    2025 arXiv

  23. [30]

    Geometric control of mechanical systems: modeling, analysis, and design for simple mechanical control systems , volume 49

    Francesco Bullo and Andrew D Lewis. Geometric control of mechanical systems: modeling, analysis, and design for simple mechanical control systems , volume 49. Springer, 2019

    2019

  24. [31]

    Riemannian motion policies

    Nathan D Ratliff, Jan Issac, Daniel Kappler, Stan Birchfield, and Dieter Fox. Riemannian motion policies. arXiv preprint arXiv:1801.02854, 2018

    2018 arXiv

  25. [32]

    Synthesis of whole-body behaviors through hierarchical control of behavioral primitives

    Luis Sentis and Oussama Khatib. Synthesis of whole-body behaviors through hierarchical control of behavioral primitives. International Journal of Humanoid Robotics , 2(04):505–518, 2005

    2005

  26. [33]

    A unified approach for motion and force con- trol of robot manipulators: The operational space formulation

    Oussama Khatib. A unified approach for motion and force con- trol of robot manipulators: The operational space formulation. IEEE Journal on Robotics and Automation , 3(1):43–53, 1987

    1987

  27. [34]

    Russell, and Patrick M

    Shubham Singh, Ryan P. Russell, and Patrick M. Wensing. On second-order derivatives of rigid-body dynamics: Theory and implementation. IEEE Transactions on Robotics, 40:2233– 2253, 2024. doi: 10.1109/TRO.2024.3370002

    2024

  28. [35]

    Rigid body dynamics algorithms

    Roy Featherstone. Rigid body dynamics algorithms . Springer, 2014

    2014

  29. [36]

    The calculation of robot dynamics using articulated-body inertias

    Roy Featherstone. The calculation of robot dynamics using articulated-body inertias. International Journal of Robotics Research, 2(1):13–30, 1983

    1983

  30. [37]

    A Lie group formulation of robot dynamics

    Frank C Park, James E Bobrow, and Scott R Ploen. A Lie group formulation of robot dynamics. International Journal of Robotics Research, 14(6):609–618, 1995

    1995

  31. [38]

    Variational integrators for constrained dynamical sys- tems

    Sigrid Leyendecker, Jerrold E Marsden, and Michael Or- tiz. Variational integrators for constrained dynamical sys- tems. ZAMM-Journal of Applied Mathematics and Mechan- ics/Zeitschrift für Angewandte Mathematik und Mechanik: Ap- plied Mathematics and Mechanics , 88(9):677–708, 2008

    2008

  32. [39]

    Howell Taylor, Simon Le Cleac’h, Zico Kolter, Mac Schwa- ger, and Zachary Manchester

    A. Howell Taylor, Simon Le Cleac’h, Zico Kolter, Mac Schwa- ger, and Zachary Manchester. Dojo: A differentiable simulator for robotics. arXiv preprint arXiv:2203.00806 , 2022

    2022 arXiv

  33. [40]

    Discrete mechanics and variational integrators

    Jerrold E Marsden and Matthew West. Discrete mechanics and variational integrators. Acta Numerica, 10:357–514, 2001

    2001

  34. [41]

    A Lie group variational integrator for the attitude dynamics of a rigid body with applications to the 3D pendulum

    Taeyoung Lee, N Harris McClamroch, and Melvin Leok. A Lie group variational integrator for the attitude dynamics of a rigid body with applications to the 3D pendulum. In Proceedings 13 of IEEE Conference on Control Applications , pages 962–967. IEEE, 2005

    2005

  35. [42]

    Chomp: Covariant Hamilto- nian optimization for motion planning

    Matt Zucker, Nathan Ratliff, Anca D Dragan, Mihail Pivtoraiko, Matthew Klingensmith, Christopher M Dellin, J Andrew Bag- nell, and Siddhartha S Srinivasa. Chomp: Covariant Hamilto- nian optimization for motion planning. International Journal of Robotics Research, 32(9-10):1164...

    2013

  36. [43]

    Motion planning with sequential convex optimization and convex collision checking

    John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Jia Pan, Sachin Patil, Ken Goldberg, and Pieter Abbeel. Motion planning with sequential convex optimization and convex collision checking. International Journal of Robotics Research , 33(9):1251–1270, 2014

    2014

  37. [44]

    Contact-implicit trajectory optimization using variational integrators

    Zachary Manchester, Neel Doshi, Robert J Wood, and Scott Kuindersma. Contact-implicit trajectory optimization using variational integrators. International Journal of Robotics Re- search, 38(12-13):1463–1476, 2019

    2019

  38. [45]

    Time Optimal Ergodic Search

    Dayi E Dong, Henry P Berger, and Ian Abraham. Time Optimal Ergodic Search. In Proceedings of Robotics: Science and Systems , Daegu, Republic of Korea, July 2023. doi: 10.15607/RSS.2023.XIX.082

    2023 doi

  39. [46]

    Autonomous navigation of underactuated bipedal robots in height-constrained environments

    Zhongyu Li, Jun Zeng, Shuxiao Chen, and Koushil Sreenath. Autonomous navigation of underactuated bipedal robots in height-constrained environments. International Journal of Robotics Research, 42(8):565–585, 2023

    2023

  40. [47]

    On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming

    Andreas Wächter and Lorenz T Biegler. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical programming, 106:25– 57, 2006

    2006

  41. [48]

    SNOPT: An SQP algorithm for large-scale constrained optimization

    Philip E Gill, Walter Murray, and Michael A Saunders. SNOPT: An SQP algorithm for large-scale constrained optimization. SIAM review, 47(1):99–131, 2005

    2005

  42. [49]

    Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control

    Donghyun Kim, Jared Di Carlo, Benjamin Katz, Gerardo Bledt, and Sangbae Kim. Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control. arXiv preprint arXiv:1909.06586, 2019

    1909 arXiv

  43. [50]

    Representation-free model predictive control for dynamic motions in quadrupeds

    Yanran Ding, Abhishek Pandala, Chuanzheng Li, Young-Ha Shin, and Hae-Won Park. Representation-free model predictive control for dynamic motions in quadrupeds. IEEE Transactions on Robotics, 37(4):1154–1171, 2021

    2021

  44. [51]

    Vision-aided dynamic quadrupedal locomotion on discrete terrain using motion libraries

    Ayush Agrawal, Shuxiao Chen, Akshara Rai, and Koushil Sreenath. Vision-aided dynamic quadrupedal locomotion on discrete terrain using motion libraries. In Proceedings of the IEEE International Conference on Robotics and Automation , pages 4708–4714. IEEE, 2022

    2022

  45. [52]

    Perceptive locomotion through nonlinear model-predictive control

    Ruben Grandia, Fabian Jenelten, Shaohui Yang, Farbod Farshid- ian, and Marco Hutter. Perceptive locomotion through nonlinear model-predictive control. IEEE Transactions on Robotics , 39 (5):3402–3421, 2023

    2023

  46. [53]

    A computationally efficient algorithm for state-to-state quadro- copter trajectory generation and feasibility verification

    Mark W Mueller, Markus Hehn, and Raffaello D’Andrea. A computationally efficient algorithm for state-to-state quadro- copter trajectory generation and feasibility verification. In Proceedings of the IEEE/RSJ International Conference on In- telligent Robots and Systems , pages ...

    2013

  47. [55]

    Discrete mechanics and optimal control

    Oliver Junge, Jerrold E Marsden, and Sina Ober-Blöbaum. Discrete mechanics and optimal control. IFAC Proceedings Volumes, 38(1):538–543, 2005

    2005

  48. [56]

    Discrete geometric optimal control on Lie groups

    Marin B Kobilarov and Jerrold E Marsden. Discrete geometric optimal control on Lie groups. IEEE Transactions on Robotics, 27(4):641–655, 2011

    2011

  49. [57]

    Unlocking aerobatic potential of quadcopters: Autonomous freestyle flight generation and execution

    Mingyang Wang, Qianhao Wang, Ze Wang, Yuman Gao, Jing- ping Wang, Can Cui, Yuan Li, Ziming Ding, Kaiwei Wang, Chao Xu, et al. Unlocking aerobatic potential of quadcopters: Autonomous freestyle flight generation and execution. Science Robotics, 10(101):eadp9905, 2025

    2025

  50. [58]

    Low-rank matrix completion by Rieman- nian optimization

    Bart Vandereycken. Low-rank matrix completion by Rieman- nian optimization. SIAM Journal on Optimization , 23(2):1214– 1236, 2013

    2013

  51. [59]

    Solving low-rank semidef- inite programs via manifold optimization

    Jie Wang and Liangbing Hu. Solving low-rank semidef- inite programs via manifold optimization. arXiv preprint arXiv:2303.01722, 2023

    2023 arXiv

  52. [60]

    A nonlinear pro- gramming algorithm for solving semidefinite programs via low- rank factorization

    Samuel Burer and Renato DC Monteiro. A nonlinear pro- gramming algorithm for solving semidefinite programs via low- rank factorization. Mathematical programming, 95(2):329–357, 2003

    2003

  53. [61]

    An SQP method for equal- ity constrained optimization on manifolds

    Anton Schiela and Julian Ortiz. An SQP method for equal- ity constrained optimization on manifolds. arXiv preprint arXiv:2005.06844, 2020

    2005 arXiv

  54. [62]

    Sequen- tial quadratic optimization for nonlinear optimization problems on Riemannian manifolds

    Mitsuaki Obara, Takayuki Okuno, and Akiko Takeda. Sequen- tial quadratic optimization for nonlinear optimization problems on Riemannian manifolds. SIAM Journal on Optimization , 32 (2):822–853, 2022

    2022

  55. [63]

    Sequential optimality conditions for nonlinear optimization on Riemannian manifolds and a globally convergent augmented Lagrangian method

    Yuya Yamakawa and Hiroyuki Sato. Sequential optimality conditions for nonlinear optimization on Riemannian manifolds and a globally convergent augmented Lagrangian method. Com- putational Optimization and Applications, 81(2):397–421, 2022

    2022

  56. [64]

    Simple algorithms for op- timization on Riemannian manifolds with constraints

    Changshuo Liu and Nicolas Boumal. Simple algorithms for op- timization on Riemannian manifolds with constraints. Applied Mathematics & Optimization , 82(3):949–981, 2020

    2020

  57. [65]

    Riemannian interior point methods for constrained optimization on manifolds

    Zhijian Lai and Akiko Yoshise. Riemannian interior point methods for constrained optimization on manifolds. Journal of Optimization Theory and Applications , 201(1):433–469, 2024

    2024

  58. [66]

    Differential Geometry

    Loring W Tu. Differential Geometry. Springer Cham, 2017

    2017

  59. [67]

    Curvatures of left invariant metrics on Lie groups

    John Milnor. Curvatures of left invariant metrics on Lie groups. Advances in Mathematics , 21(3):293–329, 1976

    1976

  60. [68]

    Dis- crete Euler-Poincaré and Lie-Poisson equations

    Jerrold E Marsden, Sergey Pekarsky, and Steve Shkoller. Dis- crete Euler-Poincaré and Lie-Poisson equations. Nonlinearity, 12(6):1647, 1999

    1999

  61. [69]

    Manopt, a Matlab toolbox for optimization on manifolds

    Nicolas Boumal, Bamdev Mishra, P-A Absil, and Rodolphe Sepulchre. Manopt, a Matlab toolbox for optimization on manifolds. The Journal of Machine Learning Research , 15(1): 1455–1459, 2014

    2014

  62. [70]

    Optimality conditions for the nonlinear programming problems on Rieman- nian manifolds

    Wei Hong Yang, Lei-Hong Zhang, and Ruyi Song. Optimality conditions for the nonlinear programming problems on Rieman- nian manifolds. Pacific Journal of Optimization, 10(2):415–434, 2014

    2014

  63. [71]

    Numerical optimization, 2006

    Stephen J Wright. Numerical optimization, 2006

    2006

  64. [72]

    Ge- ometric tracking control of a quadrotor uav on SE(3)

    Taeyoung Lee, Melvin Leok, and N Harris McClamroch. Ge- ometric tracking control of a quadrotor uav on SE(3). In Proceedings of the IEEE Conference on Decision and Control , pages 5420–5425. IEEE, 2010

    2010

  65. [73]

    CasADi: a software framework for nonlinear optimization and optimal control

    Joel AE Andersson, Joris Gillis, Greg Horn, James B Rawlings, and Moritz Diehl. CasADi: a software framework for nonlinear optimization and optimal control. Mathematical Programming Computation, 11(1):1–36, 2019

    2019

  66. [74]

    J. Löfberg. Yalmip : A toolbox for modeling and optimization in matlab. In In Proceedings of the CACSD Conference , Taipei, Taiwan, 2004

    2004

  67. [75]

    Global optimization with polynomials and the problem of moments

    Jean B Lasserre. Global optimization with polynomials and the problem of moments. SIAM Journal on optimization , 11(3): 796–817, 2001

    2001

  68. [76]

    An introduction to polynomial and semi- algebraic optimization, volume 52

    Jean Bernard Lasserre. An introduction to polynomial and semi- algebraic optimization, volume 52. Cambridge University Press, 2015

    2015

  69. [77]

    Finding and optimizing certified, collision- free regions in configuration space for robot manipulators

    Alexandre Amice, Hongkai Dai, Peter Werner, Annan Zhang, and Russ Tedrake. Finding and optimizing certified, collision- free regions in configuration space for robot manipulators. In International Workshop on the Algorithmic Foundations of Robotics, pages 328–348. Springer, 2023

    2023

  70. [78]

    Towards optimizing a convex cover of collision-free space for 14 trajectory generation

    Yuwei Wu, Igor Spasojevic, Pratik Chaudhari, and Vijay Kumar. Towards optimizing a convex cover of collision-free space for 14 trajectory generation. IEEE Robotics and Automation Letters , 2025

    2025

  71. [79]

    The discrete null space method for the energy consistent integration of constrained mechanical systems: Part i: Holonomic constraints

    Peter Betsch. The discrete null space method for the energy consistent integration of constrained mechanical systems: Part i: Holonomic constraints. Computer Methods in Applied Me- chanics and Engineering , 194(50-52):5159–5190, 2005

    2005

  72. [80]

    The discrete null space method for the energy consistent integration of constrained mechanical systems

    Peter Betsch and Sigrid Leyendecker. The discrete null space method for the energy consistent integration of constrained mechanical systems. part ii: Multibody dynamics. International journal for numerical methods in engineering , 67(4):499–552, 2006

    2006

  73. [81]

    Discrete mechanics and optimal control for constrained systems

    Sigrid Leyendecker, Sina Ober-Blöbaum, Jerrold E Marsden, and Michael Ortiz. Discrete mechanics and optimal control for constrained systems. Optimal Control Applications and Methods, 31(6):505–528, 2010

    2010

  74. [82]

    Fast and certifiable trajectory optimization

    Shucheng Kang, Xiaoyang Xu, Jay Sarva, Ling Liang, and Heng Yang. Fast and certifiable trajectory optimization. arXiv preprint arXiv:2406.05846, 2024

    2024 arXiv

  75. [83]

    An inexact projected gradient method with rounding and lifting by nonlinear programming for solving rank-one semidefinite relax- ation of polynomial optimization

    Heng Yang, Ling Liang, Luca Carlone, and Kim-Chuan Toh. An inexact projected gradient method with rounding and lifting by nonlinear programming for solving rank-one semidefinite relax- ation of polynomial optimization. Mathematical Programming, pages 1–64, 2022

    2022

  76. [84]

    Hybrid zero dynamics of planar biped walkers

    Eric R Westervelt, Jessy W Grizzle, and Daniel E Koditschek. Hybrid zero dynamics of planar biped walkers. IEEE transac- tions on automatic control , 48(1):42–56, 2003

    2003

  77. [85]

    Geometric discretization of nonholonomic systems with sym- metries

    Marin Kobilarov, Jerrold E Marsden, and Gaurav S Sukhatme. Geometric discretization of nonholonomic systems with sym- metries. Discrete and Continuous Dynamical Systems Series S , 3(1):61–84, 2010

    2010

  78. [86]

    CALIPSO: A differentiable solver for trajectory optimization with conic and complementarity constraints

    Taylor A Howell, Kevin Tracy, Simon Le Cleac’h, and Zachary Manchester. CALIPSO: A differentiable solver for trajectory optimization with conic and complementarity constraints. In The International Symposium of Robotics Research, pages 504–

Pith tools

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