processor_odom_2d exact integration if theta != 0

processor_odom_2d has two integration functions :

• exact integration if theta != 0 (impl : theta > 10e-3)
• Runge-Kutta integration otherwise

Added a test with data :

test_processor_odom_2d ~/path/to/wolf/src/example/data/odom_2d_loop.dat .

Data are basically ROS geometry_msgs/TwistStamped recorded from TIAGo. The robot in moving around for 90sec coming back to its starting point with but facing back ( ~ x:0 y:0 theta:pi ).

See e.g. slide 5

@jsola @joanvallve This is wip and aims at using exact integration in case the orientation isn't too close to zero. This is similar to the odometry of the ROS DiffDriveController. Looking forward for feedback.

Marked this merge request as a Work In Progress

Added 1 commit:

• 85a857ff - fix odom_2d math ?

Added 32 commits:

• 85a857ff...27413eeb - 29 commits from branch master
• c8818322 - add test_processor_odom_2d
• bdb0a119 - processor_odom_2d exact integration if theta != 0
• 3a44ee08 - add odom_2d data

cleaning, fixing, rebasing on master.

Unmarked this merge request as a Work In Progress

Added help wanted label

Do you think it matters so much so as to integrate with RK? Which RK scheme do you use? RK-4?

The RK method is what was done before, I add the exact one. I suppose it is RK-4 as it is refer to as classical RK.

dt wasn't taken into account - not sure why - with this pr it is.

Uhm... in the original odom2d, as was coded by JV, the arc of circle is integrated with half turn - advance - half turn. As such, it is not a RK method! At least it does not seem so to me...

dt was not taken into account because the input is directly a displacement, not a velocity. If you use velocities as input data then you need dt.

Ooh crap I got it all wrong ><. I'll roll back and make a proper class. I close this PR.

Status changed to closed

You can still use the class, but maybe if what you want to do is very different you need a new one. Possibly the word 'odometry' is misleading, it means too many different things. In our case, it's the integration of small displacements, call them dx. You can easily work with velocities by assigning a dt and doing the proper math. I think that, if this is the only difference, I would use the same class, just with a flag indicating if data is dx, of if data is v, because there is not so much difference (and dx can be easily computed with dx = v * dt). This flag can be either a bool, or just doing dt = -1 or something.

Yes I assumed that the data was velocities as it is the case I usually encounter. I think the simpler will be to make a proper class, say ProcessorDiffDrive. I ran the math but that will need a check :D.

Yes. You could even inherit from odom2d to make DiffDrive... just a suggestion. Probably, you just need to code data2delta()

For a diff drive, you use wheel velocities and dt as inputs?

Well in projects in which I need to integrate velocities myself - e.g. odometry calibration.

Its actually having the odometry calibration in mind that I was writing this, throwing the base for estimating the parameters.

Indeed. We'll probably have to do some calibration for the odometry in our project Logimatic. Our two inputs are linear and angular velocities, just as your diffDrive model. Go ahead. If you need to calibrate params, just put them in the intrinsics_ StateBlock in the sensor, and use them in the Processor. Wolf will calibrate them! :-D Well -- it will if the constraint also uses them!

What's Logimatic ? CargoAnts successor ?

Yes