New/fixing derivs no benchmarks by DVolpi256 · Pull Request #100 · ROAM-Lab-ND/generalized_rbda

DVolpi256 · 2026-05-26T17:40:28Z

Implemented a new algorithm called firstOrderInverseDynamicsDerivatives in ClusterTreeDynamics.cpp that computes the first-order derivatives of rigid-body dynamics for closed-chain kinematic systems.
Two new unit tests were added to validate the new algorithm: testInverseDynamicsDerivativesSimple validates the algorithm against a first-order finite difference to an accuracy on the order of 10^-8 and testInverseDynamicsDerivativesComplexStep validates the algorithm against a complex step derivative to an accuracy on the order of 10^-14 (i.e., machine precision).
New terms were added to handle the new algorithm. This is especially apparent in GenericJoint.h and GenericJoint.cpp, where new CasADi functions to handle the presence of a configuration-dependent joint motion subspace matrix S.
A new model of Agility Robotics' Cassie and model variants of the Tello robot were added to evaluate the algorithm's handling of closed-chain kinematic systems. A model of the KUKA LWR 4+ robot was also added to test a real robot arm.

Has temporary variables for the backward pass as well. Comments are made for new functions that need to be implemented.

…ded some efficiencies after review.

… two unit test files and fixed the resulting type issues.

Need Casadi to add additional terms

Merging to add support for Ubuntu 24 instead of 18

…tives function in testRBDAlgos

… 456 of ClusterTreeDyamics.cpp. The error pertains to a segmentation fault that I believe is related to the usage of the Psi_dot_ term.

…sses.

…h a finite difference for fixed-base robots.

…finite difference and the other does a complex step. They work well for the double pendulum case. The three link and four link serial chains and Mini Cheetah have larger errors.

…s derivatives tests now pass.

…does not pass.

…ent terms

… Cheetah. Examining how to get the complex step working and whether testRigidBodyDynamicsAlgosDerivatives is now broken.

…aking the existing unit tests.

…Dynamics to e1bf655

- Reverted src/Dynamics/ClusterTreeDynamics.cpp to e1bf655 - Reverted include/grbda/Utils/OrientationTools.h to e1bf655 - Reverted src/Dynamics/ClusterJoints/FreeJoint.cpp to e1bf655 - Reverted src/Robots/MiniCheetah.cpp to e1bf655 - Reverted UnitTests/testHelpers.hpp to e1bf655 - Modified UnitTests/testInverseDynamicsDerivativesSimple.cpp: - Updated conf_add to use [pos(3), quat(4)] ordering - Use h=1e-6 for floating base (instead of 1e-8) - Use tolerance 1e-5 for floating base tests - Joint.h remains with [pos(3), quat(4)] ordering (e1bf655 baseline) Results: - testInverseDynamicsDerivativesSimple: 4/4 PASSED (including MiniCheetahQuaternion) - testRigidBodyDynamicsAlgosDerivatives: 14/14 PASSED

…s() to Generic

…r variablees.

pwensing · 2026-06-08T20:42:40Z

+                    const DMat<Scalar> &S_j = cluster_j->S();
+
+                    // dtau_dq(ii, jj) = t1^T * Psi_ddot_j + t4^T * Psi_dot_j
+                    dtau_dq.block(ii, jj, num_vel_i, num_vel_j)=


pwensing · 2026-06-08T20:42:57Z

+
+                    if (j < i)
+                    {
+                        dtau_dq.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t3;


pwensing · 2026-06-08T20:43:09Z

+                        }
+                    }
+
+                    dtau_dq_dot.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t2;


pwensing · 2026-06-08T20:43:18Z

+
+                    dtau_dq_dot.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t2;
+                    dtau_dq_dot.block(ii, jj, num_vel_i, num_vel_j)=
+                        t1.transpose() * cluster_j->Upsilon_dot_ + t4.transpose() * S_j;


pwensing · 2026-06-08T20:45:07Z

+                    const DMat<Scalar> &S_j = cluster_j->S();
+
+                    dtau_dq.block(ii, jj, num_vel_i, num_vel_j)=
+                        t1.transpose() * cluster_j->Psi_ddot_ + t4.transpose() * cluster_j->Psi_dot_;


pwensing · 2026-06-08T20:45:12Z

+
+                    if (j < i)
+                    {
+                        dtau_dq.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t3;


pwensing · 2026-06-08T20:45:23Z

+                        }
+                    }
+
+                    dtau_dq_dot.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t2;


pwensing · 2026-06-08T20:45:29Z

+
+                    dtau_dq_dot.block(jj, ii, num_vel_j, num_vel_i)= S_j.transpose() * t2;
+                    dtau_dq_dot.block(ii, jj, num_vel_i, num_vel_j)=
+                        t1.transpose() * cluster_j->Upsilon_dot_ + t4.transpose() * S_j;


pwensing · 2026-06-08T20:47:48Z

+                const D6Mat<Scalar> F4_b = BC0_b.transpose() * S0_b;
+
+                // Diagonal blocks: accumulate over all bodies
+                dtau_dq_dot.block(ii, ii, num_vel_i, num_vel_i)+=


pwensing · 2026-06-08T20:47:58Z

+                // Diagonal blocks: accumulate over all bodies
+                dtau_dq_dot.block(ii, ii, num_vel_i, num_vel_i)+=
+                    F1_b.transpose() * Upd0_b + F4_b.transpose() * S0_b;
+                dtau_dq.block(ii, ii, num_vel_i, num_vel_i)+=


pwensing · 2026-06-08T20:49:24Z

+                const D6Mat<Scalar> &Psidblock = cluster_nodes_[parent]->Psid0_[parent_subindex];
+                const D6Mat<Scalar> &Psiddblock = cluster_nodes_[parent]->Psidd0_[parent_subindex];
+
+                // Off-diagonal blocks


noalias for these next four

…ties to avoid dynamic allocation

pwensing · 2026-06-10T13:24:13Z

+
+                // Diagonal blocks: accumulate over all bodies
+                dtau_dq_dot.block(ii, ii, num_vel_i, num_vel_i)+=
+                    F1_b.transpose() * Upd0_b + F4_b.transpose() * S0_b;


Rather than using F1 and F4, we have a different formula in the draft (using one matrix multiply instead of two). Can you confirm that the unit test will still pass with that modification?

(This came up when I was using the code to write up the world frame version of the algo, and there was an inconsistency in the formula for the diagonal blocks)

pwensing · 2026-06-10T13:24:22Z

+                dtau_dq_dot.block(ii, ii, num_vel_i, num_vel_i)+=
+                    F1_b.transpose() * Upd0_b + F4_b.transpose() * S0_b;
+                dtau_dq.block(ii, ii, num_vel_i, num_vel_i)+=
+                    F1_b.transpose() * Psidd0_b + F4_b.transpose() * Psid0_b;


Rather than using F1 and F4, we have a different formula in the draft (using one matrix multiply instead of two). Can you confirm that the unit test will still pass with that modification?

(This came up when I was using the code to write up the world frame version of the algo, and there was an inconsistency in the formula for the diagonal blocks)

…opArm accordingly. Comments were added to document how it should eventually be transformed to GenericJoint.

…atial.h dead code

Please enter the commit message for your changes. Lines starting Added an alias for std::vector<Mat6, ...> for improved readability

… some conditionals.

DVolpi256 and others added 30 commits October 23, 2025 13:28

Forward pass on first order derivatives

698d3d0

Has temporary variables for the backward pass as well. Comments are made for new functions that need to be implemented.

Added a backward pass and changed derivative matrix dimensions.

9bc3af0

Added swapped force product to first order derivative function and ad…

b594f40

…ded some efficiencies after review.

Tested firstOrderInverseDynamicsDeriviatives with additional lines in…

64162ef

… two unit test files and fixed the resulting type issues.

Added S_ring terms

7158c94

Need Casadi to add additional terms

Merge remote-tracking branch 'origin/main' into first_order_derivatives

67839c2

Merging to add support for Ubuntu 24 instead of 18

Started attempt to fix memory allocation error when calling ID deriva…

f993b1e

…tives function in testRBDAlgos

Narrowed down the memory-related errors to a problem starting on line…

412f4e3

… 456 of ClusterTreeDyamics.cpp. The error pertains to a segmentation fault that I believe is related to the usage of the Psi_dot_ term.

build cleanup for debugging

26503ec

Fixes for sizing

2f548bf

Less aggressive debug flags

8202e2d

Debugged the rest of the ID derivatives function, and now the test pa…

49cd0e1

…sses.

Added std::complex<double> support from the complex step branch

b87f990

Made adjustments to test the inverse dynamics derivative function wit…

4f96b30

…h a finite difference for fixed-base robots.

Changed the finite difference to a complex step

6aedefa

Made separate tests for the inverse dynamics derivatives. One does a …

53901fd

…finite difference and the other does a complex step. They work well for the double pendulum case. The three link and four link serial chains and Mini Cheetah have larger errors.

Fixed an issue pertaining to gravity propagation. The inverse dynamic…

4bf2ca1

…s derivatives tests now pass.

Condensed the ID derivatives forward pass.

8416cfb

Added the Mini Cheetah to the finite difference code.

3555b8e

streamlined unit test

163b6d2

Merge branch 'anon_func' into first_order_derivatives

996775e

prototype code

c104c3f

prototype code

4767fbf

Expanded out the framework for a floating base finite difference. It …

e2bf3ff

…does not pass.

Fleshed out Lie group complex step and tested with Mini Cheetah.

9ad7776

Added comments to ClusterTreeDynamics.cpp documenting necessary gradi…

e1bf655

…ent terms

Made fixes to get testInverseDynamicsDerivatives working for the Mini…

a4d792a

… Cheetah. Examining how to get the complex step working and whether testRigidBodyDynamicsAlgosDerivatives is now broken.

The finite difference step for the Mini Cheetah now works without bre…

61cae82

…aking the existing unit tests.

Fix MiniCheetah test with [pos, quat] ordering and revert ClusterTree…

8ec3fb8

…Dynamics to e1bf655

DVolpi256 added 4 commits June 3, 2026 21:53

Refined a comment in Triple and fixed outdated parts and added noalia…

3cd9938

…s() to Generic

Cleaned up noalias() and eval() usage. Also made some variables membe…

c7588ff

…r variablees.

std::swap now used for F_out

91ddb26

Clean ups

3657255