search

rock-core / base-types

C/C++ and Ruby definition of the base types in Rock
6 stars 40 forks source link

Include Euler angles rate vector to RBS data structure #151

Closed gabrielfpacheco closed 4 years ago

gabrielfpacheco commented 4 years ago

This PR includes methods that provides conversions between angular velocity (axis-angle representation) and the corresponding ZYX Euler angles rate (commit https://github.com/rock-core/base-types/commit/d4c075e16d70b295c3c9ae7cf37c38cfa813f215) and also takes the opportunity to perform some style changes (commit https://github.com/rock-core/base-types/commit/6e6ef43df0324e513ce753ef247d34f95cc17320) based on rock.vscode linter suggestions.

The Euler angles rate can be very useful in some use-cases such as for trajectory planning and control and I thought it would be a good idea to centralize this functionality directly on RBS since the Euler angles (ZYX yaw-pitch-roll convention) are well defined there.

Reference: https://opensource.docs.anymal.com/doxygen/kindr/master/cheatsheet_latest.pdf

gabrielfpacheco commented 4 years ago

This looks good to me. It's only missing unit tests.

Thanks, @doudou. I have included the unit tests in https://github.com/rock-core/base-types/pull/151/commits/6023b8ec108657a002ce74c80d138483875cd58b

doudou commented 4 years ago

Wait a minute ...

The scaled-axis vector is a Euler angle decomposition, since velocity composition is summation in this case

velocity = velocity_X + velocity_Y + velocity_Z

There is no "order of decomposition". Dealing with angular velocities, decomposition is commutative.

gabrielfpacheco commented 4 years ago

So you are saying that the angular_velocity of the RigidBodyState is already expected to be the time derivative of the Euler-angles? From its documentation, it does not seem that to me: "Angular Velocity of sourceFrame relative to targetFrame, expressed in sourceFrame, as an axis-angle representation".

I understand that the decomposition of angular velocity itself is commutative. However, what I meant is that the way it is mapped on the Euler angles time derivative vector depends on the chosen Euler angles decomposition. The "order of decomposition" was referring only to the fact that RBS uses an ZYX (yaw-pitch-roll) representation, and that the new methods consider this representation to provide the Euler rate vector. But if anything was not clear, I could try to improve the way it is written.

image

doudou commented 4 years ago

I haven't seen the rest of the text, but given what I see my guess is that the angular-rate velocity vector is expressed in the moving object's own frame. The math you show transforms this angular-rate vector to express it in the reference frame. The integral this angular-rate vector is meaningless in this case as its reference frame changes over time.

You can see that the transform matrix is basically only dependent on the orientation angles. The math is essentially transforming the body-fixed angular-rate vector expressed in the object's own frame into another frame (which, even more, supports the case that the angular-rate vector itself is just fine).

As specified in the RBS description, the angular velocity is the velocity of sourceFrame (the body) in targetFrame (the reference) expressed in targetFrame. A.k.a. this does not apply.

doudou commented 4 years ago

And, if your angular velocity is expressed in the body frame, just apply orientation (or its inverse, too lazy to check) to get the one in the reference frame.

gabrielfpacheco commented 4 years ago

I apologize if I am not making myself very clear. The whole point of creating such a way of getting the Euler rate vector from the angular_velocity of the RBS is to be completely analogous with getting the Euler angles from its orientation.

As specified in the RBS description, the angular velocity is the velocity of sourceFrame (the body) in targetFrame (the reference) expressed in targetFrame. A.k.a. this does not apply.

Actually the documentation says that the angular velocity is expressed in the sourceFrame (the body) as seen here.

I haven't seen the rest of the text, but given what I see my guess is that the angular-rate velocity vector is expressed in the moving object's own frame.

To my knowledge, the Euler angles rate vector will always be expressed the same way as the Euler angles vector is. In other words, a set of 3 predefined rotations to align a frame to another. This convention has chosen to be the Tait-Bryant angles (ZYX) for the RigidBodyState and Pose data types.

The angular_velocity could be expressed either on the body or the reference frame, the only difference being the transformation (mapping) from the corresponding angular_velocity frame expression to the euler_rate vector (time derivative vector of the Euler angles).

Note that this transformation is not a simply chain of rotations of the same vector from a frame to another, it is actually a composition of rotations of different vectors, each one representing the rate of change in roll, pitch and yaw. If you take a look at the final transformation matrix it does not belong to SO3.

You can see that the transform matrix is basically only dependent on the orientation angles. The math is essentially transforming the body-fixed angular-rate vector expressed in the object's own frame into another frame (which, even more, supports the case that the angular-rate vector itself is just fine).

I guess we could see the euler_rate vector as the angular_velocity in the so-called world-aligned to body frame (basically the body frame but with roll = pitch = 0). However, as I mentioned before, this transformation is not as straightforward as a rotation using the orientation. It is a mapping that depends on the chosen representation of the orientation (something similar to the Analytic Jacobian). Wikipedia also shows it considering a Z-X-Z convention

And, if your angular velocity is expressed in the body frame, just apply orientation (or its inverse, too lazy to check) to get the one in the reference frame.

Actually, as per the documentation, if sourceFrame is "body" and targetFrame is "world", the angular velocity would be the angular velocity of the body relative to the world, expressed in the body frame. That was the reason I've chosen the mappings that would actually use/output the angular velocity expressed in the sourceFrame (the body).

Long story short, I am not trying to prove a point or anything, I just encountered, twice, the need of working with the Euler angles rate instead of the actual angular velocity and realized that this transformation was necessary. I was firstly doing the mapping on my side only, but figured it could be useful to have it on the RBS for others to leverage it, the same way we - more often - leverage rbs.getRoll(), rbs.getPitch() and rbs.getYaw(). I have also noticed that other kinematics robot libraries such as https://github.com/ANYbotics/kindr also considered such transfoms. They are actually the authors of this document where the relationships are expressed.

doudou commented 4 years ago

Long story short, I am not trying to prove a point or anything, I just encountered, twice, the need of working with the Euler angles rate instead of the actual angular velocity and realized that this transformation was necessary.

I don't think you are. This is usually hard stuff to get right, and these discussions help feed the understanding.

Actually the documentation says that the angular velocity is expressed in the sourceFrame (the body) as seen here.

Indeed. My bad. You are therefore absolutely right on all counts ;-)

doudou commented 4 years ago

Thanks for your patience, @gabrielfpacheco.

gabrielfpacheco commented 4 years ago

Hey @doudou, no worries! I really like this kind of discussions, I think we all learn a lot and benefit from them. I'm happy to, somehow, contribute :)