diff --git a/dartsim/src/LinkFeatures_TEST.cc b/dartsim/src/LinkFeatures_TEST.cc index 8d654d7a7..31df9a83d 100644 --- a/dartsim/src/LinkFeatures_TEST.cc +++ b/dartsim/src/LinkFeatures_TEST.cc @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -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 index c488850c3..649cb0b47 100644 --- 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 index 812f215df..a918cc2bc 100644 --- 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(_id); + world->setGravity(_gravity); +} + +///////////////////////////////////////////////// +WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( + const Identity &_id) const +{ + auto world = this->ReferenceInterface(_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 index 76d4d1b4b..692969174 100644 --- 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 index 0de31feaa..b6b510473 100644 --- 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; using TestWorldPtr = physics::World3dPtr; +// 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 index 0cc2716a3..d91ec132c 100644 --- a/include/ignition/physics/World.hh +++ b/include/ignition/physics/World.hh @@ -21,6 +21,7 @@ #include #include +#include namespace ignition { @@ -61,6 +62,74 @@ namespace ignition }; }; + ///////////////////////////////////////////////// + using GravityRequiredFeatures = FeatureList; + + ///////////////////////////////////////////////// + /// \brief Get and set the World's gravity vector in a specified frame. + class IGNITION_PHYSICS_VISIBLE Gravity + : public virtual + FeatureWithRequirements + { + /// \brief The World API for getting and setting the gravity vector. + public: template + class World : public virtual Feature::World + { + public: using LinearVectorType = + typename FromPolicy::template Use; + + public: using RelativeForceType = + typename FromPolicy::template Use; + + /// \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 + class Implementation : public virtual Feature::Implementation + { + public: using LinearVectorType = + typename FromPolicy::template Use; + + /// \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 index e44397600..079618485 100644 --- 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 { diff --git a/include/ignition/physics/detail/World.hh b/include/ignition/physics/detail/World.hh index 7fd36baf5..007e61a6f 100644 --- a/include/ignition/physics/detail/World.hh +++ b/include/ignition/physics/detail/World.hh @@ -44,6 +44,61 @@ const std::string &CollisionDetector::World:: ->GetWorldCollisionDetector(this->identity); } +///////////////////////////////////////////////// +template +void Gravity::World::SetGravity( + const RelativeForceType &_gravity) +{ + // Resolve to world coordinates + auto &impl = *this->template Interface(); + auto gravityInWorld = + detail::Resolve(impl, _gravity, FrameID::World(), FrameID::World()); + + this->template Interface() + ->SetWorldGravity(this->identity, gravityInWorld); +} + +///////////////////////////////////////////////// +template +void Gravity::World::SetGravity( + const LinearVectorType &_gravity, + const FrameID &_inCoordinatesOf) +{ + // Call SetWorldGravity directly if using world coordinates + if (_inCoordinatesOf == FrameID::World()) + { + this->template Interface() + ->SetWorldGravity(this->identity, _gravity); + } + // Otherwise make a RelativeForce object and call the other API + else + { + RelativeForceType gravityInRef(_inCoordinatesOf, _gravity); + this->SetGravity(gravityInRef); + } +} + +///////////////////////////////////////////////// +template +auto Gravity::World::GetGravity( + const FrameID &_inCoordinatesOf) const + -> LinearVectorType +{ + // Return quickly if using world coordinates + auto gravityInWorld = this->template Interface() + ->GetWorldGravity(this->identity); + if (_inCoordinatesOf == FrameID::World()) + { + return gravityInWorld; + } + + // Otherwise resolve to proper frame + auto &impl = *this->template Interface(); + RelativeForceType gravityInRef(FrameID::World(), gravityInWorld); + return detail::Resolve(impl, gravityInRef, FrameID::World(), + _inCoordinatesOf); +} + ///////////////////////////////////////////////// template void Solver::World::SetSolver(