From 4c34f4eb2cae0382e5f93bd19532088b876be948 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 31 May 2024 21:50:18 +0900 Subject: [PATCH] Parse and set bullet solver iterations (#2351) Signed-off-by: Ian Chen --- include/gz/sim/components/Physics.hh | 7 +++++ src/LevelManager.cc | 44 +++++++++++++++++---------- src/SdfEntityCreator.cc | 45 ++++++++++++++++++---------- src/systems/physics/Physics.cc | 25 +++++++++++++++- test/integration/physics_system.cc | 23 +++++++++++--- test/worlds/physics_options.sdf | 5 ++++ 6 files changed, 114 insertions(+), 35 deletions(-) diff --git a/include/gz/sim/components/Physics.hh b/include/gz/sim/components/Physics.hh index e486c0d0b9..22f6b3f849 100644 --- 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 #include #include @@ -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; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.PhysicsSolverIterations", + PhysicsSolverIterations) } } } diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 0513fb4f30..8f0ace19ae 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -18,6 +18,7 @@ #include "LevelManager.hh" #include +#include #include #include @@ -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("collision_detector"); + if (dartElem->HasElement("collision_detector")) + { + auto collisionDetector = + dartElem->Get("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("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("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("iters"); + this->runner->entityCompMgr.CreateComponent(worldEntity, + components::PhysicsSolverIterations(solverIterations)); + } + } } } diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index f4b693533b..35a3246107 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include #include @@ -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("collision_detector"); + if (dartElem->HasElement("collision_detector")) + { + auto collisionDetector = + dartElem->Get("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("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("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("iters"); + this->dataPtr->ecm->CreateComponent(worldEntity, + components::PhysicsSolverIterations(solverIterations)); + } + } } } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 6a56cb101c..7b59091d74 100644 --- 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(_entity); + if (solverItersComp) + { + auto solverFeature = + this->entityWorldMap.EntityCast( + _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 in SDF) auto worldModelFeature = diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 027005a406..2e7989b09a 100644 --- 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.EachData()); } - checked = true; + checkedDart = true; return true; }); + _ecm.Each( + [&](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 index 3659636878..1d68175219 100644 --- a/test/worlds/physics_options.sdf +++ b/test/worlds/physics_options.sdf @@ -8,6 +8,11 @@ pgs + + + 100 + + 0