|
| 1 | +# Gazebo / DART inertia conversion |
| 2 | + |
| 3 | +An explanation of the Gazebo to DART inertial conversion and the different |
| 4 | +conventions used by each system. The conclusion is that there is a missing |
| 5 | +term when populating the DART spatial tensor with added mass because the |
| 6 | +moments of inertia are taken about different axis. |
| 7 | + |
| 8 | +## Drake labelling conventions |
| 9 | + |
| 10 | +- https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html |
| 11 | +- https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__inertia.html |
| 12 | + |
| 13 | +### Frame labels |
| 14 | + |
| 15 | +- `W` - world frame. |
| 16 | +- `B` - body frame. |
| 17 | +- `Bcm` - body CoM frame. |
| 18 | + |
| 19 | +### Point labels |
| 20 | + |
| 21 | +- `Bo` - body origin. |
| 22 | +- `Bcm` - body center of mass. |
| 23 | + |
| 24 | +### Pose |
| 25 | + |
| 26 | +- `X_WB` - the pose of the body frame `B` in the world frame `W`. |
| 27 | +- `X_BBcm` - the pose of the body CoM frame `Bcm` in the body frame `B`. |
| 28 | + |
| 29 | + |
| 30 | +### Inertia matrix |
| 31 | + |
| 32 | +The inertia matrix, taken about a point `P`, expressed in frame `F`. |
| 33 | + |
| 34 | +- `I_BBcm_Bcm` - inertia matrix of body `B` about the body center of mass `Bcm` in frame `Bcm` |
| 35 | +- `I_BBcm_B` - inertia matrix of body `B` about the body center of mass `Bcm` in frame `B` |
| 36 | +- `I_BBo_B` - inertia matrix of body `B` about the body origin `Bo` in frame `B` |
| 37 | + |
| 38 | +### Position vectors |
| 39 | + |
| 40 | +- `c_BoBcm_B` position vector from the body origin `Bo` to the body center of mass `Bcm` in the body frame `B`. |
| 41 | + |
| 42 | + |
| 43 | +## Gazebo inertial object |
| 44 | + |
| 45 | +The SDF documentation http://sdformat.org/spec?ver=1.9&elem=link#inertial_inertia for the `<inertia>` element states: |
| 46 | + |
| 47 | + |
| 48 | +> Description: This link's moments of inertia ixx, iyy, izz and products of inertia ixy, ixz, iyz about Co |
| 49 | +> (the link's center of mass) for the unit vectors Ĉx, Ĉy, Ĉᴢ fixed in the center-of-mass-frame C. |
| 50 | +
|
| 51 | +This means that the object `gz::math::MassMatrix3` contains: |
| 52 | + |
| 53 | +- `m` - the scalar mass. |
| 54 | +- `I_BBcm_Bcm` - inertia matrix of body `B` about the body center of mass `Bcm` in frame `Bcm`. |
| 55 | + |
| 56 | +This is used to construct the object `gz::math::Inertial` which is the inertial taken about point `Bcm` |
| 57 | +for body `B` in frame `F`. Since the pose from SDFFormat is `X_BBcm`, the object stores internally: |
| 58 | + |
| 59 | +- `m` |
| 60 | +- `I_BBcm_Bcm` |
| 61 | +- `X_BBcm` |
| 62 | + |
| 63 | +The functions return the following: |
| 64 | + |
| 65 | +- `MassMatrix -> (m, I_BBcm_Bcm)` |
| 66 | +- `Pose -> (X_BBcm)` |
| 67 | +- `Moi -> (I_BBcm_B = X_BBcm.R * I_BBcm_Bcm * X_BBcm.R^T)` |
| 68 | + |
| 69 | +The key point to note is that when accessing the the moment of inertia it is transformed |
| 70 | +to the body frame `B` but is taken about a point located at the CoM `P = Bcm`. |
| 71 | + |
| 72 | +## DART spatial tensor |
| 73 | + |
| 74 | +The Gazebo function we are interested in is: `gz::physics::dartsim::SDFFeatures::ConstructSdfLink` |
| 75 | +where `gz::math::Inertial` is translated into `dart::dynamics::Inertia`. |
| 76 | + |
| 77 | +### 1. First examine the case when there is no fluid added mass: |
| 78 | + |
| 79 | +```c++ |
| 80 | + const gz::math::Inertiald &sdfInertia = _sdfLink.Inertial(); |
| 81 | + bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); |
| 82 | + |
| 83 | + const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi()); |
| 84 | + |
| 85 | + bodyProperties.mInertia.setMoment(I_link); |
| 86 | + |
| 87 | + const Eigen::Vector3d localCom = |
| 88 | + math::eigen3::convert(sdfInertia.Pose().Pos()); |
| 89 | + |
| 90 | + bodyProperties.mInertia.setLocalCOM(localCom); |
| 91 | +``` |
| 92 | +
|
| 93 | +Break down what is going on: |
| 94 | +
|
| 95 | +- Set `m` - the scalar mass `m`. |
| 96 | +- Set `I_BBcm_B` - the moment of inertia of the body `B`, taken about the body CoM `Bcm` in frame `B`. |
| 97 | +- Set `c_BoBcm_B` - position vector from the body origin `Bo` to the body center of mass `Bcm` in the body frame `B`. |
| 98 | +
|
| 99 | +From these inputs DART, calculates the spatial tensor for rigid body rotations about the **body origin** `Bo`. |
| 100 | +
|
| 101 | +This is the spatial inertial tensor from Roy Featherstone, Rigid Body Dynamics Algorithms, §2.13, p33, Eq(2.63) Springer, 2008. Internally DART computes the spatial tensor to have the following elements: |
| 102 | +
|
| 103 | +- `TL = I_BBo_B = I_BBcm_B + m * cx * cx^T` |
| 104 | +- `TR = m * cx` |
| 105 | +- `BL = m * cx^T` |
| 106 | +- `BR = m * Identity(3)` |
| 107 | +
|
| 108 | +where `cx` is the skew-symmetric operator created from `c_BoBcm_B`. |
| 109 | +
|
| 110 | +Compare this to the definition for the Gazebo body matrix returned by: `gz::math::Inertial::BodyMatrix`: |
| 111 | +
|
| 112 | +- `BR = I_BBcm_B |
| 113 | +- `BL = m * cx` |
| 114 | +- `TR = m * cx^T` |
| 115 | +- `TL = m * Identity(3)` |
| 116 | +
|
| 117 | +which is for rigid body rotations about the **body center of mass** `Bm`. |
| 118 | +
|
| 119 | +Now in this case, the different convention for the axis of rotation does not matter because Gazebo does not use |
| 120 | +the `BodyMatrix` function to set the DART spatial inertial tensor, and the change of axis is calculated internally |
| 121 | +by DART. However this is not the case when added mass is considered. |
| 122 | +
|
| 123 | +### 2. With fluid added mass |
| 124 | +
|
| 125 | +In this case the DART spatial inertial tensor is first calculated as in case 1., then a spatial inertial tensor |
| 126 | +for the fluid added mass is calculated and the sum of the two is set using |
| 127 | +`dart::dynamics::Inertia::setSpatialTensor`: |
| 128 | +
|
| 129 | +```c++ |
| 130 | + bodyProperties.mInertia.setSpatialTensor( |
| 131 | + bodyProperties.mInertia.getSpatialTensor() + |
| 132 | + math::eigen3::convert(featherstoneMatrix) |
| 133 | +``` |
| 134 | + |
| 135 | +So the important note here is that the featherstoneMatrix must be for rotations |
| 136 | +about the body orgin `Bo` and with respect to the body frame `B`. |
| 137 | + |
| 138 | + |
| 139 | +While not clearly documented in the Gazebo class, it is suggested by the function: |
| 140 | + |
| 141 | +```c++ |
| 142 | + /// \brief Spatial mass matrix, which includes the body's inertia, as well |
| 143 | + /// as the inertia of the fluid that is dislocated when the body moves. |
| 144 | + /// The matrix is expressed in the object's frame F, not to be confused |
| 145 | + /// with the center of mass frame Bi. |
| 146 | + /// \return The spatial mass matrix. |
| 147 | + /// \sa BodyMatrix |
| 148 | + /// \sa FluidAddedMass |
| 149 | + public: Matrix6<T> SpatialMatrix() const |
| 150 | + { |
| 151 | + return this->addedMass.has_value() ? |
| 152 | + this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix(); |
| 153 | + } |
| 154 | +``` |
| 155 | +
|
| 156 | +that the Gazebo fluid added mass is expected to be computed in the body frame `B` for rotations about |
| 157 | +the body center of mass `Bcm` (because the quantities are being summed, and this is consistent with |
| 158 | +the other interfaces to this class that return moment of intertia etc.) |
| 159 | +
|
| 160 | +## Conclusion |
| 161 | +
|
| 162 | +The above suggests that there is a term missing in the current calculation of the featherstone matrix |
| 163 | +in `gz::physics::dartsim::SDFFeatures::ConstructSdfLink` to account for the change of axis |
| 164 | +from `Bcm` to `Bo`. |
| 165 | +
|
| 166 | +For fluid added mass the mass matrix is not diagonal with equal entries, so we can't calculate |
| 167 | +`m * cx * cx^T` as for the inertial term. My suggestion is to use: |
| 168 | +
|
| 169 | +``` |
| 170 | +I_BBo_B = I_BBcm_B + cx * M * cx^T |
| 171 | +``` |
| 172 | +
|
| 173 | +and deduce `CMCT = cx * M * cx^T` using: |
| 174 | +
|
| 175 | +```c++ |
| 176 | + auto M = sdfInertia.FluidAddedMass().value().Submatrix( |
| 177 | + math::Matrix6d::TOP_LEFT); |
| 178 | +
|
| 179 | + auto CM = sdfInertia.FluidAddedMass().value().Submatrix( |
| 180 | + math::Matrix6d::BOTTOM_LEFT); |
| 181 | +
|
| 182 | + auto invM = M.Inverse(); |
| 183 | + auto C = CM * invM; |
| 184 | + auto CMCT = CM * C.Transposed(); |
| 185 | +``` |
0 commit comments