Create a "Robot Sensors" library and add a first "IMU" block

[nunoguedelha]

In order to use the simulator in other frameworks, typically within the controller models in whole-body-controllers (refer to whole-body-controllers#121, we create a RobotSensors library and link it to the main library "Matlab Whole-body Simulator" (mwbs_lib.slx) as a sub-library .

[nunoguedelha]

The "RobotSensors" sub-library mwbs_robotSensors_lib.slx is accessible in the Library Browser under the main "Matlab Whole-body Simulator":

Note: For creating a tree of sub-libraries in a library, refer to https://www.mathworks.com/help/simulink/ug/adding-libraries-to-the-library-browser.html.

Simulink IMUsensor block in the "robotSensors" sub-library:

[nunoguedelha]

Computation of the IMU sensor outputs

• The simulator block provides directly the inputs ${}^wH_b, s, \nu$ and $\dot{\nu}$ (w_H_b, s, nu, nuDot).
• We first compute the Jacobian of the IMU frame and the frame pose ${}^wJ_{\text{imu}}, {}^wH_{imu}$, using respectively the blocks "Jacobian" and "Forward Kinematics".

The IMU outputs are computed as follows...

Gyroscope and Accelerometer measurements

The IMU twist velocity and acceleration w.r.t. the world, expressed in the world frame, is given by:

$$\begin{align} \begin{bmatrix} {}^\textsf{w}v _{\textsf{imu},\textsf{w}} \ {}^\textsf{w}\omega _{\textsf{imu},\textsf{w}} \end{bmatrix} &= {}^\textsf{w}J _{\textsf{imu}} \nu \newline \begin{bmatrix} {}^\textsf{w}a _{\textsf{imu},\textsf{w}} \ {}^\textsf{w}\dot{\omega} _{\textsf{imu},\textsf{w}} \end{bmatrix} &= {}^\textsf{w}J _{\textsf{imu}} \dot{\nu} + {}^\textsf{w}\dot{J} _{\textsf{imu}} \nu \end{align}$$

The same quantities expressed in the sensor frame $S$ are given by:

$$\begin{align} \begin{bmatrix} {}^\textsf{s}v _{\textsf{imu},\textsf{w}} \ {}^\textsf{s}\omega _{\textsf{imu},\textsf{w}} \end{bmatrix} &= {}^{\textsf{s}}X _{\textsf{w}} {}^\textsf{w}J _{\textsf{imu}} \nu \newline \begin{bmatrix} {}^\textsf{s}a _{\textsf{imu},\textsf{w}} \ {}^\textsf{s}\dot{\omega} _{\textsf{imu},\textsf{w}} \end{bmatrix} &= {}^{\textsf{s}}X _{\textsf{w}} \left( {}^\textsf{w}J _{\textsf{imu}} \dot{\nu} + {}^\textsf{w}\dot{J} _{\textsf{imu}} \nu \right) \end{align}$$

Only the quantities expressed in the world frame (equations (1) and (2)) have been implemented in #41 .

Orientation measurement

It is the orientation of the sensor frame with respect to the world frame, expressed as Euler angles following the same convention used in the iDynTree library, "Roll, Pitch, Yaw", such that the resulting rotation matrix is:
$$
{}^{\textsf{w}}R _{\textsf{s}} = R _z(\textsf{yaw}) \cdot R _y(\textsf{pitch}) \cdot R _x(\textsf{roll})
$$
Where $\textsf{s}$ is the sensor frame.

In a real IMU, the orientation is estimated from the accelerometer's, gyroscope's and magnetometer's measurements and some sensor fusion algorithm. In the simulatior, we computed the Euler angles directly from the rotation transform ${}^{\textsf{w}}R_{\textsf{s}}$.

We use the method wbs.rollPitchYawFromRotation() from the package wbc. For additional documentation, please refer to:

• http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Rotation.html#a600352007d9250f7f227f21db85611f2
• http://www.geometrictools.com/Documentation/EulerAngles.pdf

=> Implemented in #41 .

[nunoguedelha]

Side note on the selectors feeding the "dotJnu" WBT block

• For both selectors, the input port size = -1 --> inherit size from the driving block.
• The selector extracting the joint velocities is set as follows:

robot_NDOF is set from the "IMUsensor" sub-system mask.

The IMUsensor sub-system mask

• The mask allows to set the frame name of the IMU.
• It has a button that toggles the Config block highlighting, WBT block style.
• The initialization commands (parsing the WBTConfig parameter of the closer ConfigIm block) extract the robot_NDOF parameter.

=> Implemented in #41 .

[nunoguedelha]

CC @traversaro

[nunoguedelha]

As the measured quantity is the proper acceleration, the gravity term was missing from equation (4) in #40 (comment).

$$\begin{equation} \begin{bmatrix} {}^\textsf{s}a _{\textsf{imu},\textsf{w}} \\ {}^\textsf{s}\dot{\omega} _{\textsf{imu},\textsf{w}} \end{bmatrix} = {}^{\textsf{s}}X _{\textsf{w}} \left( {}^\textsf{w}J _{\textsf{imu}} \dot{\nu} + {}^\textsf{w}\dot{J} _{\textsf{imu}} \nu + \begin{bmatrix} 0 & 0 & g & 0 & 0 & 0 \end{bmatrix}^{\top} \right) \end{equation}$$

Thanks @traversaro .

[nunoguedelha]

To be more accurate, and use the notation in traversaro's Thesis in sections 2.4.3 and 4.3.2:

(here, I chose as inertial frame the world frame $W$. $S$ is the sensor frame)

$$ \begin{align} y _{\mathrm{acc}} &= {}^{\textsf{s}}R _{\textsf{w}} {}^{\textsf{w}}\ddot{o} _{S} - {}^{\textsf{s}}R _{\textsf{w}} {}^{\textsf{w}}g \newline &= \left[ 1 _3 \; 0 _{3 \times 3} \right] \alpha _{\textsf{w},\textsf{s}} - {}^{\textsf{s}}R _{\textsf{w}} {}^{\textsf{w}}g \newline \alpha _{\textsf{w},\textsf{s}} &= \begin{bmatrix} {}^{\textsf{s}}R _{\textsf{w}} {}^{\textsf{w}}\ddot{o} _{\textsf{s}} \\\ {}^\textsf{s}\dot{\omega} _{\textsf{w},\textsf{s}} \end{bmatrix} \newline &= \begin{bmatrix} {}^{\textsf{s}}R _{\textsf{w}} & 0 _{3 \times 3} \\\ 0 _{3 \times 3} & {}^{\textsf{s}}R _{\textsf{w}} \end{bmatrix} \begin{bmatrix} {}^{\textsf{w}}\ddot{o} _{\textsf{s}} \\\ {}^\textsf{w}\dot{\omega} _{\textsf{w},\textsf{s}} \end{bmatrix} \newline &= {}^{\textsf{s}}X _{\textsf{s}[\textsf{w}]} {}^{\textsf{s}[\textsf{w}]}\dot{\mathbf{v}} _{\textsf{w},\textsf{s}} \end{align} $$

and ...

$$ \begin{align} y _{\mathrm{gyr}} &= {}^{\textsf{s}}\omega _{\textsf{w}, \textsf{s}} \newline &= \left[ 0 _{3 \times 3} \; 1 _{3} \right] {}^{\textsf{s}}\mathrm{v} _{\textsf{w}, \textsf{s}} \newline {}^{\textsf{s}}\mathrm{v} _{\textsf{w}} &= {}^{\textsf{s}}X _{\textsf{s}[\textsf{w}]} {}^{\textsf{s}[\textsf{w}]}\mathbf{v} _{\textsf{w},\textsf{s}} \end{align} $$

[nunoguedelha]

... we can then compute the velocity ${}^{\textsf{s}[\textsf{w}]}\mathbf{v} _{\textsf{w},\textsf{s}}$ from the Jacobian using one of the three frame velocity representations...

• Inertial fixed:

$$ \begin{equation} {}^{S[W]}\mathrm{v} _{W,S} = {}^{S[W]}X _{W} {}^{W}J _S \nu \end{equation} $$

• Body fixed:

$$ \begin{equation} {}^{S[W]}\mathrm{v} _{W,S} = {}^{S[W]}X _{S} {}^{S}J _S \nu \end{equation} $$

where ${}^{S}J_S$ is the Jacobian of body frame $S$ expressed in body coordinates as described in Featherstone RBDA textbook section 2.7 and 4.2.

• Mixed (body frame origin, inertial orientation):

$$ \begin{equation} {}^{S[W]}\mathrm{v} _{W,S} = {}^{S[W]}J _S \nu \end{equation} $$

We then get for the acceleration:

$$ \begin{align} \alpha _{W,S} &= {}^{S}X _{S[W]} \frac{d}{dt} \left( {}^{S[W]}\mathrm{v} _{W,S} \right) \newline &= {}^{S}X _{S[W]} \left( {}^{S[W]}J _{S} \dot{\nu} + {}^{S[W]}\dot{J} _{S} \nu \right) \newline y _{\mathrm{acc}} &= {}^{S}R _{W} \left( \left[\begin{array}{ll} 1 _{3} & 0 _{3 \times 3} \end{array}\right] \left( {}^{S[W]}J _{S} \dot{\nu} + {}^{S[W]}\dot{J} _{S} \nu \right) - {}^{W}g \right) \end{align} $$

... where $g$ is the gravity vector. We get for the angular velocity:

$$ \begin{equation} y _{\mathrm{gyr}} = {}^{S}R _{W} \left[\begin{array}{ll} 0 _{3 \times 3} & 1 _{3} \end{array}\right] {}^{S[W]}J _{S} \nu \end{equation} $$

we can compute the Jacobian ${}^{S[W]}J _{S}$ from the Jacobian WBT block.

(Refer to discussion comments #41 (comment) to #41 (comment) for further details)