merge from main

Signed-off-by: Ian Chen <[email protected]>

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

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<WorldInfo>(_id);
+  if (worldInfo)
+  {
+    worldInfo->world->getSolverInfo().m_numIterations = _iterations;
+  }
+}
+
+/////////////////////////////////////////////////
+std::size_t WorldFeatures::GetWorldSolverIterations(const Identity &_id) const
+{
+  const auto worldInfo = this->ReferenceInterface<WorldInfo>(_id);
+  if (worldInfo)
+  {
+    return worldInfo->world->getSolverInfo().m_numIterations;
+  }
+  return 0u;
+}
+
 }
 }
 }

bullet-featherstone/src/WorldFeatures.hh

@@ -18,6 +18,7 @@
 #ifndef GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_
 #define GZ_PHYSICS_BULLET_SRC_WORLDFEATURES_HH_
 
+#include <cstddef>
 #include <string>
 
 #include <gz/physics/World.hh>
@@ -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;
 };
 
 }

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<dart::simulation::World>(_id);
+
+  auto solver =
+      dynamic_cast<dart::constraint::BoxedLcpConstraintSolver *>(
+      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<const dart::constraint::PgsBoxedLcpSolver>(
+        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<dart::constraint::PgsBoxedLcpSolver>();
+    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<dart::simulation::World>(_id);
+
+  auto solver =
+      dynamic_cast<dart::constraint::BoxedLcpConstraintSolver *>(
+      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<const dart::constraint::PgsBoxedLcpSolver>(
+        boxedLcpSolver);
+    return pgsSolver->getOption().mMaxIteration;
+  }
+  else
+  {
+    gzwarn << "Solver iterations is only supported for [PgsBoxedLcpSolver]"
+            << std::endl;
+  }
+  return 0u;
+}
+
 }
 }
 }

dartsim/src/WorldFeatures.hh

@@ -18,6 +18,7 @@
 #ifndef GZ_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_
 #define GZ_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_
 
+#include <cstddef>
 #include <string>
 
 #include <gz/physics/World.hh>
@@ -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;
 };
 
 }

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());
 }

include/gz/physics/World.hh

@@ -18,6 +18,7 @@
 #ifndef GZ_PHYSICS_WORLD_HH_
 #define GZ_PHYSICS_WORLD_HH_
 
+#include <cstddef>
 #include <string>
 
 #include <gz/physics/FeatureList.hh>
@@ -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;
       };
     };
   }

include/gz/physics/detail/World.hh

@@ -18,6 +18,7 @@
 #ifndef GZ_PHYSICS_DETAIL_WORLD_HH_
 #define GZ_PHYSICS_DETAIL_WORLD_HH_
 
+#include <cstddef>
 #include <string>
 
 #include <gz/physics/World.hh>
@@ -135,6 +136,24 @@ const std::string &Solver::World<PolicyT, FeaturesT>::
       ->GetWorldSolver(this->identity);
 }
 
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+void Solver::World<PolicyT, FeaturesT>::SetSolverIterations(
+    std::size_t _iterations)
+{
+  this->template Interface<Solver>()
+      ->SetWorldSolverIterations(this->identity, _iterations);
+}
+
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+std::size_t Solver::World<PolicyT, FeaturesT>::
+    GetSolverIterations() const
+{
+  return this->template Interface<Solver>()
+      ->GetWorldSolverIterations(this->identity);
+}
+
 }  // namespace physics
 }  // namespace gz
 

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);

test/common_test/world_features.cc

@@ -15,6 +15,7 @@
  *
 */
 #include <gtest/gtest.h>
+#include <string>
 
 #include <gz/common/Console.hh>
 #include <gz/plugin/Loader.hh>
@@ -39,6 +40,7 @@
 #include <gz/physics/sdf/ConstructWorld.hh>
 
 #include <sdf/Root.hh>
+#include <sdf/Types.hh>
 
 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<WorldSolverFeatureList>
+{
+  public: gz::physics::World3dPtr<WorldSolverFeatureList> LoadWorld(
+      const std::string &_pluginName)
+  {
+    gz::plugin::PluginPtr plugin = this->loader.Instantiate(_pluginName);
+
+    auto engine =
+        gz::physics::RequestEngine3d<WorldSolverFeatureList>::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);