wrench factor with expressions

borglab / GTDynamics

Full kinodynamics constraints for arbitrary robot configurations with factor graphs.

BSD 2-Clause "Simplified" License

39 stars 10 forks source link

wrench factor with expressions #308

Closed yetongumich closed 2 years ago

yetongumich commented 2 years ago

Re-writing wrench factor and related factors using expressions, so we can easily transition to constrained optimization later. This is transitional, because eventually all "soft" factors will disappear and instead be generated by a call to createFactor of the EqualityConstraint class.

Added Python tests to factors.

varunagrawal commented 2 years ago

@yetongumich CI is failing due to both C++ and Python.

dellaert commented 2 years ago

All this extra work for nothing! @varunagrawal I am rather upset about this.

varunagrawal commented 2 years ago

@yetongumich Frank and I spoke about this PR and the fix is easy. We just need to remove the wrapped functions for all the factors (since that is the root cause of the breakage) and then you can also kill the python unit tests for the factors. Once you do that, this PR will be awesome and I will gladly approve. :)

dellaert commented 2 years ago

Thanks @varunagrawal . For all the "factor functions". These functions were stopgap measures to make the c++ code work (almost) as is, and the corresponding factors were not used (except PoseFactor, other PR). Sorry about the extra work, @yetongumich , I tried to say this in our meeting and in the PR comments but I guess I there was a breakdown in communications.

varunagrawal commented 2 years ago

One more thing before I forget. We can undo the change from ExpressionFactor to NoiseModelFactor::shared_ptr that was done due to my comments. Since the wrapper is not an issue anymore, having full type info of the ExpressionFactor will be fantastic!

yetongumich commented 2 years ago

Got all factor wrappers removed (except for a temporary wrapper for WrenchFactor which keeps the cablerobot tests still working).
Added a TODO and issue in removing the wrapper for WrenchFactor in a later PR.
As for whether the factor functions should return an Expression factor or a shared_ptr, Frank and I had this discussion before, and it's still reasonable to return a shared_ptr, because this will avoid the copy of the factor when adding to the factor graph.

varunagrawal commented 2 years ago

@yetongumich we've merged this which is awesome, but we should still revert all the NoiseModelFactor::shared_ptrs to ExpressionFactors.