New motion models

[giafranchini]

We should add three new motion models:

• ackermann
• point turn
• crab walk

And implement a method to change the behavior at runtime.

[giafranchini]

Models corresponding to different kinematic modes can be implemented in a single class that derives from fuse_core::AsyncMotionModel. To this end, it should be sufficient to:

• implement different template version of predict(), which accept a string with the locomotion mode to be used;
• implement a state kinematic constraint cost function. For this we have two options:
  • a single class which accepts locomotion mode as input -> calls predict() correct version, if statement to compute correct jacobians
  • multiple class for different cost functions -> calls predict() correct version, compute only jacobian corresponding to the correct kinematic mode

Problem: how to "remember" which kinematic mode was active between 2 specific timestamps where we would like to compute the transaction? This problem arises since we are able to read the current active kinematic mode from a dedicated ROS topic, but the optimizer will not ask immediately to generate a motion model transaction: applyMotionModels() is called at the start every optimization loop.

Possible solutions:

• create a map time - locomotion mode;
• ?????????????

[roncapat]

Which is the time window of each optimization loop? Because there will be for sure a "consistent" amount of time between two mode switching, where the rover will be steady.

[giafranchini]

It can be configured with a frequency parameter. In tests I set it to 30hz so the loop is binded to a timer with that frequency. The thing is, nothing is preventing the loop to take more than this time interval (this is for example what was happening on the rover last week). In general, these kind of delays are minimal (some ms) and can be avoided decreasing the frequency, but what if for some reason one iteration takes some seconds to finish, and in between there is a change in locomotion mode?

[roncapat]

I think we can at least for now ignore the issue, and if then it becomes evident that there is a practical problem, we can:

• first of all, quickly introduce a delay between each mode change (in EMRS locomotion software)
• then, understand how to fix Fuse properly

but at least we have workarounds for this corner cases.

[roncapat]

About: "create a map time - locomotion mode" can you expand a bit?

[giafranchini]

About: "create a map time - locomotion mode" can you expand a bit?

Something like a std::map with timestamp as keys and locomotion mode as values