Added tests for chains

[danbarla]

This PR adds tests to testChain.cpp.
The first test I added compares between classic analysis (wrench constraint for every link) of an A1 leg and kinematic chain analysis. This comes after I had trouble when comparing bigger factor graphs (classic way and chain way) and I had to go simple and follow the math. There is also an overleaf with detailed explanations.

The second test I added sets a basic graph for one leg of the A1 quadruped using DynamicsGraph, and another graph using Chain constraints. the test compares the results and checks them to be identical.

The third test I added compares a Dynamics Graph and a Chain graph for all 4 legs of the A1, for 1 time slice and 4 legs on the ground (robot standing still)

[dellaert]

I tried to read the overleaf but I am confused. I propose you

1. use L, U, H, T for the link frames, not ABC. Hard enough to read without translating.
2. when using a word in the text, always but symbol, and vice versa
3. most importantly: I think the parent-child relationship should be consistent, from the trunk (great-grandparent) to hip (grandparent) to upper (parent) to lower (child). So, the wrench unknowns will be F^1_L, F^2_U, F^3_H.
4. finally, do the joint numbers agree with A1 URDF? This is maybe the meta-advice: avoid introducing extra translation tables between symbols will make debugging and understanding more clear.

I'll take another look after you make those changes in doc and corresponding symbols in code. Hopefully just global replaces...

[danbarla]

adapted overleaf and symbols in code.

[dellaert]

@danbarla can you give me write access on the overleaf? Mostly for commenting but when I see a typo I can immediately fix...

[dellaert]

Still puzzled, in both overleaf and code, where the wrench from the ground is. Ultimately, we need a force exerted by the ground to keep the robot from falling, right? In the youtube video I did that by creating a "virtual joint" at the contact point and then asserting (via a constraint) that there was no moment exteted by the joint on the child link. I think in that case the child link is the "floor".

[danbarla]

overleaf and code updated with the ground constraint.

[dellaert]

Unfortunately I don't have cycles now to do a thorough review. If you think everything is correct, feel free to merge.

[varunagrawal]

@danbarla should we merge this?

[danbarla]

@danbarla should we merge this?

I am actively working on this branch. I will merge it next week.

[danbarla]

Hi @dellaert ,
I am ready to merge this PR.
It was opened and approved a year ago, but a lot has changed since then.
Most of the new code and constraints which I used for the paper and thesis was written and pushed to this PR after it was approved the first time. I realize it wasn't the best thing to do :) .
The hardcore changes are in the Chain and ChainDynamicsGraph classes.

[dellaert]

LGTM
Many TODOs left in code - code is fairly specific now to A1 - but will finally merge this.

[Copilot]

Pull Request Overview

This PR adds and refactors support for chain-based dynamics analysis and includes auxiliary setter methods and example cleanup. Key changes include:

• Refactoring the zero-initializer to public inheritance and improving value insertion logic in ChainInitializer.
• Extending Link, Joint, and Chain with new setters and chain-based expression APIs, and introducing ChainDynamicsGraph.
• Adding an alternative expression overload for contact moment constraints, adjusting simulation script, and updating example targets.

Reviewed Changes

Copilot reviewed 16 out of 16 changed files in this pull request and generated 7 comments.

Show a summary per file

File	Description
gtdynamics/utils/ChainInitializer.h	Made inheritance from Initializer public
gtdynamics/utils/ChainInitializer.cpp	Updated zero-value initialization, TODO cleanup, and twist insertion
gtdynamics/universal_robot/Link.h	Added setMass and setInertia setters
gtdynamics/universal_robot/Joint.h / Joint.cpp	Introduced childWrenchAdjoint API
gtdynamics/factors/ContactDynamicsMomentFactor.h	Added expression-based overload for moment constraint
gtdynamics/dynamics/DynamicsGraph.h	Marked factor methods virtual, exposed gravity()
gtdynamics/dynamics/ChainDynamicsGraph.h / ChainDynamicsGraph.cpp	New chain-based dynamics graph implementation
gtdynamics/dynamics/Chain.h / Chain.cpp	Extended Chain with expression factor methods
examples/example_a1_walking/sim.py	Adjusted simulation parameters and cleaned debugging
examples/example_a1_walking/CMakeLists.txt	Added combined runnable target

Comments suppressed due to low confidence (2)

gtdynamics/universal_robot/Link.h:145

• [nitpick] Add a Doxygen comment for setMass (and similarly for setInertia) to document parameter units and expected range, matching the style of existing getters.

inline void setMass(const double mass) { mass_ = mass; }

gtdynamics/universal_robot/Joint.h:455

• Document the childWrenchAdjoint method purpose and parameters; consider taking wrench_p as a const gtsam::Vector6_& if it is not modified to prevent unintended side effects.

gtsam::Vector6_ childWrenchAdjoint(gtsam::Vector6_ &wrench_p,

[dellaert]

Thanks @varunagrawal for thorough review. Co-pilot found mostly same issues but I ran it mostly to get an overview of the PR for posterity. THis code has some issues, mostly about generality to different robot topologies - but given that @danbarla graduated long ago we'll have to live with this until someone picks up this work.