Inconsistency in frame transformations

[edkoch]

There appears to be an inconsistency in the frame transformations between the poses that are returned by the local_position and what is sent to setpoint_position that was introduced with the last set of frame of reference changes a couple of months ago. Below are the transformations that are done in the corresponding plugins:

LocalPositionPlugin

    auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
    auto orientation = uas->get_attitude_orientation();

SetpointPositionPlugin
auto p = UAS::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
auto q = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation()));

It appears that you can not take a pose received from the local_position topic and feed it back to the setpoint_posiition without it causing the vehicle to change its yaw. Perhaps I'm missing something, but that seems incorrect.

[TSC21]

uas->get_attitude_orientation(); gets attitude in ENU. So, if you want to feed it back to the FCU, you have to apply UAS::transform_frame_enu_ned(Eigen::Quaterniond(uas->get_attitude_orientation())) so to feed it in NED.

[edkoch]

Are you suggesting I apply some sort of transformation in the application or are you suggesting a fix for mavros? If I understand the last set of reference frame changes from issue #216 then in theory one should be able to write an app that listens on the local_position topic which should give you an orientation in ENU and if I take that same pose without any translations and publish it to the setpoint_position topic then I would expect the UAS to not change position or orientation since I just told it to go the to the very same pose that it told me it was at. That does not appear to be what is happening. The yaw values I am getting from the local_position topic do not appear to have the 90 degree translation that is described in #216 in the local_position topic (i.e. NED->ENU), especially since it seems to be getting applied when you publish to the position_setpoint topic (i.e. ENU->NED). Thats where I think the inconsistency in mavros is. I'll take a closer look at the mavros code to make sure I'm not doing something stupid, but do people agree with my basic premise that I should be able to take a pose received from the local_position topic and send it to the setpoint_position topic without it changing the vehicle position or orientation?

[TSC21]

Well if you are feeding the orientation back through setpoint_point position directly I suppose it should work. Others already used this code with success so I can't point this is an inconsistency on mavros. I also tested it on ROS SITL with success. So I suppose there must be something wrong on what you are doing.

[edkoch]

Fair enough. Before I dig too deep I just want to make sure that my basic premise about feeding the local_position pose into the setpoint_position should work in principle especially since my code base use to work just fine and now it doesn't since I upgraded to the more recent version of mavros. In what file are the routines get_attitude_orientation() and transform_frame_enu_ned defined?

[TjarkP]

I also had some difficulties using the frame transformations. I encountered the problem while using the mocap_pose_estimate plugin. As motion capturing system I use OptiTrack. I digged into this issue and I think I know what all the confusion is about.

First of all there are two kinds of frames: ENU and NED which refers to respectively East North Up and North East Down:

The optitrack area is defined in an ENU frame:

The Pixhawk is defined in a NED frame:

So a frame transformation is needed to transform the frame from OptiTrack ENU to Pixhawk NED. The key here is in the initialization of your rigid body inside OptiTrack. Since a rigid body is made of multiple markers the software can't know the absolute orientation of your pixhawk with respect to the OptiTrack frame. When you create a rigid body it defines its current orientation as a unity rotation (1,0,0,0 quaternion) so it depends on how you initialize your Pixhawk inside the OptiTrack.

The mavros package expects you to align the x-axis of your Pixhawk with the x-axis of OptiTrack:

Then the frame conversion is only a rotation of 180 degrees around the x-axis to align both the frames and this is what happens inside the function transform_frame_enu_ned.

I would expect that both North of the axis would be aligned like this:

In that case a pure ENU-NED transformation will be needed which means swopping the x- and y- axis and negating the z-axis or for quaternions: w_ned = w_enu; x_ned = y_enu; y_ned = x_enu; z_ned = -z_ned.

I tested both situations succesfully so when I initialize the Pixhawk x-axis aligned with the x-axis of OptiTrack and I use the transformations provided by mavros it works and when I initialize the Pixhawk North (x-axis) with the OptiTrack North (y-axis) and use my own ENU-NED transformation it also works.

I think the naming of the function is a little bit confusing. I would expect an ENU-NED to swop the x- and y-axis and negate the z-axis but that is not what this function does. Maybe it should be renamed? Anyway, I hope you agree on my analysis.

[tonybaltovski]

Can you provide details on your software setup, particularly Motive version? The frames are are refering to are body fixed frames (ie both use X as forward). See #49.

[TjarkP]

I use NaturalPoint Tracking Tools version 2.3.3. This runs on Windows and will stream the rigid body information to an Ubuntu computer running ROS. There the package mocap_optitrack will collect the streamed data and publish the pose on a ROS topic. The OptiTrack ENU frame I am referring to is the frame as it is published on the ROS topic by mocap_optitrack.

[tonybaltovski]

Which branch of mocap_optitrack are you using?

[TjarkP]

I work on it with somebody else and I copied the package from him so I can't see it directly in git. But I compared my package with the master branch using diff command and my version is the same as the master.

[TjarkP]

There is also a frame conversion from NaturalPoint Tracking Tools to the ROS pose published by mocap_optitrack. In NPTT the frame is as follows:

[vooon]

Updates?