From d2a93c20fd1447d87b718a2462d011a129e696ae Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 29 Mar 2024 20:29:30 +0000 Subject: [PATCH 1/3] set physics solver iterations Signed-off-by: Ian Chen --- include/gz/sim/components/Physics.hh | 6 ++++ src/LevelManager.cc | 43 ++++++++++++++++++---------- src/SdfEntityCreator.cc | 43 ++++++++++++++++++---------- src/Server.cc | 1 + src/systems/physics/Physics.cc | 25 +++++++++++++++- 5 files changed, 87 insertions(+), 31 deletions(-) diff --git a/include/gz/sim/components/Physics.hh b/include/gz/sim/components/Physics.hh index e486c0d0b9..e6ffd28c74 100644 --- a/include/gz/sim/components/Physics.hh +++ b/include/gz/sim/components/Physics.hh @@ -65,6 +65,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..1d31513799 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -119,26 +119,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..a034a6e47d 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -305,26 +305,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/Server.cc b/src/Server.cc index 7aca6f33f3..8ed8632b71 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -145,6 +145,7 @@ Server::Server(const ServerConfig &_config) sdf::Root sdfRoot; sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); + sdfParserConfig.SetStoreResolvedURIs(true); sdfParserConfig.SetCalculateInertialConfiguration( sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1f6defbf44..9c989f309d 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 = From ad3353ce29d701bb1f61c616caac6d1753ababc7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 1 Apr 2024 20:28:34 +0000 Subject: [PATCH 2/3] add test Signed-off-by: Ian Chen --- src/Server.cc | 1 - test/integration/physics_system.cc | 23 +++++++++++++++++++---- test/worlds/physics_options.sdf | 5 +++++ 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 8ed8632b71..7aca6f33f3 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -145,7 +145,6 @@ Server::Server(const ServerConfig &_config) sdf::Root sdfRoot; sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); - sdfParserConfig.SetStoreResolvedURIs(true); sdfParserConfig.SetCalculateInertialConfiguration( sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); 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 Date: Tue, 2 Apr 2024 18:30:12 +0000 Subject: [PATCH 3/3] add includes Signed-off-by: Ian Chen --- include/gz/sim/components/Physics.hh | 1 + src/LevelManager.cc | 1 + src/SdfEntityCreator.cc | 2 ++ 3 files changed, 4 insertions(+) diff --git a/include/gz/sim/components/Physics.hh b/include/gz/sim/components/Physics.hh index e6ffd28c74..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 diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 1d31513799..8f0ace19ae 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -18,6 +18,7 @@ #include "LevelManager.hh" #include +#include #include #include diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index a034a6e47d..35a3246107 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include #include