Link::SetWorldPose() not behaving as expected

edit

Dear all,

I am currently putting in place a simulation trick for mecanum wheels, as seens in V-Rep. This trick allows to simulate mecanum wheels stably and without the need to simulate real mecanum wheels (quite daunting numerically speaking because of the numerous contacts).

A single roller is attached to the wheel and its axis is reoriented at each step in a gazebo plugin to enforce the roller to be always aligned with the ground plane (otherwise the roller axis would turn due to the turn of its wheel parent).

The hierarchy goes like this:

• mobile platform base link
  • wheel joint: continuous / revolute (its axis is actuated and is the normal axis of the wheel)
    • dummy wheel link
      • visual representation of the wheel (this is the mesh of the mecanum wheel)
      • roller joint: continuous / revolute (its axis is oriented at 45°)
        • roller link
          • collision geometry (this is a collision sphere)

This method is illustrated on the following video shot in VRep (see here). Note how the roller joints (in blue) remain aligned rather than rotating with the wheels (which they should normally do without the plugin, as the roller joint is a child of the wheel): this is the job of the plugin to realign the rollers at each step.

According to this hierarchy, the joint which needs to have its axis oriented at 45° at all time is the roller joint. In order to enforce this, a gazebo plugin is used and calls Link::SetWorldPose(). So basically we do not modify the axis coordinates with respect to the joint: what we do is reorient the joint itself using Link::SetWorldPose. The plugin works fine expect for one thing: I obtain the expected movement of the platform when I turn the wheels, but unfortunately the wheels do not visually look like they are rotating, see for yourself here.

Hence the following questions: I find it weird to have to call Link::SetWorldPose() on the roller link in order to modify the pose of the roller joint, is that the normal way to do? I could not find any other way... And what frame is actually modified when calling Link::SetWorldPose()? Is it the relative position of the input joint frame with respect to its parents (input joint frame = frame of the joint before rotation of the joint)? Basically I have the feeling that Link::SetWorldPose() somehow alters more than just a single frame...

Now because I am not too sure which frame is modified by Link::SetWorldPose(), I have tested Link::SetRelativePose() but my simulation "explodes" when I do that hence it is probably also not doing what I believe it does. Hence the same question for Link::SetRelativePose(): what frame is actually modified when calling Link::SetRelativePose()?

Am I using the right functions to reorient the roller joint at each step?

I am using ROS indigo and gazebo 2.2.

Thanks,

Antoine.

NOTE: obviously I have read the API doc 30 times before posting this question ;)