Joint refactor by dellaert · Pull Request #414 · borglab/GTDynamics

dellaert · 2026-02-10T15:33:06Z

Drastic simplification of Jacobian
Added fixed matrix pMc as instance variable
Store pMj not jMp (which does not make a lot of sense)

@varunagrawal this does change serialization. Is that a problem for you? Don't think it should matter (to anyone) as we always read from URDFs anyway.

dellaert · 2026-02-10T15:33:48Z

  // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`.
-  gtsam::Matrix6 exp_H_screw;
-  const Pose3 exp = Pose3::Expmap(screw, pTc_H_q ? &exp_H_screw : 0);
+  const Pose3 exp = Pose3::Expmap(screw);


Important simplification: exp_H_screw is expensive

dellaert · 2026-02-10T15:34:14Z

+    *pTc_H_q = cScrewAxis_;
  }
-  return pMc() * exp;  // Note: derivative of compose in exp is identity.
+  return pMc_ * exp;  // Note: derivative of compose in exp is identity.


Avoids 4x4 matrixmul in inner loop.

Copilot

Pull request overview

Refactors gtdynamics::Joint’s internal transform bookkeeping and Jacobian computation, updating C++/SWIG/Python call sites accordingly and adding a focused Jacobian test.

Changes:

Rename/flip the stored “rest” transform from jMp to pMj, and add a cached pMc transform.
Simplify Joint::parentTchild Jacobian computation and add direct numerical-vs-analytic tests.
Update SWIG interface and Python notebook usage for the new API (pMj() / pMc()).

Reviewed changes

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

Show a summary per file

File	Description
`gtdynamics/universal_robot/Joint.h`	Renames `jMp`→`pMj`, adds cached `pMc`, updates equality and Boost serialization fields.
`gtdynamics/universal_robot/Joint.cpp`	Updates constructor transform computation, simplifies `parentTchild` Jacobian path, uses cached `pMc_`.
`gtdynamics.i`	Updates wrapped API to `pMj()` and returns `pMc()` by const reference.
`tests/testJoint.cpp`	Adds numerical Jacobian checks for `parentTchild` (and axis equality assertions).
`tests/testRevoluteJoint.cpp`	Simplifies `using` declarations and updates serialization helper namespace usage.
`python/tests/chain.ipynb`	Switches from `jMp().inverse()` to `pMj()` usage.
`python/notebooks/cmopt_benchmark_dashboard.ipynb`	Notebook re-execution/output updates (benchmark timings/counts).

Copilot · 2026-02-10T15:53:10Z

+      pMj_(parent_link.lock()->bMcom().between(bTj)),
+      jMc_(bTj.between(child_link.lock()->bMcom())),
+      pMc_(pMj_ * jMc_),
+      pScrewAxis_(-pMj_.AdjointMap() * jScrewAxis),  // Negative sign ???


The constructor leaves an unresolved question in a code comment (// Negative sign ???). This reads like uncertainty about the correctness of the sign on the screw-axis transform; please either remove the comment and keep the sign, or replace it with a concrete justification (e.g., which adjoint/coordinate convention requires the minus sign). If the sign is actually wrong, this will silently flip joint directions in downstream kinematics/dynamics.

Suggested change

pScrewAxis_(-pMj_.AdjointMap() * jScrewAxis), // Negative sign ???

/// pScrewAxis_ is the joint screw axis expressed in the parent COM frame.

/// The negative sign reflects that the joint motion is defined as the child's

/// motion relative to the parent, while pScrewAxis_ is stored as a twist of

/// the parent frame under the chosen adjoint/twist convention.

pScrewAxis_(-pMj_.AdjointMap() * jScrewAxis),

dellaert · 2026-02-10T18:21:31Z

@copilot open a new pull request to apply changes based on the comments in this thread

Copilot · 2026-02-10T18:21:43Z

@dellaert I've opened a new pull request, #415, to work on those changes. Once the pull request is ready, I'll request review from you.

Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com>

Address code review feedback: clarify screw axis sign and fix pMc_ serialization

dellaert · 2026-02-10T23:11:37Z

@varunagrawal I added a few more optimizations in this PR

varunagrawal

Some comments

varunagrawal · 2026-02-11T01:23:45Z

    gtsam::Matrix6 H_twistdt;
    auto pose_t1_hat = predictPose(pose_t0, twistdt, H_pose_t0, H_twistdt);
-    gtsam::Vector6 error = pose_t1.logmap(pose_t1_hat);
+    gtsam::Vector error = pose_t1.logmap(pose_t1_hat);


What was the purpose of this? I actually liked the Vector6 since it specified the dimension within the type.

Error is returned by RVO, might as well immediately allocate right type, which is Vector

Won't the compiler do that for us? Might as well make it easier for the developer and be self-documenting.

I don't think so. Vector 6 is stack allocated, Vector is heap allocated. I think it will just lead to a copy.

RVO

I'm trying to be helpful as a reviewer by asking clarifying questions.

There's no need to be condescending.

RVO is literally a Google search away. I don't know why you would not search for it and make me type the answer instead.

Which is what I did and why I deleted the question before you could respond.

varunagrawal · 2026-02-11T01:28:58Z

        joint_->poseOf(joint_->child(), wTp, measured_joint_coordinate_, H_wTp);

-    gtsam::Vector6 error = wTc.logmap(wTc_hat, H_wTc, H_wTp ? &H : 0);
+    gtsam::Vector error = wTc.logmap(wTc_hat, H_wTc, H_wTp ? &H : 0);


Same comment about type here.

Answered above

varunagrawal · 2026-02-11T01:35:15Z

   * @param new_name The new name of the joint.
   */
-  void rename(const std::string& new_name) {name_ = new_name; }
+  void rename(const std::string &new_name) { name_ = new_name; }


You probably need to check your formatter settings. In the Google C++ Style Guide (which you recommended) the ampersand is on the type, not the name.

I don’t reformat files on PRs with single line changes to avoid adding a lot of unrelated diffs

This line has been reformatted though? Asking because when someone runs a formatter later, this will add a lot of distracting changes.

varunagrawal · 2026-02-11T01:35:49Z

  friend class boost::serialization::access;
  template <class ARCHIVE>
-  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+  void save(ARCHIVE &ar, const unsigned int /*version*/) const {


Shouldn't this be serialize since GTSAM's save and load will call this?

I am not sure I understand what is going on here...

Save/load is an alternative pipeline- this is to avoid saving pMc_

varunagrawal · 2026-02-11T01:59:00Z

      const double &torque,
      gtsam::OptionalMatrixType H_torque = nullptr) const override {
-    gtsam::Vector error = (gtsam::Vector(1) << torque).finished();
+    gtsam::Vector error(1);


You can instead define this via:

gtsam::Vector errror {{torque}};

I have verified this on my local machine.

Fine as it is, no?

dellaert · 2026-02-11T03:10:54Z

Appreciate the comments - but none of them rise to doing another CI cycle. Will merge if you’re ok.
My main purpose here is to was to shown you the improved FK code - although I suspect it’s not the bottleneck - did you ever profile that FK code that was so slow?

dellaert · 2026-02-11T03:11:49Z

Also, @varunagrawal , did you see question in PR comment?

dellaert added 2 commits February 10, 2026 07:45

Drastic simplification of Jacobian

99283d5

Refactor Joint

e19cb55

dellaert requested review from Copilot and varunagrawal February 10, 2026 15:33

dellaert commented Feb 10, 2026

View reviewed changes

Copilot started reviewing on behalf of dellaert February 10, 2026 15:47 View session

Copilot AI reviewed Feb 10, 2026

View reviewed changes

Initial plan

aed1168

Copilot AI mentioned this pull request Feb 10, 2026

Address code review feedback: clarify screw axis sign and fix pMc_ serialization #415

Merged

Copilot AI and others added 5 commits February 10, 2026 18:25

Address code review feedback on Joint refactor

424af87

Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com>

Fix trailing whitespace style issue

9a9356f

Co-authored-by: dellaert <10515273+dellaert@users.noreply.github.com>

Merge pull request #415 from borglab/copilot/sub-pr-414

3969f73

Address code review feedback: clarify screw axis sign and fix pMc_ serialization

Get rid of useless H

bc75549

optimize Eigen types

7b7b04e

varunagrawal reviewed Feb 11, 2026

View reviewed changes

dellaert merged commit 919f512 into master Feb 11, 2026
6 checks passed

dellaert deleted the feature/joint_refactor branch February 11, 2026 05:28

-      pScrewAxis_(-pMj_.AdjointMap() * jScrewAxis),  // Negative sign ???
+      /// pScrewAxis_ is the joint screw axis expressed in the parent COM frame.
+      /// The negative sign reflects that the joint motion is defined as the child's
+      /// motion relative to the parent, while pScrewAxis_ is stored as a twist of
+      /// the parent frame under the chosen adjoint/twist convention.
+      pScrewAxis_(-pMj_.AdjointMap() * jScrewAxis),

Uh oh!

Conversation

dellaert commented Feb 10, 2026

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Feb 10, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

dellaert commented Feb 10, 2026

Uh oh!

Copilot AI commented Feb 10, 2026

Uh oh!

dellaert commented Feb 10, 2026

Uh oh!

varunagrawal left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

dellaert commented Feb 11, 2026

Uh oh!

dellaert commented Feb 11, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants