search

borglab / GTDynamics

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

Improvements to ContactEqualityFactor #339

Closed varunagrawal closed 2 years ago

varunagrawal commented 2 years ago

The ContactEqualityFactor is a bit half-baked (which is my fault) so making amends with this PR. Added traits and a bunch of other niceties to make it work in proper optimization.

~~The most important change is the flag enforce_equality_ which if set to true activates the factor, else returns error zero. This is so that it can be used directly with the DCMixtureFactor where it is set to true for stance feet and false for everything else.~~

varunagrawal commented 2 years ago

We could also maybe add an OptionalFactor to gtsam/hybrid which serves as a wrapper around regular factors and provides the enforce_ flag. That way the signature would be DCMixtureFactor<OptionalFactor<ContactEqualityFactor>>.

dellaert commented 2 years ago

Hmmm, not loving it, and the OptionalFactor goes towards the better way to handle in hybrid. Maybe it is even better to just have the ability to have a nullptr as component in a mixture? No need for extra c++ hassle (OptionalFactor), as long as we document it well?

varunagrawal commented 2 years ago

That sounds doable. An alternative I was thinking of was doing was using the base Factor class, so we could do DCMixtureFactor<Factor> and have one component be JacobianFactor(key1, Matrix::Zero(), b1, key2, Matrix::Zero(), b2).

A nullptr is a billion dollar mistake after all.

dellaert commented 2 years ago

That sounds doable. An alternative I was thinking of was doing was using the base Factor class, so we could do DCMixtureFactor<Factor> and have one component be JacobianFactor(key1, Matrix::Zero(), b1, key2, Matrix::Zero(), b2).

A nullptr is a billion dollar mistake after all.

We already explicitly allow nullptrs in Factor graphs. As long as you document and test well this is the cleanest solution, and it is also the fastest in terms of computation time. No need for dummy factors that output zero.

varunagrawal commented 2 years ago

@dellaert I've removed the enforce_equality flag. I'll be using just a large covariance value for the noise model.