search

borglab / GTDynamics

Full kinodynamics constraints for arbitrary robot configurations with factor graphs.
BSD 2-Clause "Simplified" License
39 stars 10 forks source link

Replace `AdjointMapJacobianQ` with `gtsam::Adjoint` w/ Jacobians #292

Closed gchenfc closed 2 years ago

gchenfc commented 2 years ago

After https://github.com/borglab/gtsam/pull/885 merges, then this will use, e.g.

Pose3 T = joint->parentTchild(q, T_H_q);
Vector6 twist_p = T.Adjoint(twist_c, H_T, H_twist_c);
H_q = H_T * T_H_q;

instead of:

Vector6 twist_p = T.Adjoint() * twist_c;
H_q = AdjointMapJacobianQ(joint, q) * twist_c;

This is just much more sane and generalizeable and uses more GTSAM code instead of custom code.

varunagrawal commented 2 years ago

@gchenfc I am undoing some duplicate changes from #290 since those changes are moot given the current state of master. I'm waiting for folks on Office Hours so going to use this time to land this. :)