Angular velocity representations differ in mCurrentState.angular and merged_command.command.angular

[gabrielfpacheco]

I have notice what appears to be, IMHO, an inconsistency on the angular velocity control due to possibly having different representations for the angular velocity when considering the controller reference and the measured value. From the updateHook() method of PIDController.cpp

else
{
    mCurrentCov.linear = pose_sample.cov_velocity;
    mCurrentCov.angular = pose_sample.cov_angular_velocity;
    mCurrentState.angular = pose_sample.angular_velocity;

    // velocity control
    if (_world_frame){
        mCurrentState.linear = pose_sample.velocity;
    }
    else
    {
        double yaw = base::getYaw(pose_sample.orientation);
        mCurrentState.linear = Eigen::AngleAxisd(-yaw, Eigen::Vector3d::UnitZ()) *
            pose_sample.velocity;
    }
}

As mCurrentState.angular = pose_sample.angular_velocity, from the RigidBodyState documentation, we see that angular_velocity is the "Angular Velocity of sourceFrame relative to targetFrame, expressed in sourceFrame, as an axis-angle representation. The direction of the vector is the axis, its length the speed". Until that point, everything appears to be good to me and consistent to what is written in this section of the README of this repository: "in the world-aligned body velocity domain, linear is a (v_x, v_y, v_z) velocity and angular is a scaled-axis representation of the system’s angular velocity."

However, the angular part of merged_command is not changed neither on WorldToAligned nor in any other controller task. Thus, in my opinion, unless the velocity control loop is receiving its setpoint from an external component that is actually sending it as it should (an axis-angle representation of the angular velocity), the PIDController might be using a different representation for the reference angular velocity.

This could happen with a simple cascade from a position to an inner velocity control loop. As an example, consider the following, not unusual, use-case:

First, a position control loop is performed considering world-frame as the command frame. In that case, as per the documentation, the expected LinearAngular6DCommand "linear is a (x, y, z) position and angular is a (yaw, pitch, roll) expression of an attitude, where (yaw, pitch, roll) are a decomposition of the attitude over euler angles using the (Z, Y, X) decomposition."

Then, the output of the position loop could be cascaded to a velocity control loop and for that case, this velocity LinearAngular6DCommand would mean: linear is a (vx, vy, vz) reference while angular is an Euler angle rate (v_yaw, v_pitch, v_roll) reference. Thus, performing the velocity control loop either in the world or in the aligned frame would lead to the same issue: a comparison between (v_roll, v_pitch, v_yaw) with (wx, wy, wz).

This mapping from angular velocity to Euler angles rate (Jacobian of the representation) is actually not hard to obtain and its expression for the ZYX-order would be (from Fossen):

I guess this is not easily perceived because for roll ≃ pitch ≃ 0, which is quite common for AUVs, the Euler angles rate is equal to angular velocity (mapping equals to Identity). I have performed some unit tests that actually confirmed that merged_command.angular is not altered in any case when position_control = false for the PIDController task.

@joaobrittoneto , @saarnold , @doudou , do you have any thoughts on this? It may be that I got something wrong and I apologize in advance if I misunderstood something. If that is the case, I will close this issue.

[bwehbe]

Hello Gabriel, I have noticed this issue a while back when I was trying to use the PIDController as a pose control directly without the WorldToAligned component. The transformation in L108 of PIDController.cpp always assumes that the input is provided by the WorldToAligned which removes the roll and pitch, thus the rotation in yaw only. This fact makes the PIDController unusable for direct position and velocity control without having the WorldToAligned (which IMO should not be the case).

However my speculation is that this control chain was designed that way (meaning it is necessary to have the WorldToAligned component) was to avoid the singularity that you get in equation (2.28) when the pitch angle is close to 90 deg.

This issue is inherent to the cascaded control architecture, as the output of the position controller is considered as a reference velocity that is fed into the velocity controller. If the Euler difference is used as the error in the position control, that makes life easy as long as the pitch is not close to 90 deg, since this orientation error is a 3-component-vector representation and can be easier perceived geometrically. This would also be transformed to the body frame using the equation (2.28), keeping in mind the singularity.

Another method is to use the quaternion error representation described in [Antonelli, 2001]. Assuming the reference attitude is and the actual attitude is , the attitude error can be given as or expanded as the following

• 
• 

This method has a dual representation problem (one real attitude can have 2 quaternion representations) however that is fixed by looking at the sign of the and multiplying by -1 if . Pros of this method are no singularities, and equation (2.62) from the book by Fossen can be used to recover the angular velocities in body frame. However, the main issue here is that the control output is nonlinear with respect to angle errors.

There is also a third method that was suggested to me by @saarnold is which is to use manifolds to calculate the orientation error. I am not familiar with this method yet, but here is a paper that describes the mathematical formulation. It seems quite dense from the first look, but I will take a deeper look into it when I get the time.

[gabrielfpacheco]

Hi @bwehbe , thanks for your input. I really appreciate it!

I do agree with everything you mentioned, specially that the control-chain might have been designed that way to avoid the singularity around 90 deg. However, I've made this comment on the PR that I think we could leverage from your perspective as well.

My point there is that it would be better to perform such transform on the PIDController as @joaobrittoneto suggested. The way it is today, even if the user sets the command frame to be aligned, the angular velocity control is always performed in body command frame, unless the given RBS is already represented in the aligned frame, which would be the case if someone have set the angular velocity as an Euler rate vector.

However, doing it on the PIDController would require the transforms (direct and inverse) shown in (2.28) to be applied on the received RBS angular velocity (represented in the body frame) in PIDController (direct: body -> aligned) and in the AlignedToBody task (inverse: aligned -> body). This would be possible and consistent beacause the Euler rate vector and the axis-angle representation in the aligned frame are equivalent. As I mentioned in the comment, we could have a property to define in which frame the angular velocity is represented, to be backward compatible.

Another method is to use the quaternion error representation described in [Antonelli, 2001].

I think this is a great idea but, IMHO, if implemented, it should be added as an option for the attitude control for backward compatibility reasons.

There is also a third method that was suggested to me by @saarnold is which is to use manifolds to calculate the orientation error.

I think this is also a great idea, I gave a first look into the paper and it seems interesting. Another reference that I like on that topic is this paper.