Parse and set bullet solver iterations (#2351)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
gazebosim · May 31, 2024 · 4c34f4e · 4c34f4e
1 parent 6ab114f
commit 4c34f4e
Show file tree

Hide file tree

Showing 6 changed files with 114 additions and 35 deletions.
diff --git a/include/gz/sim/components/Physics.hh b/include/gz/sim/components/Physics.hh
@@ -17,6 +17,7 @@
 #ifndef GZ_SIM_COMPONENTS_PHYSICS_HH_
 #define GZ_SIM_COMPONENTS_PHYSICS_HH_
 
+#include <cstdint>
 #include <string>
 
 #include <gz/msgs/physics.pb.h>
@@ -65,6 +66,12 @@ namespace components
       class PhysicsSolverTag, serializers::StringSerializer>;
   GZ_SIM_REGISTER_COMPONENT("gz_sim_components.PhysicsSolver",
        PhysicsSolver)
+
+  /// \brief The number of solver iterations for each step.
+  using PhysicsSolverIterations = Component<uint32_t,
+      class PhysicsSolverIterationsTag>;
+  GZ_SIM_REGISTER_COMPONENT("gz_sim_components.PhysicsSolverIterations",
+       PhysicsSolverIterations)
 }
 }
 }

diff --git a/src/LevelManager.cc b/src/LevelManager.cc
@@ -18,6 +18,7 @@
 #include "LevelManager.hh"
 
 #include <algorithm>
+#include <cstdint>
 
 #include <sdf/Actor.hh>
 #include <sdf/Atmosphere.hh>
@@ -119,26 +120,39 @@ void LevelManager::ReadLevelPerformerInfo()
 
   // Populate physics options that aren't accessible outside the Element()
   // See https://github.com/osrf/sdformat/issues/508
-  if (physics->Element() && physics->Element()->HasElement("dart"))
+  if (physics->Element())
   {
-    auto dartElem = physics->Element()->GetElement("dart");
-
-    if (dartElem->HasElement("collision_detector"))
+    if (auto dartElem = physics->Element()->FindElement("dart"))
     {
-      auto collisionDetector =
-          dartElem->Get<std::string>("collision_detector");
+      if (dartElem->HasElement("collision_detector"))
+      {
+        auto collisionDetector =
+            dartElem->Get<std::string>("collision_detector");
 
-      this->runner->entityCompMgr.CreateComponent(worldEntity,
-          components::PhysicsCollisionDetector(collisionDetector));
+        this->runner->entityCompMgr.CreateComponent(worldEntity,
+            components::PhysicsCollisionDetector(collisionDetector));
+      }
+      if (auto solverElem = dartElem->FindElement("solver"))
+      {
+        if (solverElem->HasElement("solver_type"))
+        {
+          auto solver = solverElem->Get<std::string>("solver_type");
+          this->runner->entityCompMgr.CreateComponent(worldEntity,
+              components::PhysicsSolver(solver));
+        }
+      }
     }
-    if (dartElem->HasElement("solver") &&
-        dartElem->GetElement("solver")->HasElement("solver_type"))
+    if (auto bulletElem = physics->Element()->FindElement("bullet"))
     {
-      auto solver =
-          dartElem->GetElement("solver")->Get<std::string>("solver_type");
-
-      this->runner->entityCompMgr.CreateComponent(worldEntity,
-          components::PhysicsSolver(solver));
+      if (auto solverElem = bulletElem->FindElement("solver"))
+      {
+        if (solverElem->HasElement("iters"))
+        {
+          uint32_t solverIterations = solverElem->Get<uint32_t>("iters");
+          this->runner->entityCompMgr.CreateComponent(worldEntity,
+              components::PhysicsSolverIterations(solverIterations));
+        }
+      }
     }
   }
 

diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
@@ -15,6 +15,8 @@
  *
 */
 
+#include <cstdint>
+
 #include <gz/common/Console.hh>
 #include <gz/common/Profiler.hh>
 #include <sdf/Types.hh>
@@ -305,26 +307,39 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world)
 
   // Populate physics options that aren't accessible outside the Element()
   // See https://github.com/osrf/sdformat/issues/508
-  if (physics->Element() && physics->Element()->HasElement("dart"))
+  if (physics->Element())
   {
-    auto dartElem = physics->Element()->GetElement("dart");
-
-    if (dartElem->HasElement("collision_detector"))
+    if (auto dartElem = physics->Element()->FindElement("dart"))
     {
-      auto collisionDetector =
-          dartElem->Get<std::string>("collision_detector");
+      if (dartElem->HasElement("collision_detector"))
+      {
+        auto collisionDetector =
+            dartElem->Get<std::string>("collision_detector");
 
-      this->dataPtr->ecm->CreateComponent(worldEntity,
-          components::PhysicsCollisionDetector(collisionDetector));
+        this->dataPtr->ecm->CreateComponent(worldEntity,
+            components::PhysicsCollisionDetector(collisionDetector));
+      }
+      if (auto solverElem = dartElem->FindElement("solver"))
+      {
+        if (solverElem->HasElement("solver_type"))
+        {
+          auto solver = solverElem->Get<std::string>("solver_type");
+          this->dataPtr->ecm->CreateComponent(worldEntity,
+              components::PhysicsSolver(solver));
+        }
+      }
     }
-    if (dartElem->HasElement("solver") &&
-        dartElem->GetElement("solver")->HasElement("solver_type"))
+    if (auto bulletElem = physics->Element()->FindElement("bullet"))
     {
-      auto solver =
-          dartElem->GetElement("solver")->Get<std::string>("solver_type");
-
-      this->dataPtr->ecm->CreateComponent(worldEntity,
-          components::PhysicsSolver(solver));
+      if (auto solverElem = bulletElem->FindElement("solver"))
+      {
+        if (solverElem->HasElement("iters"))
+        {
+          uint32_t solverIterations = solverElem->Get<uint32_t>("iters");
+          this->dataPtr->ecm->CreateComponent(worldEntity,
+              components::PhysicsSolverIterations(solverIterations));
+        }
+      }
     }
   }
 

diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc
@@ -623,7 +623,6 @@ class gz::sim::systems::PhysicsPrivate
 
   //////////////////////////////////////////////////
   // Nested Models
-
   /// \brief Feature list to construct nested models
   public: struct NestedModelFeatureList : physics::FeatureList<
             MinimumFeatureList,
@@ -1024,6 +1023,30 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm,
             solverFeature->SetSolver(solverComp->Data());
           }
         }
+        auto solverItersComp =
+            _ecm.Component<components::PhysicsSolverIterations>(_entity);
+        if (solverItersComp)
+        {
+          auto solverFeature =
+              this->entityWorldMap.EntityCast<SolverFeatureList>(
+              _entity);
+          if (!solverFeature)
+          {
+            static bool informed{false};
+            if (!informed)
+            {
+              gzdbg << "Attempting to set physics options, but the "
+                     << "phyiscs engine doesn't support feature "
+                     << "[SolverFeature]. Options will be ignored."
+                     << std::endl;
+              informed = true;
+            }
+          }
+          else
+          {
+            solverFeature->SetSolverIterations(solverItersComp->Data());
+          }
+        }
 
         // World Model proxy (used for joints directly under <world> in SDF)
         auto worldModelFeature =

diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc
@@ -1834,12 +1834,13 @@ TEST_F(PhysicsSystemFixture, PhysicsOptions)
   serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH),
     "test", "worlds", "physics_options.sdf"));
 
-  bool checked{false};
+  bool checkedDart{false};
+  bool checkedBullet{false};
 
   // Create a system to check components
   test::Relay testSystem;
   testSystem.OnPostUpdate(
-    [&checked](const sim::UpdateInfo &,
+    [&checkedDart, &checkedBullet](const sim::UpdateInfo &,
     const sim::EntityComponentManager &_ecm)
     {
       _ecm.Each<components::World, components::PhysicsCollisionDetector,
@@ -1858,16 +1859,30 @@ TEST_F(PhysicsSystemFixture, PhysicsOptions)
           {
             EXPECT_EQ("pgs", _solver->Data());
           }
-          checked = true;
+          checkedDart = true;
           return true;
         });
+      _ecm.Each<components::World, components::PhysicsSolverIterations>(
+        [&](const gz::sim::Entity &, const components::World *,
+            const components::PhysicsSolverIterations *_solverIters)->bool
+        {
+          EXPECT_NE(nullptr, _solverIters);
+          if (_solverIters)
+          {
+            EXPECT_EQ(100, _solverIters->Data());
+          }
+          checkedBullet = true;
+          return true;
+        });
+
     });
 
   sim::Server server(serverConfig);
   server.AddSystem(testSystem.systemPtr);
   server.Run(true, 1, false);
 
-  EXPECT_TRUE(checked);
+  EXPECT_TRUE(checkedDart);
+  EXPECT_TRUE(checkedBullet);
 }
 
 /////////////////////////////////////////////////

diff --git a/test/worlds/physics_options.sdf b/test/worlds/physics_options.sdf
@@ -8,6 +8,11 @@
           <solver_type>pgs</solver_type>
         </solver>
       </dart>
+      <bullet>
+        <solver>
+          <iters>100</iters>
+        </solver>
+      </bullet>
       <real_time_factor>0</real_time_factor>
     </physics>
     <plugin