Adding Non Holonomic constraint

[Alessio Parmeggiani]

Hello, I would like to know if anyone know of tips or references on how to add NH constraints to a system, as I am estimating the poses of a robot that can't move along the vertical and lateral direction.
I tried using prior factors on the velocity (setting lateral and upward velocity to 0, and setting high sigma for forward velocity and low sigma for the other directions) and between factors between subsequent Pose3 (setting the delta pose as 0 for the lateral and upward direction and sigmas like above) but I am getting mixed results. 
Looking at the marginals of the poses it doesn't seems to have the correct influence and strange behavior happens when the NH prior factor is used with a combinedIMUFactor. 

Do you know if there is a standard way to implement this type of constraint?
Thank you

[Brice Rebsamen]

Hello

Have you tried without the between factors? your IMU factor should
take care of it... Also for the velocity prior, do you have some sort
of wheel speed sensor? in that case use a low sigma on forward
velocity (up to the estimated noise model of your wheel speed sensor)
and a larger sigma on lateral velocity (after all you don't really
know how much slip there is, especially when turning), and also on the
vertical velocity (if you have some sort of suspension between the
wheels and the body). And of course, make sure to take into account
that the IMU factor models the velocity in world frame, but your NH
factors will be in body frame...

best of luck
Brice

> --
> You received this message because you are subscribed to the Google Groups "gtsam users" group.
> To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
> To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/4189784a-6f29-4939-bae2-b17306f29068n%40googlegroups.com.

[Dellaert, Frank]

Thans Brice!

Last bit you said is important.

In C++ You could create a custom factor 2D on body velocity, but you need to carefully unit-test the Jacobians! Of use might be this method in NavState (if you use ImuFactor2):

// Return velocity in body frame

Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;

which already has at least the Jacobian of body velocity wrp a change NavState. So, an ExpressionFactor with a Constrained noise model will already do the trick.

If in python, you’d have to create a customFactor, see https://borglab.github.io/gtsam/customfactor/

Unfortunately, bodyVelocity is not exposed in wrapper yet, but with the custom factor you could create the right Jacobian, even 2*9 (as you are only constraining 2 directions, both a function of 9-D NavState).

Best

Frank

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/CAP4g%3DjV5jqztS-NDBQb7cpXKYB0Eqc5w9NidPpGQPNc91SB8YQ%40mail.gmail.com.