Add Gravity Feature, fix LinkFeatures_TEST (#275)

* Add Feature to Get/Set Gravity vector in a World * dartsim: implement Gravity Feature * Test Gravity APIs in WorldFeatures_TEST * FrameSemantics: improve handling of World frame Avoid calling FrameDataRelativeToWorld for quantities relative to the world frame but expressed in different coordinates. This prevents an assertion in KinematicsFeatures.cc * Fix LinkFeatures_TEST by using Gravity Feature * ShapeFeatures_TEST: remove unused LoadWorld function * Fix doxygen parameter name in World.hh Signed-off-by: Steven Peters <[email protected]>
gazebosim · Jul 16, 2021 · 164f088 · 164f088
1 parent 5b5207d
commit 164f088
Show file tree

Hide file tree

Showing 8 changed files with 287 additions and 41 deletions.
diff --git a/dartsim/src/LinkFeatures_TEST.cc b/dartsim/src/LinkFeatures_TEST.cc
@@ -30,6 +30,7 @@
 #include <ignition/physics/FrameSemantics.hh>
 #include <ignition/physics/GetBoundingBox.hh>
 #include <ignition/physics/Link.hh>
+#include <ignition/physics/World.hh>
 #include <ignition/physics/sdf/ConstructWorld.hh>
 #include <ignition/physics/sdf/ConstructModel.hh>
 #include <ignition/physics/sdf/ConstructLink.hh>
@@ -42,6 +43,7 @@
 struct TestFeatureList : ignition::physics::FeatureList<
     ignition::physics::AddLinkExternalForceTorque,
     ignition::physics::ForwardStep,
+    ignition::physics::Gravity,
     ignition::physics::sdf::ConstructSdfWorld,
     ignition::physics::sdf::ConstructSdfModel,
     ignition::physics::sdf::ConstructSdfLink,
@@ -72,25 +74,6 @@ class LinkFeaturesFixture : public ::testing::Test
   protected: TestEnginePtr engine;
 };
 
-TestWorldPtr LoadWorld(
-    const TestEnginePtr &_engine,
-    const std::string &_sdfFile,
-    const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8})
-{
-  sdf::Root root;
-  const sdf::Errors errors = root.Load(_sdfFile);
-  EXPECT_TRUE(errors.empty());
-  const sdf::World *sdfWorld = root.WorldByIndex(0);
-  // Make a copy of the world so we can set the gravity property
-  // TODO(addisu) Add a world property feature to set gravity instead of this
-  // hack
-  sdf::World worldCopy;
-  worldCopy.Load(sdfWorld->Element());
-
-  worldCopy.SetGravity(math::eigen3::convert(_gravity));
-  return _engine->ConstructWorld(worldCopy);
-}
-
 // A predicate-formatter for asserting that two vectors are approximately equal.
 class AssertVectorApprox
 {
@@ -114,11 +97,42 @@ class AssertVectorApprox
   private: double tol;
 };
 
+TestWorldPtr LoadWorld(
+    const TestEnginePtr &_engine,
+    const std::string &_sdfFile,
+    const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8})
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(_sdfFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+  const sdf::World *sdfWorld = root.WorldByIndex(0);
+  EXPECT_NE(nullptr, sdfWorld);
+
+  auto graphErrors = sdfWorld->ValidateGraphs();
+  EXPECT_EQ(0u, graphErrors.size()) << graphErrors;
+
+  TestWorldPtr world = _engine->ConstructWorld(*sdfWorld);
+  EXPECT_NE(nullptr, world);
+  world->SetGravity(_gravity);
+
+  AssertVectorApprox vectorPredicate(1e-10);
+  EXPECT_PRED_FORMAT2(vectorPredicate, _gravity,
+                      world->GetGravity());
+
+  return world;
+}
+
 // Test setting force and torque.
 TEST_F(LinkFeaturesFixture, LinkForceTorque)
 {
   auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf",
                          Eigen::Vector3d::Zero());
+  {
+    AssertVectorApprox vectorPredicate(1e-10);
+    EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(),
+                        world->GetGravity());
+  }
+
   // Add a sphere
   sdf::Model modelSDF;
   modelSDF.SetName("sphere");

diff --git a/dartsim/src/ShapeFeatures_TEST.cc b/dartsim/src/ShapeFeatures_TEST.cc
@@ -98,25 +98,6 @@ class ShapeFeaturesFixture : public ::testing::Test
   protected: TestEnginePtr engine;
 };
 
-TestWorldPtr LoadWorld(
-    const TestEnginePtr &_engine,
-    const std::string &_sdfFile,
-    const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8})
-{
-  sdf::Root root;
-  const sdf::Errors errors = root.Load(_sdfFile);
-  EXPECT_TRUE(errors.empty());
-  const sdf::World *sdfWorld = root.WorldByIndex(0);
-  // Make a copy of the world so we can set the gravity property
-  // TODO(addisu) Add a world property feature to set gravity instead of this
-  // hack
-  sdf::World worldCopy;
-  worldCopy.Load(sdfWorld->Element());
-
-  worldCopy.SetGravity(math::eigen3::convert(_gravity));
-  return _engine->ConstructWorld(worldCopy);
-}
-
 // A predicate-formatter for asserting that two vectors are approximately equal.
 class AssertVectorApprox
 {

diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc
@@ -80,6 +80,22 @@ const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id)
   return world->getConstraintSolver()->getCollisionDetector()->getType();
 }
 
+/////////////////////////////////////////////////
+void WorldFeatures::SetWorldGravity(
+    const Identity &_id, const LinearVectorType &_gravity)
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+  world->setGravity(_gravity);
+}
+
+/////////////////////////////////////////////////
+WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity(
+    const Identity &_id) const
+{
+  auto world = this->ReferenceInterface<dart::simulation::World>(_id);
+  return world->getGravity();
+}
+
 /////////////////////////////////////////////////
 void WorldFeatures::SetWorldSolver(const Identity &_id,
     const std::string &_solver)

diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh
@@ -30,6 +30,7 @@ namespace dartsim {
 
 struct WorldFeatureList : FeatureList<
   CollisionDetector,
+  Gravity,
   Solver
 > { };
 
@@ -45,6 +46,13 @@ class WorldFeatures :
   public: const std::string &GetWorldCollisionDetector(const Identity &_id)
       const override;
 
+  // Documentation inherited
+  public: void SetWorldGravity(
+      const Identity &_id, const LinearVectorType &_gravity) override;
+
+  // Documentation inherited
+  public: LinearVectorType GetWorldGravity(const Identity &_id) const override;
+
   // Documentation inherited
   public: void SetWorldSolver(const Identity &_id, const std::string &_solver)
       override;

diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc
@@ -35,6 +35,8 @@
 
 struct TestFeatureList : ignition::physics::FeatureList<
     ignition::physics::CollisionDetector,
+    ignition::physics::Gravity,
+    ignition::physics::LinkFrameSemantics,
     ignition::physics::Solver,
     ignition::physics::ForwardStep,
     ignition::physics::sdf::ConstructSdfWorld,
@@ -46,6 +48,29 @@ using namespace ignition;
 using TestEnginePtr = physics::Engine3dPtr<TestFeatureList>;
 using TestWorldPtr = physics::World3dPtr<TestFeatureList>;
 
+// A predicate-formatter for asserting that two vectors are approximately equal.
+class AssertVectorApprox
+{
+  public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol)
+  {
+  }
+
+  public: ::testing::AssertionResult operator()(
+              const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m,
+              Eigen::Vector3d _n)
+  {
+    if (ignition::physics::test::Equal(_m, _n, this->tol))
+      return ::testing::AssertionSuccess();
+
+    return ::testing::AssertionFailure()
+           << _mExpr << " and " << _nExpr << " ([" << _m.transpose()
+           << "] and [" << _n.transpose() << "]"
+           << ") are not equal";
+  }
+
+  private: double tol;
+};
+
 //////////////////////////////////////////////////
 class WorldFeaturesFixture : public ::testing::Test
 {
@@ -98,6 +123,76 @@ TEST_F(WorldFeaturesFixture, CollisionDetector)
   EXPECT_EQ("dart", world->GetCollisionDetector());
 }
 
+//////////////////////////////////////////////////
+TEST_F(WorldFeaturesFixture, Gravity)
+{
+  auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/falling.world");
+  ASSERT_NE(nullptr, world);
+
+  // Check default gravity value
+  AssertVectorApprox vectorPredicate10(1e-10);
+  EXPECT_PRED_FORMAT2(vectorPredicate10, Eigen::Vector3d(0, 0, -9.8),
+                      world->GetGravity());
+
+  auto model = world->GetModel("sphere");
+  ASSERT_NE(nullptr, model);
+
+  auto link = model->GetLink(0);
+  ASSERT_NE(nullptr, link);
+
+  // initial link pose
+  const Eigen::Vector3d initialLinkPosition(0, 0, 2);
+  {
+    auto pos = link->FrameDataRelativeToWorld().pose.translation();
+    EXPECT_PRED_FORMAT2(vectorPredicate10,
+                        initialLinkPosition,
+                        pos);
+  }
+
+  auto linkFrameID = link->GetFrameID();
+
+  // Get default gravity in link frame, which is pitched by pi/4
+  EXPECT_PRED_FORMAT2(vectorPredicate10,
+                      Eigen::Vector3d(6.92964645563, 0, -6.92964645563),
+                      world->GetGravity(linkFrameID));
+
+  // set gravity along X axis of linked frame, which is pitched by pi/4
+  world->SetGravity(Eigen::Vector3d(1.4142135624, 0, 0), linkFrameID);
+
+  EXPECT_PRED_FORMAT2(vectorPredicate10,
+                      Eigen::Vector3d(1, 0, -1),
+                      world->GetGravity());
+
+  // test other SetGravity API
+  // set gravity along Z axis of linked frame, which is pitched by pi/4
+  physics::RelativeForce3d relativeGravity(
+      linkFrameID, Eigen::Vector3d(0, 0, 1.4142135624));
+  world->SetGravity(relativeGravity);
+
+  EXPECT_PRED_FORMAT2(vectorPredicate10,
+                      Eigen::Vector3d(1, 0, 1),
+                      world->GetGravity());
+
+  // Confirm that changed gravity direction affects pose of link
+  ignition::physics::ForwardStep::Input input;
+  ignition::physics::ForwardStep::State state;
+  ignition::physics::ForwardStep::Output output;
+
+  const size_t numSteps = 1000;
+  for (size_t i = 0; i < numSteps; ++i)
+  {
+    world->Step(output, state, input);
+  }
+
+  AssertVectorApprox vectorPredicate3(1e-3);
+  {
+    auto pos = link->FrameDataRelativeToWorld().pose.translation();
+    EXPECT_PRED_FORMAT2(vectorPredicate3,
+                        Eigen::Vector3d(0.5, 0, 2.5),
+                        pos);
+  }
+}
+
 //////////////////////////////////////////////////
 TEST_F(WorldFeaturesFixture, Solver)
 {

diff --git a/include/ignition/physics/World.hh b/include/ignition/physics/World.hh
@@ -21,6 +21,7 @@
 #include <string>
 
 #include <ignition/physics/FeatureList.hh>
+#include <ignition/physics/FrameSemantics.hh>
 
 namespace ignition
 {
@@ -61,6 +62,74 @@ namespace ignition
       };
     };
 
+    /////////////////////////////////////////////////
+    using GravityRequiredFeatures = FeatureList<FrameSemantics>;
+
+    /////////////////////////////////////////////////
+    /// \brief Get and set the World's gravity vector in a specified frame.
+    class IGNITION_PHYSICS_VISIBLE Gravity
+        : public virtual
+          FeatureWithRequirements<GravityRequiredFeatures>
+    {
+      /// \brief The World API for getting and setting the gravity vector.
+      public: template <typename PolicyT, typename FeaturesT>
+      class World : public virtual Feature::World<PolicyT, FeaturesT>
+      {
+        public: using LinearVectorType =
+            typename FromPolicy<PolicyT>::template Use<LinearVector>;
+
+        public: using RelativeForceType =
+            typename FromPolicy<PolicyT>::template Use<RelativeForce>;
+
+        /// \brief Set the World gravity vector.
+        /// \param[in] _gravity The desired gravity as a Relative Gravity
+        /// (a quantity that contains information about the coordinates
+        /// in which it is expressed).
+        public: void SetGravity(const RelativeForceType &_gravity);
+
+        /// \brief Set the World gravity vector. Optionally, you may specify
+        /// the frame whose coordinates are used to express the gravity vector.
+        /// The World frame is used as a default if no frame is specified.
+        /// \param[in] _gravity Gravity vector.
+        /// \param[in] _inCoordinatesOf Frame whose coordinates are used
+        /// to express _gravity.
+        public: void SetGravity(
+            const LinearVectorType &_gravity,
+            const FrameID &_forceInCoordinatesOf = FrameID::World());
+
+        /// \brief Get the World gravity vector. Optionally, you may specify
+        /// the frame whose coordinates are used to express the gravity vector.
+        /// The World frame is used as a default if no frame is specified.
+        /// \param[in] _inCoordinatesOf Frame whose coordinates are used
+        /// to express _gravity.
+        /// \return Gravity vector in corrdinates of _inCoordinatesOf.
+        public: LinearVectorType GetGravity(
+            const FrameID &_forceInCoordinatesOf = FrameID::World()) const;
+      };
+
+      /// \private The implementation API for the gravity.
+      public: template <typename PolicyT>
+      class Implementation : public virtual Feature::Implementation<PolicyT>
+      {
+        public: using LinearVectorType =
+            typename FromPolicy<PolicyT>::template Use<LinearVector>;
+
+        /// \brief Implementation API for setting the gravity vector, which is
+        /// expressed in the World frame..
+        /// \param[in] _id Identity of the world.
+        /// \param[in] _gravity Value of gravity.
+        public: virtual void SetWorldGravity(
+            const Identity &_id, const LinearVectorType &_gravity) = 0;
+
+        /// \brief Implementation API for getting the gravity expressed in the
+        /// world frame.
+        /// \param[in] _id Identity of the world.
+        /// \return Value of gravity.
+        public: virtual LinearVectorType GetWorldGravity(
+            const Identity &_id) const = 0;
+      };
+    };
+
     /////////////////////////////////////////////////
     class IGNITION_PHYSICS_VISIBLE Solver : public virtual Feature
     {
@@ -83,7 +152,7 @@ namespace ignition
       {
         /// \brief Implementation API for setting the solver.
         /// \param[in] _id Identity of the world.
-        /// \param[in] _collisionDetector Name of solver.
+        /// \param[in] _solver Name of solver.
         public: virtual void SetWorldSolver(
             const Identity &_id, const std::string &_solver) = 0;
 

diff --git a/include/ignition/physics/detail/FrameSemantics.hh b/include/ignition/physics/detail/FrameSemantics.hh
@@ -57,8 +57,16 @@ namespace ignition
           }
 
           q = _quantity.RelativeToParent();
-          currentCoordinates = _impl.FrameDataRelativeToWorld(
-                _relativeTo).pose.linear();
+          if (_relativeTo.IsWorld())
+          {
+            // The World Frame has all zero fields
+            currentCoordinates = RotationType::Identity();
+          }
+          else
+          {
+            currentCoordinates = _impl.FrameDataRelativeToWorld(
+                  _relativeTo).pose.linear();
+          }
         }
         else
         {