diff --git a/bullet-featherstone/src/Base.cc b/bullet-featherstone/src/Base.cc index a64aced52..ac66f5746 100644 --- a/bullet-featherstone/src/Base.cc +++ b/bullet-featherstone/src/Base.cc @@ -43,12 +43,17 @@ WorldInfo::WorldInfo(std::string name_) this->world->getSolverInfo().m_jointFeedbackInJointFrame = true; this->world->getSolverInfo().m_jointFeedbackInWorldSpace = false; - // By default a large impulse is applied when mesh collisions penetrate + // By default a large impulse is applied when collisions penetrate // which causes unstable behavior. Bullet featherstone does not support // configuring split impulse and penetration threshold parameters. Instead // the penentration impulse depends on the erp2 parameter so set to a small - // value (default is 0.2). + // value (default in bullet is 0.2). this->world->getSolverInfo().m_erp2 = btScalar(0.02); + + // Set solver iterations to the same as the default value in SDF, + // //world/physics/solver/bullet/iters + // (default in bullet is 10) + this->world->getSolverInfo().m_numIterations = 50u; } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/WorldFeatures.cc b/bullet-featherstone/src/WorldFeatures.cc index c685c5784..5746034f9 100644 --- a/bullet-featherstone/src/WorldFeatures.cc +++ b/bullet-featherstone/src/WorldFeatures.cc @@ -52,6 +52,51 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( return WorldFeatures::LinearVectorType(0, 0, 0); } } + +///////////////////////////////////////////////// +void WorldFeatures::SetWorldSolver(const Identity &, + const std::string &_solver) +{ + // bullet-featherstone implementation uses btMultiBodyDynamicsWorld + // so currently only supports BT_MULTIBODY_SOLVER + if (_solver != "MultiBodySolver") + { + gzwarn << "bullet-featherstone implementation currently only supports " + << "MultiBodySolver (BT_MULTIBODY_SOLVER)" << std::endl; + } +} + +///////////////////////////////////////////////// +const std::string &WorldFeatures::GetWorldSolver(const Identity &) const +{ + // bullet-featherstone implementation uses btMultiBodyDynamicsWorld + // so currently only supports BT_MULTIBODY_SOLVER + static std::string solverType = "MultiBodySolver"; + return solverType; +} + +///////////////////////////////////////////////// +void WorldFeatures::SetWorldSolverIterations(const Identity &_id, + std::size_t _iterations) +{ + const auto worldInfo = this->ReferenceInterface(_id); + if (worldInfo) + { + worldInfo->world->getSolverInfo().m_numIterations = _iterations; + } +} + +///////////////////////////////////////////////// +std::size_t WorldFeatures::GetWorldSolverIterations(const Identity &_id) const +{ + const auto worldInfo = this->ReferenceInterface(_id); + if (worldInfo) + { + return worldInfo->world->getSolverInfo().m_numIterations; + } + return 0u; +} + } } } diff --git a/bullet-featherstone/src/WorldFeatures.hh b/bullet-featherstone/src/WorldFeatures.hh index aefa84fd1..3e8f11403 100644 --- a/bullet-featherstone/src/WorldFeatures.hh +++ b/bullet-featherstone/src/WorldFeatures.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_ #define GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_ +#include #include #include @@ -29,7 +30,8 @@ namespace physics { namespace bullet_featherstone { struct WorldFeatureList : FeatureList< - Gravity + Gravity, + Solver > { }; class WorldFeatures : @@ -42,6 +44,21 @@ class WorldFeatures : // Documentation inherited public: LinearVectorType GetWorldGravity(const Identity &_id) const override; + + // Documentation inherited + public: void SetWorldSolver(const Identity &_id, const std::string &_solver) + override; + + // Documentation inherited + public: const std::string &GetWorldSolver(const Identity &_id) const override; + + // Documentation inherited + public: void SetWorldSolverIterations(const Identity &_id, std::size_t) + override; + + // Documentation inherited + public: std::size_t GetWorldSolverIterations(const Identity &_id) const + override; }; } diff --git a/dartsim/src/WorldFeatures.cc b/dartsim/src/WorldFeatures.cc index da7088580..039b55e99 100644 --- a/dartsim/src/WorldFeatures.cc +++ b/dartsim/src/WorldFeatures.cc @@ -196,6 +196,78 @@ const std::string &WorldFeatures::GetWorldSolver(const Identity &_id) const return solver->getBoxedLcpSolver()->getType(); } +///////////////////////////////////////////////// +void WorldFeatures::SetWorldSolverIterations(const Identity &_id, + std::size_t _iterations) +{ + auto world = this->ReferenceInterface(_id); + + auto solver = + dynamic_cast( + world->getConstraintSolver()); + + if (!solver) + { + gzwarn << "Failed to cast constraint solver to [BoxedLcpConstraintSolver]" + << std::endl; + return; + } + + auto boxedLcpSolver = solver->getBoxedLcpSolver(); + if (boxedLcpSolver->getType() == "PgsBoxedLcpSolver") + { + auto pgsSolver = + std::dynamic_pointer_cast( + boxedLcpSolver); + + // getBoxedLcpSolver returns a const shared_ptr so in order to update + // the solver parameters, we need to create a new boxed lcp solver with + // updated values and set it back to the constraint solver. + auto newPgsSolver = std::make_shared(); + auto option = pgsSolver->getOption(); + option.mMaxIteration = _iterations; + newPgsSolver->setOption(option); + solver->setBoxedLcpSolver(newPgsSolver); + } + else + { + gzwarn << "Solver iterations is only supported for [PgsBoxedLcpSolver]" + << std::endl; + } +} + +///////////////////////////////////////////////// +std::size_t WorldFeatures::GetWorldSolverIterations(const Identity &_id) const +{ + auto world = this->ReferenceInterface(_id); + + auto solver = + dynamic_cast( + world->getConstraintSolver()); + + if (!solver) + { + gzwarn << "Failed to cast constraint solver to [BoxedLcpConstraintSolver]" + << std::endl; + return 0u; + } + + auto boxedLcpSolver = solver->getBoxedLcpSolver(); + if (boxedLcpSolver->getType() == "PgsBoxedLcpSolver") + { + auto pgsSolver = + std::dynamic_pointer_cast( + boxedLcpSolver); + return pgsSolver->getOption().mMaxIteration; + } + else + { + gzwarn << "Solver iterations is only supported for [PgsBoxedLcpSolver]" + << std::endl; + } + return 0u; +} + } } } diff --git a/dartsim/src/WorldFeatures.hh b/dartsim/src/WorldFeatures.hh index da15810dd..6538747be 100644 --- a/dartsim/src/WorldFeatures.hh +++ b/dartsim/src/WorldFeatures.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_ #define GZ_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_ +#include #include #include @@ -68,6 +69,14 @@ class WorldFeatures : // Documentation inherited public: const std::string &GetWorldSolver(const Identity &_id) const override; + + // Documentation inherited + public: void SetWorldSolverIterations(const Identity &_id, std::size_t) + override; + + // Documentation inherited + public: std::size_t GetWorldSolverIterations(const Identity &_id) const + override; }; } diff --git a/dartsim/src/WorldFeatures_TEST.cc b/dartsim/src/WorldFeatures_TEST.cc index fe0eef69a..ce6d4793b 100644 --- a/dartsim/src/WorldFeatures_TEST.cc +++ b/dartsim/src/WorldFeatures_TEST.cc @@ -115,6 +115,12 @@ TEST_F(WorldFeaturesFixture, Solver) world->SetSolver("dantzig"); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); + world->SetSolverIterations(20u); + EXPECT_EQ(0u, world->GetSolverIterations()); + world->SetSolver("pgs"); EXPECT_EQ("PgsBoxedLcpSolver", world->GetSolver()); + + world->SetSolverIterations(20u); + EXPECT_EQ(20u, world->GetSolverIterations()); } diff --git a/include/gz/physics/World.hh b/include/gz/physics/World.hh index dabc453ef..ddeb8f245 100644 --- a/include/gz/physics/World.hh +++ b/include/gz/physics/World.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_WORLD_HH_ #define GZ_PHYSICS_WORLD_HH_ +#include #include #include @@ -184,6 +185,14 @@ namespace gz /// \brief Get the name of the solver in use. /// \return Name of solver. public: const std::string &GetSolver() const; + + /// \brief Set the number of solver iterations per step. + /// \param[in] _iterations Number of solver iterations per step + public: void SetSolverIterations(std::size_t _iterations); + + /// \brief Get the number of solver iterations per step. + /// \return Number of solver iterations per step. + public: std::size_t GetSolverIterations() const; }; /// \private The implementation API for the solver. @@ -201,6 +210,20 @@ namespace gz /// \return Name of solver. public: virtual const std::string &GetWorldSolver( const Identity &_id) const = 0; + + /// \brief Implementation API for setting the number of solver + /// iterations. + /// \param[in] _id Identity of the world. + /// \param[in] _solver Number of solver iterations. + public: virtual void SetWorldSolverIterations( + const Identity &_id, std::size_t _iterations) = 0; + + /// \brief Implementation API for getting the number of solver + /// iterations. + /// \param[in] _id Identity of the world. + /// \return Number of solver iterations. + public: virtual std::size_t GetWorldSolverIterations( + const Identity &_id) const = 0; }; }; } diff --git a/include/gz/physics/detail/World.hh b/include/gz/physics/detail/World.hh index d5270913c..27a0daf5e 100644 --- a/include/gz/physics/detail/World.hh +++ b/include/gz/physics/detail/World.hh @@ -18,6 +18,7 @@ #ifndef GZ_PHYSICS_DETAIL_WORLD_HH_ #define GZ_PHYSICS_DETAIL_WORLD_HH_ +#include #include #include @@ -135,6 +136,24 @@ const std::string &Solver::World:: ->GetWorldSolver(this->identity); } +///////////////////////////////////////////////// +template +void Solver::World::SetSolverIterations( + std::size_t _iterations) +{ + this->template Interface() + ->SetWorldSolverIterations(this->identity, _iterations); +} + +///////////////////////////////////////////////// +template +std::size_t Solver::World:: + GetSolverIterations() const +{ + return this->template Interface() + ->GetWorldSolverIterations(this->identity); +} + } // namespace physics } // namespace gz diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 346821bb6..cb7d88497 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1133,7 +1133,7 @@ TYPED_TEST(JointFeaturesAttachDetachTest, JointAttachDetach) // After a while, body2 should reach the ground and come to a stop std::size_t stepCount = 0u; - const std::size_t maxNumSteps = 1000u; + const std::size_t maxNumSteps = 2000u; while (stepCount++ < maxNumSteps) { world->Step(output, state, input); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index e1e68c0f0..9dbc804fe 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -15,6 +15,7 @@ * */ #include +#include #include #include @@ -39,6 +40,7 @@ #include #include +#include using AssertVectorApprox = gz::physics::test::AssertVectorApprox; @@ -457,6 +459,55 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) } } +struct WorldSolverFeatureList : gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::Solver, + gz::physics::sdf::ConstructSdfWorld +> { }; + +class WorldSolverTest : public WorldFeaturesTest +{ + public: gz::physics::World3dPtr LoadWorld( + const std::string &_pluginName) + { + gz::plugin::PluginPtr plugin = this->loader.Instantiate(_pluginName); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + + sdf::Root root; + const sdf::Errors errors = root.Load( + common_test::worlds::kEmptySdf); + EXPECT_TRUE(errors.empty()) << errors; + if (errors.empty()) + { + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + return world; + } + return nullptr; + } +}; + +TEST_F(WorldSolverTest, WorldSolver) +{ + for (const std::string &name : this->pluginNames) + { + auto world = this->LoadWorld(name); + ASSERT_NE(nullptr, world); + + EXPECT_FALSE(world->GetSolver().empty()); + EXPECT_NO_THROW(world->SetSolver("invalid")); + EXPECT_NE("invalid", world->GetSolver()); + + if (PhysicsEngineName(name) == "bullet-featherstone") + { + EXPECT_LT(0u, world->GetSolverIterations()); + world->SetSolverIterations(100u); + EXPECT_EQ(100u, world->GetSolverIterations()); + } + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv);