Support fluid added mass (#384)

Signed-off-by: Louise Poubel <[email protected]> Signed-off-by: Joan Aguilar Mayans <[email protected]> Co-authored-by: Joan Aguilar Mayans <[email protected]>
gazebosim · Nov 28, 2022 · bab13ef · bab13ef
1 parent 48acd7a
commit bab13ef
Show file tree

Hide file tree

Showing 3 changed files with 102 additions and 0 deletions.
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -615,6 +615,32 @@ Identity SDFFeatures::ConstructSdfLink(
 
   bodyProperties.mInertia.setLocalCOM(localCom);
 
+  // If added mass is provided, add that to DART's computed spatial tensor
+  // TODO(chapulina) Put in another feature
+  if (sdfInertia.FluidAddedMass().has_value())
+  {
+    // Note that the ordering of the spatial inertia matrix used in DART is
+    // different than the one used in Gazebo and SDF.
+    math::Matrix6d featherstoneMatrix;
+    featherstoneMatrix.SetSubmatrix(math::Matrix6d::TOP_LEFT,
+        sdfInertia.FluidAddedMass().value().Submatrix(
+        math::Matrix6d::BOTTOM_RIGHT));
+    featherstoneMatrix.SetSubmatrix(math::Matrix6d::TOP_RIGHT,
+        sdfInertia.FluidAddedMass().value().Submatrix(
+        math::Matrix6d::BOTTOM_LEFT));
+    featherstoneMatrix.SetSubmatrix(math::Matrix6d::BOTTOM_LEFT,
+        sdfInertia.FluidAddedMass().value().Submatrix(
+        math::Matrix6d::TOP_RIGHT));
+    featherstoneMatrix.SetSubmatrix(math::Matrix6d::BOTTOM_RIGHT,
+        sdfInertia.FluidAddedMass().value().Submatrix(
+        math::Matrix6d::TOP_LEFT));
+
+    bodyProperties.mInertia.setSpatialTensor(
+        bodyProperties.mInertia.getSpatialTensor() +
+        math::eigen3::convert(featherstoneMatrix)
+    );
+  }
+
   dart::dynamics::FreeJoint::Properties jointProperties;
   jointProperties.mName = bodyProperties.mName + "_FreeJoint";
   // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any

diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc
@@ -935,3 +935,36 @@ TEST_P(SDFFeatures_TEST, Shapes)
     ASSERT_EQ(1u, skeleton->getNumBodyNodes());
   }
 }
+
+/////////////////////////////////////////////////
+TEST_P(SDFFeatures_TEST, AddedMass)
+{
+  // Expected spatial inertia matrix. This includes inertia due to the body's
+  // mass and added mass. Note that the ordering of the matrix is different
+  // than the one used in SDF.
+  Eigen::Matrix6d expectedSpatialInertia;
+  expectedSpatialInertia <<
+    17, 17, 18, 4, 9, 13,
+    17, 20, 20, 5, 10, 14,
+    18, 20, 22, 6, 11, 15,
+    4, 5, 6, 2, 2, 3,
+    9, 10, 11, 2, 8, 8,
+    13, 14, 15, 3, 8, 13;
+
+  auto world = this->LoadWorld(TEST_WORLD_DIR"/added_mass.sdf");
+  ASSERT_NE(nullptr, world);
+
+  auto dartWorld = world->GetDartsimWorld();
+  ASSERT_NE(nullptr, dartWorld);
+
+  ASSERT_EQ(1u, dartWorld->getNumSkeletons());
+  const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton("body");
+  ASSERT_NE(skeleton, nullptr);
+
+  ASSERT_EQ(1u, skeleton->getNumBodyNodes());
+  const dart::dynamics::BodyNode *link = skeleton->getBodyNode("link");
+  ASSERT_NE(link, nullptr);
+
+  const Eigen::Matrix6d spatialInertia = link->getSpatialInertia();
+  ASSERT_TRUE(expectedSpatialInertia.isApprox(spatialInertia));
+}
diff --git a/dartsim/worlds/added_mass.sdf b/dartsim/worlds/added_mass.sdf
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="added_mass">
+    <model name="body">
+      <link name="link">
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <fluid_added_mass>
+            <xx>1</xx>
+            <xy>2</xy>
+            <xz>3</xz>
+            <xp>4</xp>
+            <xq>5</xq>
+            <xr>6</xr>
+            <yy>7</yy>
+            <yz>8</yz>
+            <yp>9</yp>
+            <yq>10</yq>
+            <yr>11</yr>
+            <zz>12</zz>
+            <zp>13</zp>
+            <zq>14</zq>
+            <zr>15</zr>
+            <pp>16</pp>
+            <pq>17</pq>
+            <pr>18</pr>
+            <qq>19</qq>
+            <qr>20</qr>
+            <rr>21</rr>
+          </fluid_added_mass>
+        </inertial>
+      </link>
+    </model>
+  </world>
+</sdf>