Planning in a different frame to world

[Andrew Symington]

I am using MoveIt! (indigo release on 14.04) as a motion planning system for a free-flying platform similar to a quadrotor. I have modeled the propulsion system as a 6DoF floating joint. I am currently testing the planning system through rviz. One issue I am facing results from my visualization reference frame being different to the planning reference frame, because our GNC system presumes Z=down, while rviz requires Z=up. I therefore have the following TF relationship: /body --[D]--> /world --[S]--> /rviz, where [S] and [D] are static and dynamic transforms respectively.

Here is my SRDF file:

<?xml version="1.0" ?>
<robot name="freeflyer">
    <group name="freeflyer">
      <link name="body" />
      <joint name="pmc" />
    </group>
    <virtual_joint name="pmc" type="floating" parent_frame="world" child_link="body" />
</robot>

The fixed frame for rviz is /rviz.

It appears as if the rviz plugins don't seem to like the fact that the interactive markers are expressed with respect to the /rviz frame, while the robot trajectory is expressed with respect to the /world frame. The symptom is that the interactive markers work correctly, but the end state (orange) appears to move correctly in the opposite direction  -- as if it's being expressed incorrectly in the /world frame rather than the /rviz frame.

Interestingly, if I toggle the RViz fixed frame to /world and then back to /rviz using the GUI, the orange end state of the robot and the interactive marker now works correctly. However, the generated plan are in the incorrect coordinate frame.

Is there a way to explicitly specify the correct planning and visualization frames to the MoveIt! rviz plugins? Right now it seems as if I'll need to change my SRDF file to express the virtual joint with respect to /rviz in stead of /world, and then do a TF transform to /world in my action controller. This seems sub-optimal.

Andrew