Apply gravity external to dartsim for added mass (#462)

When fluid added mass (#384) is used, dartsim will apply gravitational force to the total virtual mass of each link. Since this mass is only to model the inertial aspects of the dynamics, this is incorrect behavior. In this PR, I disable dart's application of gravity to links that have added mass defined in their sdf file. Instead, gravity is applied as an external force right before the world update by multiplying the link mass by the gravity vector (F = ma!) Signed-off-by: Michael Carroll <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
gazebosim · Jan 19, 2023 · 65a0beb · 65a0beb
1 parent 70059ae
commit 65a0beb
Show file tree

Hide file tree

Showing 14 changed files with 796 additions and 60 deletions.
diff --git a/dartsim/src/AddedMassFeatures.cc b/dartsim/src/AddedMassFeatures.cc
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "AddedMassFeatures.hh"
+#include <dart/dynamics/Inertia.hpp>
+
+namespace gz::physics::dartsim
+{
+
+void AddedMassFeatures::SetLinkAddedMass(const Identity &_link,
+    const gz::math::Matrix6d &_addedMass)
+{
+  auto linkInfo = this->ReferenceInterface<LinkInfo>(_link);
+  auto bn = linkInfo->link;
+
+  if (linkInfo->inertial.has_value())
+  {
+    auto &sdfInertia = linkInfo->inertial.value();
+    sdfInertia.SetFluidAddedMass(_addedMass);
+
+    dart::dynamics::Inertia newInertia;
+    newInertia.setMass(sdfInertia.MassMatrix().Mass());
+
+    const Eigen::Matrix3d I_link = math::eigen3::convert(sdfInertia.Moi());
+    newInertia.setMoment(I_link);
+
+    const Eigen::Vector3d localCom =
+        math::eigen3::convert(sdfInertia.Pose().Pos());
+    newInertia.setLocalCOM(localCom);
+
+    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));
+
+      // If using added mass, gravity needs to be applied as a separate
+      // force at the center of mass using F=ma;
+      bn->setGravityMode(false);
+
+      newInertia.setSpatialTensor(
+          newInertia.getSpatialTensor() +
+          math::eigen3::convert(featherstoneMatrix));
+    }
+    bn->setInertia(newInertia);
+  }
+}
+
+gz::math::Matrix6d
+AddedMassFeatures::GetLinkAddedMass(const Identity &_link) const
+{
+  auto linkInfo = this->ReferenceInterface<LinkInfo>(_link);
+
+  if (linkInfo->inertial.has_value() &&
+      linkInfo->inertial->FluidAddedMass().has_value())
+  {
+    return linkInfo->inertial->FluidAddedMass().value();
+  }
+  else
+  {
+    return gz::math::Matrix6d::Zero;
+  }
+}
+
+}  // namespace gz::physics::dartsim
diff --git a/dartsim/src/AddedMassFeatures.hh b/dartsim/src/AddedMassFeatures.hh
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef GZ_PHYSICS_DARTSIM_SRC_ADDEDMASSFEATURES_HH_
+#define GZ_PHYSICS_DARTSIM_SRC_ADDEDMASSFEATURES_HH_
+
+#include <gz/physics/AddedMass.hh>
+
+#include "Base.hh"
+
+namespace gz::physics::dartsim
+{
+
+struct AddedMassFeatureList: FeatureList<
+  AddedMass
+> { };
+
+class AddedMassFeatures :
+    public virtual Base,
+    public virtual Implements3d<AddedMassFeatureList>
+{
+  public: void SetLinkAddedMass(const Identity &_link,
+                                const gz::math::Matrix6d &_addedMass) override;
+
+  public: gz::math::Matrix6d GetLinkAddedMass(
+              const Identity &_link) const override;
+};
+}  // namespace gz::physics::dartsim
+
+#endif  // GZ_PHYSICS_DARTSIM_SRC_ADDEDMASSFEATURES_HH_
diff --git a/dartsim/src/AddedMassFeatures_TEST.cc b/dartsim/src/AddedMassFeatures_TEST.cc
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+#include <dart/dynamics/BodyNode.hpp>
+
+#include <gz/common/Console.hh>
+#include <gz/math/eigen3.hh>
+#include <gz/plugin/Loader.hh>
+
+#include <gz/physics/FrameSemantics.hh>
+#include <gz/physics/GetEntities.hh>
+#include <gz/physics/Joint.hh>
+#include <gz/physics/RequestEngine.hh>
+#include <gz/physics/RevoluteJoint.hh>
+#include <gz/physics/AddedMass.hh>
+
+#include <gz/physics/sdf/ConstructLink.hh>
+#include <gz/physics/sdf/ConstructModel.hh>
+#include <gz/physics/sdf/ConstructWorld.hh>
+#include <gz/physics/dartsim/World.hh>
+
+#include <sdf/Link.hh>
+#include <sdf/Root.hh>
+#include <sdf/World.hh>
+
+#include <test/Utils.hh>
+
+struct TestFeatureList : gz::physics::FeatureList<
+    gz::physics::GetEntities,
+    gz::physics::GetBasicJointState,
+    gz::physics::SetBasicJointState,
+    gz::physics::LinkFrameSemantics,
+    gz::physics::dartsim::RetrieveWorld,
+    gz::physics::sdf::ConstructSdfLink,
+    gz::physics::sdf::ConstructSdfModel,
+    gz::physics::sdf::ConstructSdfWorld,
+    gz::physics::AddedMass
+> { };
+
+using World = gz::physics::World3d<TestFeatureList>;
+using WorldPtr = gz::physics::World3dPtr<TestFeatureList>;
+using ModelPtr = gz::physics::Model3dPtr<TestFeatureList>;
+using LinkPtr = gz::physics::Link3dPtr<TestFeatureList>;
+
+/////////////////////////////////////////////////
+auto LoadEngine()
+{
+  gz::plugin::Loader loader;
+  loader.LoadLib(dartsim_plugin_LIB);
+
+  gz::plugin::PluginPtr dartsim =
+      loader.Instantiate("gz::physics::dartsim::Plugin");
+
+  auto engine =
+      gz::physics::RequestEngine3d<TestFeatureList>::From(dartsim);
+  return engine;
+}
+
+/////////////////////////////////////////////////
+WorldPtr LoadWorld(const std::string &_world)
+{
+  auto engine = LoadEngine();
+  EXPECT_NE(nullptr, engine);
+
+  sdf::Root root;
+  const sdf::Errors &errors = root.Load(_world);
+  EXPECT_EQ(0u, errors.size());
+  for (const auto & error : errors) {
+    std::cout << error << std::endl;
+  }
+
+  EXPECT_EQ(1u, root.WorldCount());
+  const sdf::World *sdfWorld = root.WorldByIndex(0);
+  EXPECT_NE(nullptr, sdfWorld);
+
+  auto world = engine->ConstructWorld(*sdfWorld);
+  EXPECT_NE(nullptr, world);
+
+  return world;
+}
+
+/////////////////////////////////////////////////
+TEST(AddedMassFeatures, 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;
+
+  // 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 expectedZeroSpatialInertia;
+  expectedZeroSpatialInertia <<
+    1, 0, 0, 0, 0, 0,
+    0, 1, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0,
+    0, 0, 0, 1, 0, 0,
+    0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 1;
+
+  auto world = LoadWorld(TEST_WORLD_DIR"/added_mass.sdf");
+  ASSERT_NE(nullptr, world);
+
+  auto dartWorld = world->GetDartsimWorld();
+  ASSERT_NE(nullptr, dartWorld);
+
+  ASSERT_EQ(3u, dartWorld->getNumSkeletons());
+
+  {
+    const auto skeleton = dartWorld->getSkeleton("body_no_added_mass");
+    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(expectedZeroSpatialInertia.isApprox(spatialInertia));
+
+    const auto linkAddedMass =
+      world->GetModel("body_no_added_mass")->GetLink("link")->GetAddedMass();
+    ASSERT_TRUE(Eigen::Matrix6d::Zero().isApprox(
+          gz::math::eigen3::convert(linkAddedMass)));
+  }
+
+  {
+    const auto skeleton = dartWorld->getSkeleton("body_zero_added_mass");
+    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(expectedZeroSpatialInertia.isApprox(spatialInertia));
+
+    auto linkAddedMass =
+      world->GetModel("body_zero_added_mass")->GetLink("link")->GetAddedMass();
+    ASSERT_TRUE(Eigen::Matrix6d::Zero().isApprox(
+          gz::math::eigen3::convert(linkAddedMass)));
+  }
+
+  {
+    const auto skeleton = dartWorld->getSkeleton("body_added_mass");
+    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));
+
+    auto linkAddedMass =
+      world->GetModel("body_added_mass")->GetLink("link")->GetAddedMass();
+    ASSERT_FALSE(Eigen::Matrix6d::Zero().isApprox(
+          gz::math::eigen3::convert(linkAddedMass)));
+  }
+}
diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc
@@ -63,6 +63,7 @@
 #include <sdf/Visual.hh>
 #include <sdf/World.hh>
 
+#include "AddedMassFeatures.hh"
 #include "CustomMeshShape.hh"
 
 namespace gz {
@@ -373,6 +374,7 @@ static ShapeAndTransform ConstructGeometry(
 
 }  // namespace
 
+/////////////////////////////////////////////////
 dart::dynamics::BodyNode *SDFFeatures::FindBodyNode(
     const std::string &_worldName, const std::string &_jointModelName,
     const std::string &_linkRelativeName)
@@ -621,31 +623,6 @@ 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";
@@ -684,6 +661,23 @@ Identity SDFFeatures::ConstructSdfLink(
 
   auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID));
 
+  if (sdfInertia.FluidAddedMass().has_value())
+  {
+    auto* amf = dynamic_cast<AddedMassFeatures*>(this);
+
+    if (nullptr == amf)
+    {
+      gzwarn << "Link [" << _sdfLink.Name() << "] in model ["
+        << modelInfo.model->getName() <<
+        "] has added mass specified in SDF, but AddedMassFeatures" <<
+        "was not available on this engine.  Added mass will not be applied.\n";
+    }
+    else
+    {
+      amf->SetLinkAddedMass(linkIdentity, sdfInertia.FluidAddedMass().value());
+    }
+  }
+
   if (_sdfLink.Name() == modelInfo.canonicalLinkName ||
       (modelInfo.canonicalLinkName.empty() &&
        modelInfo.model->getNumBodyNodes() == 1))

diff --git a/dartsim/src/SDFFeatures.hh b/dartsim/src/SDFFeatures.hh
@@ -18,6 +18,7 @@
 #ifndef GZ_PHYSICS_DARTSIM_SRC_SDFFEATURES_HH_
 #define GZ_PHYSICS_DARTSIM_SRC_SDFFEATURES_HH_
 
+#include <gz/math/Inertial.hh>
 #include <string>
 
 #include <gz/physics/sdf/ConstructCollision.hh>