Mecanum drive

[arennuit]

This is a proposition to integrate a mecanum_drive_controller in ros_controllers. This controller computes the wheels velocities given a desired twist and computes the odometry given the observed wheels velocity.

This PR relates to issue #147 : Making room for mecanum_drive_controller.

There is no unit test yet.

Also it is not clear yet which twists are expected as input (desired twist) and output (odometry twist). This question is raised in issue #147 and in ROS answers: http://answers.ros.org/question/196233/geometry_msgstwist-references-navigation-stack-ros_control/.

[arennuit]

This code still needs further testing, unit tests and review. I will try to do this when pressure cools down a bit :)

[bmagyar]

You can find plenty of tests in diff_drive_controller/test, feel free to refactor them. I'm sure most of them will come very useful to you!

((It might make sense after you have them to run a coverage tool to see which parts of the code are not being excersized during the tests.))

[arennuit]

@bmagyar You are right, this is exactely how I plan to proceed. Only need to get out of my rush to consolidate all this. Thanks ;)

[adolfo-rt]

Please let us know when the code stops being in flux and is in shape for a review. Adding tests would be a big plus.

[arennuit]

As for the unit tests I agree with you and this is my intention. As for the review, code has been rather stable for a week or two now, but I am still undergoing a few hand tests before making things officially for review ;)

[mikepurvis]

I'm interested in this, and may be able to contribute to tests and verification in the new year.

Regarding simulation, how do you expect this to work with Gazebo? Is a simulation of mecanum geometry sufficient to get accurate behaviour, or is it necessary to have a separate plugin which handles the properties of the wheels themselves?

Related: http://answers.gazebosim.org/question/4078/how-to-simulate-a-mecanum-wheel-in-gazebo/

[arennuit]

Glad to see there is interest about this package ;)

Regarding the simulation, I recommend not to use gazebo for mecanum wheels. I spent a lot of time (I mean A LOT ;)) only to realize gazebo is not quite ready yet for the scenario.

Basically you have 2 choices for simulating mecanum wheels:

• you simulate the real mecanics with the many rollers
  • this leads to complex simulations, hard to get stable
• you emulate them using this trick: http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=390
  • unfortunately you cannot do this in gazebo, see here why http://answers.gazebosim.org/question/7126/change-frame-of-a-linkjoint-without-moving-the/

The solution shown in the link you sent does not simulate the mecanum wheels at all: it is only a kinematic trick allowing to get good looking visuals, but unfortunately you do not get a proper mecanum wheels behavior.

Because of this we decided to use vrep rather than gazebo, and... well, this turned out to be an excellent decision. Vrep really rocks and does feel professional. I have coded a ros_control / vrep back-end example (equivalent to ros-indigo-gazebo-ros-control, but for vrep rather than gazebo), and it is available here: https://github.com/arennuit/vrep_ros_control_example.

Regarding your contribution, let me know when you feel ready, I may be able to help ;)

[mikepurvis]

@arennuit We're now using your mecanum_drive_controller on our prototype Ridgeback units, and we're very happy with it. One thing to note is that it's definitely hard-coded for the rectangular case, and I wonder if the name of the controller should reflect that (ie, that it is not suitable for a platform with six mecanum wheels, or three, or whatever).

Also, have you coalesced on an ideal approach to simulating mecanum wheels in Gazebo? We've not really been satisfied with any of the existing options.

[arennuit]

Hello Mike,

And thanks for your feedback. You are right, the fixed number of wheels the controller can handle is not clear from its name (and you certainly have to dig into the doc and code to know about it). I need to come back to the controller's code in a few weeks so this observation may lead to a bit of renaming ;)

Now as for simulating mecanum wheels inside gazebo, there are several ways to do it: 1) simulate the real mechanics: this has turned out to be very resource intensive and is not very stability, or 2) use a trick such as described here: this is the method I decided to go for. I have found using 2) within gazebo required code modifications of gazebo really deeply rooted and too time consuming to meet our R&D constraints. The restriction inside gazebo is described in this post and reported in this issue. The workaround I have found is to use vrep (after using it for a few months now, I can say this simulation software highly deserves attention, it really rocks) and the trick mentioned in 2). I have shared some code bridging vrep and ros_controllers in github here (this allows me to use the mecanum_drive_controller in a clean and straightforward way).
Now maybe you allow me to take advantage of your message to ask you another question. The mecanum_drive_controller takes a cartesian velocity as input (i.e. the command input in the mecanum_drive_controller is a twist). Now what we want to do is to have our mobile robot follow a cartesian path. We can sample our path in such a way that each sample contains the desired cartesian position, twist (and even acceleration but we do not use the acceleration) of the mobile robot - the desired cartesian path is obviously on a plane. In order to transform the desired path into a velocity command (i.e. the input of the mecanum_drive_controller) we use the following command law (this is C++ code):

twist_command[i] = kf[i] * desiredVelocity_inBody[i] + kp_[i] * errorPosition_inBody[i] + kv_[i] * errorVelocity_inBody[i];_

that is the twist command is made up out of the desired twist + feedback on the position and velocity errors.

This command law turns out not to be as stable as I had hoped. Hence my question: what method do you use in order to have your robot follow a path precisely? Do you use a command law such as mine, or do you have another method?

PS: sorry GitHub does not seem to regard my markdown instructions...

[efernandez]

std::abs, I think with abs only it's truncating to int type

[arennuit]

now fixed

[efernandez]

@arennuit Could you provide the reference for the kinematics equations you're using for the mecanum drive?
In principle, it seems to be based on this presentation by Mackenzie; although some signs differ on your implementation, the kinematic parameter wheels_k_ seems to be 4 times the a + b on Mackenzie's model, and it seems you don't follow the same wheels enumeration as him, as in this equations. According to it, the name mecanum drive looks appropriate to me.
Anyway, you're applying more equations... (at least in the odometry integration) so I guess you're using another reference.

[efernandez]

Since now the wheel velocities are actually velocities, you don't have to divide by dt, so you can get rid of this.

[arennuit]

Not sure I understand this one...

[arennuit]

Hello @efernandez, thanks for your feedback (sorry I have only been able to come back to the mecanum controller now. The equations I use come from here. The reference frame is indeed different from the one used Mackenzie (which explains the signs difference).
Now I am not sure I understand why you say my wheels_k is 4 times Mackenzie's a+b. wheels_k is computed in file mecanum_drive_controller.cpp
and is a mean on the 4 wheels, this looks fine to me, no?
And to answer your last question, the reason why there is a frame transformation in the odometry is because the odometry computes a body velocity (i.e. a body twist) but a world position (well the position is actually not expressed in the world frame but in the /odom frame, which can be different).

[arennuit]

Also, have you experimented with the quality of the odometry? I have a large drift which I am not happy with: a few centimers on a 10s movement - could be due to the fact I use simulation at 40Hz only. Can you let me know if the results you obtain with the odometry are good enough in your case?

--- UPDATE ---

The drift was actually caused by the feedback using odometry measures (there is not much that can be done here: drift is a well know problem of odometry). So I have double checked and I am now confident that the IK and FK bits of code are fine.

[arennuit]

I have thought about a workaround to get useful unit tests despite of not being able to simulate mecanum wheels in gazebo. We could actually design a robot using standard wheels (rather than mecanum wheels) and unit test the mecanum IK and FK (odometry) on this scene.
Running the scene and the unit test would not show that the controller is working (the behavior would obviously be nonesense) but this unit test could actually show that the behavior of the code does not change over time... which is actually the reason of being unit tests.
We know that the controller works, if we enforce its behavior does not change over time then we get what we want out of the unit tests, no?

[mikepurvis]

Note that I'm working on trying to simulate these wheels in Gazebo using directional friction. I believe it's pretty close to functional, though there is some remaining oddball behaviour that I was hoping to get some assistance from the Gazebo devs on: http://answers.gazebosim.org/question/8693/mecanum-simulator-with-directional-friction/

[adolfo-rt]

Can't you get some basic testing in place without a dynamic simulator?. Something like comparing controller outputs to known correct results?. Some white-box (implementation aware) testing could maybe exercise different known code paths, or validate different configurations (wheel placement, size, etc.). I typically like to unit test the guts (actual computation) in simple, very targeted tests, and then test the whole system (ROS interfaces) in a higher-level, but less exhaustive integration rostest.

[arennuit]

@mikepurvis interesting, keep us informed on how this goes...

@adolfo-rt you are right the gazebo-based tests are more functional test than unit tests. What you propose is real unit tests (atomic as unit tests should be).

[LiohAu]

any chance to see this merged one day ? :)

[mikepurvis]

I pulled the guts of it into ridgeback_control: https://github.com/ridgeback/ridgeback/tree/indigo-devel/ridgeback_control/include/mecanum_drive_controller

[jonbinney]

I'd love to have this merged in; it sounds like lack of tests is the main barrier? @mikepurvis do you have any new thoughts on the feasibility of accurate omni-drive simulation in gazebo?

[mikepurvis]

Re-pasting ridgeback/ridgeback#23 (comment):

I really wanted the directional friction solution to work, and sunk a nontrivial amount of time on it, but all that was ~3 years ago and under Gazebo 7. It's possible that the situation is different under Gazebo 8 or 9— if anyone's interested to pick up the work again, you can start here:

https://github.com/ridgeback/ridgeback_simulator/blob/indigo-devel/mecanum_gazebo_plugin/src/mecanum_plugin.cpp

https://github.com/ridgeback/ridgeback/blob/kinetic-devel/ridgeback_description/urdf/ridgeback.gazebo#L89-L107

Basically just switch the plugins in the URDF over to the commented-out ones, launch the sim, and give it a series of twists that command it to move in X and Y. Generally I found that the "strafe" commands would seem to work correctly in either direction for a metre or two and then it would seemingly go unstable and spin out of control. I strongly suspect that the plugin was not correctly updating the directional friction as the wheel turned, but without a reliable way to visualize the friction vector in realtime, it seems like a dead end to try to fix it.

[bmagyar]

Let's try this for Noetic, see ref above