Fix angvel representation issue

[gabrielfpacheco]

This PR would close isse #35 that I have reported. This does so by creating a new property in the WorldToAligned task that actually allows the user to explicitly say that provided angular part of the command is represented by an Euler rate vector and, only in this case, performs the corresponding transformation.

This new property does not make any modifications when the position control domain is used. Also, it is set to false by default so that this PR is backward compatible with already deployed systems.

IMHO, this now allows the user to properly set a cascade from a world position control (that provides euler rates as angular commands) to a world-aligned body velocity control that actually expects that command to be a scaled-axis representation.

Depends on

• Include Euler angles rate vector to RBS data structure rock-core/base-types#151

How to test

~$ acd control/orogen/auv_control
~$ autoproj test enable .
~$ amake
~$ cd test/
~$ ruby test_world_aligned.rb

@joaobrittoneto @saarnold @bwehbe

[gabrielfpacheco]

ping @joaobrittoneto @doudou @saarnold

[joaobrittoneto]

Looks good.
So one can have a control chain and set a pitch or yaw angle on the position controller, which generates an euler-rate as angular velocity to the velocity controller. And with this PR, the euler-rate is converted to axis-angle velocity and can be compared with the feedback that is in axis-angle as well. Is my understanding correct?

Would be possible to have this conversion on the PIDController, so all angular velocity output are in the same representation?
Would be possible to update the README.md with this new behaviour?

As far as I remember, I'd been using an auv with roll and pitch near zero, so I hadn't paid much attention to it.
Would be nice to see on simulation how this impact the behaviour of an auv for attitude control.

[gabrielfpacheco]

Is my understanding correct?

Yep, your understanding is exactly right.

Would be possible to have this conversion on the PIDController, so all angular velocity output are in the same representation?

Regarding this one, after thinking a bit more, I got that the world-aligned body frame (roll = pitch = 0) angular velocity represented as axis-angle is equivalent to the Euler-rate vector.

Thus, one might say that the conversion performed in the WorldToAligned task is unnecessary. And this would be true if the measured angular velocity state was also represented in the world-aligned body frame. However, IMHO, the problem is actually there, as we can see here no transformation is applied to the received angular velocity (no matter the command frame used) as we do have for other quantities such as position and orientation.

The world-aligned body frame, AFAIK, is something defined in this component for convenience, so one should not expect that the angular velocity is already given in such frame on the received RigidBodyState. However, if we simply apply such a transformation on the PIDController we would have two consequences:

1. As the control would really be done at the world-aligned frame this time, the inverse transform would need to take place at the AlignedToBody task. Today the angular velocity is not altered in AlignedToBody, only the linear part is changed
2. This wouldn't be backward compatible since there might be someone that has actually used the angular velocity property of the RBS directly as the euler-rate vector or the AlignedToBody task expecting that any kind of transformation would be applied to the angular velocity.

In summary, I think the solution currently provided on this PR is the simplest one because it avoids the representation error with minimal change to the code. However, it has the problem of performing the angular velocity control in the body-frame instead of the world-aligned body frame, which is inconsistent IMO.

If you think the other solution I presented above - applying the transformation on the PIDController - would be more adequate (that is my opinion too) I can avoid the backward compatibility issue by creating a property on PIDController that would allow the user to specify if the given angular velocity is represented in the body or world- aligned frame and another one in AlignedToBody to define the same thing. If you are OK with that, I will modify the PR.

Would be possible to update the README.md with this new behaviour?

Sure thing, as soon as we agree on the solution, I will update the documentation.

[saarnold]

If no one objects I'll merge this 👍