icub-models
icub-models copied to clipboard
robotology
Wrong rotation order in /icubGazeboSim/inertial port
On the real icub, data received from the imu sensor is transmitted on the /inertial port in the order: roll pitch yaw. Anyway the values is produced by the sensor using the aeronautic convention pitch roll yaw.
On the simulator, the imu sensor sends values in the same order, roll pitch yaw, but the data is produced using the robotic convention roll pitch yaw.
The result is that the values are the same when the rotation is only on one of the axes, but when the rotation is on 2 or 3 axes the values are different and the correct rotation matrix must be obtained by switching the order of the roll and pitch terms in the matrix product.
The same issue probably applies to the velocity and acceleration vectors.
I'm not sure if this is a bug that should be fixed in the real robot (i.e. use robotics convention in the robot) or in the model.
I guess (unless we at least change the name of the port) we cannot change the /icub/inertial behaviour. Hence we should fix this in the simulator.
I agree with @traversaro . @drdanz thanks for reporting this, which I hope will save the time we lost together in figuring this out :)
@traversaro what about changing the IMU implementation in the gazebo plugin to match the robot one?
The header file of inertial server explains how the data should be broadcasted through the port, so any imu device using it has to comply with that. https://github.com/robotology/yarp/blob/master/src/libYARP_dev/src/modules/ServerInertial/ServerInertial.h @line 50
@barbalberto unfortunately the header file does not specify whether roll pitch and yaw should be generated using the robotics or aeronautical convention.
Ii know there are a lot of standards, the file now says "xyz in global frame", that should have a unique meaning, once you know how your xyz are oriented. If it not clear enough, let's change again the header in such a way there are no more doubt :-D
For the sake of clarity I propose:
- Reference to one existing definition (in the form of a scientific article or a page of citable website/database) of the euler angles that match the one that you use.
- Explain what rotation are these euler angles describing: the rotation that multiplied by a 3d vector expressed in imu frame returns a vector expressed in the "global" frame (in Siciliano and Featherstone notation,
{}^{global} R_{imu}) or the rotation that multiplied by a 3d vector expressed in global frame returns a vector expressed in the "imu" frame (in Siciliano and Featherstone notation,{}^{imu} R_{global}). The only unambiguous definitions of this semantics that I know is discussed in http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6476696 , but I guess it would be to confusing for the average user to refer it. - Provide somewhere a ready to use function (in the form of actual formula used) mapping this definition of the euler angles to the rotation matrix, that once defined what discussed at point 2 is usually less ambiguous, for example linking the documentation of the yarp::math::rpy2dcm function if it uses the same convention used in the imu interface.
Additional points (probably too much work to do, but would be nice):
- Write a test for both the simulated and real robot on the fixed base, checking that by moving the head the rotation between the imu frame provided by the kinematics (for example using iKin or iDynTree) and the imu frame provided by the imu is constant.
The order is given by the gazebo-yarp-imu plugin, so it is not related specifically to models. Moving this issue to icub-models .