Add Gravity Feature, fix LinkFeatures_TEST

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,
@@ -79,16 +81,17 @@ TestWorldPtr LoadWorld(
 {
   sdf::Root root;
   const sdf::Errors errors = root.Load(_sdfFile);
-  EXPECT_TRUE(errors.empty());
+  EXPECT_TRUE(errors.empty()) << errors;
   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);
+  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);
+  return world;
 }
 
 // A predicate-formatter for asserting that two vectors are approximately equal.
@@ -119,6 +122,12 @@ 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(physics::FrameID::World()));
+  }
+
   // Add a sphere
   sdf::Model modelSDF;
   modelSDF.SetName("sphere");

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
 {

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)

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;

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,81 @@ 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 RelativeGravityType = RelativeQuantity<
+            LinearVectorType, PolicyT::Dim,
+            detail::VectorSpace<typename PolicyT::Scalar, PolicyT::Dim>>;
+
+        /// \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 RelativeGravityType &_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 as a Relative Gravity
+        /// (a quantity that contains information about the coordinates
+        /// in which it is expressed).
+        /// \return Relative Gravity vector.
+        public: RelativeGravityType GetGravity() const;
+
+        /// \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 Name 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 Name of gravity.
+        public: virtual LinearVectorType GetWorldGravity(
+            const Identity &_id) const = 0;
+      };
+    };
+
     /////////////////////////////////////////////////
     class IGNITION_PHYSICS_VISIBLE Solver : public virtual Feature
     {
@@ -83,7 +159,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;
 

include/ignition/physics/detail/World.hh

@@ -44,6 +44,72 @@ const std::string &CollisionDetector::World<PolicyT, FeaturesT>::
       ->GetWorldCollisionDetector(this->identity);
 }
 
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+void Gravity::World<PolicyT, FeaturesT>::SetGravity(
+    const RelativeGravityType &_gravity)
+{
+  // Resolve to world coordinates
+  auto &impl = *this->template Interface<FrameSemantics>();
+  auto gravityInWorld =
+      detail::Resolve(impl, _gravity, FrameID::World(), FrameID::World());
+
+  this->template Interface<Gravity>()
+      ->SetWorldGravity(this->identity, gravityInWorld);
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+void Gravity::World<PolicyT, FeaturesT>::SetGravity(
+    const LinearVectorType &_gravity,
+    const FrameID &_inCoordinatesOf)
+{
+  // Call SetWorldGravity directly if using world coordinates
+  if (_inCoordinatesOf == FrameID::World())
+  {
+    this->template Interface<Gravity>()
+        ->SetWorldGravity(this->identity, _gravity);
+  }
+  // Otherwise make a RelativeGravity object and call the other API
+  else
+  {
+    RelativeGravityType gravityInRef(_inCoordinatesOf, _gravity);
+    this->SetGravity(gravityInRef);
+  }
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+auto Gravity::World<PolicyT, FeaturesT>::GetGravity() const
+    -> RelativeGravityType
+{
+  return RelativeGravityType(
+      FrameID::World(),
+      this->template Interface<Gravity>()
+          ->GetWorldGravity(this->identity));
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+auto Gravity::World<PolicyT, FeaturesT>::GetGravity(
+    const FrameID &_inCoordinatesOf) const
+    -> LinearVectorType
+{
+  // Return quickly if using world coordinates
+  auto gravityInWorld = this->template Interface<Gravity>()
+                            ->GetWorldGravity(this->identity);
+  if (_inCoordinatesOf == FrameID::World())
+  {
+    return gravityInWorld;
+  }
+
+  // Otherwise resolve to proper frame
+  auto &impl = *this->template Interface<FrameSemantics>();
+  RelativeGravityType gravityInRef(FrameID::World(), gravityInWorld);
+  return detail::Resolve(impl, gravityInRef, FrameID::World(),
+                         _inCoordinatesOf);
+}
+
 /////////////////////////////////////////////////
 template <typename PolicyT, typename FeaturesT>
 void Solver::World<PolicyT, FeaturesT>::SetSolver(