Vision Estimates and/or setpoints not rotated properly (forwarded quaternions are wrong)

[MarcGyongyosi]

Hey guys... me again.

So when I switched to the new firmware I tried to use the external heading option which seems to work in general. I then tried to use the setpoint position plugin together with vision estimations (in a dry run) and well....

First, I think there might be a sign error somewhere since the yaw setpoint is off by PI/2 (i sent a quat of 0,0,0,1 for yaw to be 0 or "forward" expecting the yaw setpoint to be 0).

Here is a plot of the setpoint yaw and att.yaw. You can see it's off by PI which I suspect is probably to a plus/minus mistake in the rotation from ENU to NED.

In addition, why is att.yaw at -pi/2 ? I would expect it to be at 0 since I haven't rotated the quad.

In the case that the pi/2 offset is necessary to do the rotation from ENU to NED (which might be the case here) I can see how they need to be off by that pi/2. However, when I send a yaw=0 setpoint in a pose where yaw is already 0 then I would expect them both to be offset by pi/2 in the same direction - i.e. not setpoint +pi/2 and att.yaw -pi/2 as in the above plot.

Furthermore, I am getting some very spooky quaternions for the vision estimates coming through to the pixhawk (i.e. the visn.quatxyzw ones). the qx value jumps up and down quite dramatically every once in a while (in this plot once I send the setpoint).

Maybe this behavior is unqiue to my setup (the pixhawk is rotated) and I will do my own rotations for now, but this might be something that could cause some pretty bad and unexpected behavior for others..

What do you guys think?

Best, marc

[TSC21]

@andrea-nisti updates?

[andrea-nisti]

Hello, me again.

unfortunately we experienced a crash and I'm trying to fix things up. On the other hand I was able to retrieve a flight log which with stable performances.

http://logs.uaventure.com/view/rKbMtzZuQpYaJ5FVrmGYw3

What I do is send the ENU pose got from the mocap_optitrack ROS package directly to Mavros and everything works fine. Moreover, no conversions are made during this step (just feed the pose you get to the appropriated mavros topic). Estimator is LPE + att_q , I was not able to use ekf2 properly but I will give it a shot in the future. Yaw estimation is external and I had to increase to 0.6 LPE_PN_V param in order to obtain a decent flight performance.

[TSC21]

unfortunately we experienced a crash and I'm trying to fix things up.

Sorry to hear about that. Do you know what caused it?

What I do is send the ENU pose got from the mocap_optitrack ROS package directly to Mavros and everything works fine. Moreover, no conversions are made during this step (just feed the pose you get to the appropriated mavros topic). Estimator is LPE + att_q , I was not able to use ekf2 properly but I will give it a shot in the future. Yaw estimation is external and I had to increase to 0.6 LPE_PN_V param in order to obtain a decent flight performance.

Sounds great! Sou basically that confirms that the MAVROS transformations are ok, right? What about the angular velocities? (https://github.com/mavlink/mavros/issues/523#issuecomment-306529753)

[TSC21]

Also about your findings, setup and results: would be awesome if you could share them on the Devguide (setup, conventions, tips) and on the discuss.px4.io/ (results). It would help others that want to use MOCAP systems with PX4 and LPE.

[SiddharthPatel45]

@andrea-nisti yes, that is what even I was saying all that time that I was not using any conversion and directly sending the mocap data to Pixhawk via ROS, which worked for me. Glad to hear that you could also make it work (though sorry to hear about your crash). And I was also not able to get it work with ekf2 but only the lpe with att q estimator. Hope this ends all confusions.

[andrea-nisti]

Hello! Regarding the crash, wasn't a Firmware/mavros fault. We are running experiments on landing on a floating platform and there was an issue on the software managing that part. Basically the alignment on the final descent was wrong and the robot crashed on the ground; nothing serious but we need time for replacements.

in a minute i will give you feedback on angular velocities; since that measure is independent from position estimation, I think that they will be in robot frame.

Next step will be posting on devguide how to setup a motion capture system with PX4.

My setup is:

• Windows machine running motive software (Optitrack)
• RaspberryPI3 connected through telem2 to PixHawk running mavros and mocap_optitrack

Rasp and windows pc are on the same network and they communicate through wifi. Beside optitrack node and mavros, the raspberry runs different modules concerning the landing and the communicate with PX4 sending position setpoints to mavros. Rasp runs Ubuntu MATE 16.04

Yaw estimation is external (from mocap).

[TSC21]

@andrea-nisti sounds great! Thank you for this!

[andrea-nisti]

@TSC21 I do not have access to the mocap area right now but I tried echoing the values in mavros/imu/data and I see angular velocities in robot frame (ENU). This should close any doubt about this issue I think

[TSC21]

@andrea-nisti ok cool! This should close this issue now.

Just a quick note: there's a plan for implementing a TF listener to establish the frame transforms, instead of using static transforms as done right now. This should be something to test, so it would be cool if you could then offer your help to test that. Thanks!

[andrea-nisti]

@TSC21 Thanks, just ping me when you need some testing and I will provide help ;)

[suraj2596]

Alright, so I disabled all the mavros internal rotations and got it working nicely. No more jumps and weird behavior. My implementation is hacky and probably only works on my setup because of my vision setup with multiple cameras, but I can share the few lines of code that I use to rotate here:

void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr) {
        /**
         * @warning Issue #60.
         * This now affects pose callbacks too.
         */
        if (last_transform_stamp == stamp) {
            ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped.");
            return;
        }
        last_transform_stamp = stamp;

        auto position = Eigen::Vector3d(tr.translation());

        double px = position.x();
        double py = -position.y();
        double pz = -position.z();

        double roll, pitch, yaw;

        Eigen::Quaterniond q_eigen(tr.rotation());

        tf::Quaternion q_tf(q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());

        tf::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);

        vision_position_estimate(stamp.toNSec() / 1000,
                px,py,pz,
                roll, -pitch, -yaw);

        /*std::cout << "input roll: " << roll << " output roll: " << roll << std::endl;
        std::cout << "input pitch: " << pitch << " output pitch: " << -pitch << std::endl;
        std::cout << "input yaw: " << yaw << " output yaw: " << -yaw << std::endl;*/

    }

This assumes the following orientation of the vision inputs (coming into mavros)

Vision estimates relative to Pixhawk at origin: x - Forward y - Left z - Up

Roll, Pitch, Yaw are according to the standard convention around these 3 axes.

I know this is not ideal since the orientation of my vision coordinate frame is non-standard, but it works properly for me.

So in the end, I have the following mapping:

Vision Input: <-> NED output (in terms of vision input) x <-> x y <-> -y z <-> -z

roll <-> roll pitch <-> -pitch yaw <-> -yaw

I hope this helps some of you!

Hi, How did you disable mavros internal rotations? I gave a 180deg yaw offset and my values on ODOMETRY message are jumping a lot before stabilizing. Reason for doing the 180deg offset: x and y axis directions are swapped. TIA

[AlexisTM]

This is a 3y old issue, thus not valid anymore.

The swaps are due to ENU <-> NED conversions. The Pixhawk is in NED (FRD) and ROS is in ENU (FLU).

[cnpcshangbo]

Hello @AlexisTM, I am encountering similar issues. Since the discussion in this issue is not valid anymore, which documentation should I refer to correctly set up the orientation from Vicon to MAVROS? Currently, I directly ported Vicon(w,x,y,z) to Mavros(w,x,y,z), but the plot shows that that quaternion seems to be the opposite. Thanks.

[AlexisTM]

This is close to a 6y issue. Thank you not to use it.