diff --git a/examples/worlds/physics_options.sdf b/examples/worlds/physics_options.sdf new file mode 100644 index 0000000000..a02c603296 --- /dev/null +++ b/examples/worlds/physics_options.sdf @@ -0,0 +1,173 @@ + + + + + + 0.001 + 1.0 + + bullet + + pgs + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + 0 0 -0.5 0 -0.52 0 + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 1.57079632679 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + diff --git a/include/ignition/gazebo/components/Physics.hh b/include/ignition/gazebo/components/Physics.hh index 804059dedc..117c258367 100644 --- a/include/ignition/gazebo/components/Physics.hh +++ b/include/ignition/gazebo/components/Physics.hh @@ -17,6 +17,8 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ #define IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ +#include + #include #include @@ -48,6 +50,21 @@ namespace components serializers::PhysicsSerializer>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Physics", Physics) + + /// \brief The name of the collision detector to be used. The supported + /// options will depend on the physics engine being used. + using PhysicsCollisionDetector = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.PhysicsCollisionDetector", + PhysicsCollisionDetector) + + /// \brief The name of the solver to be used. The supported options will + /// depend on the physics engine being used. + using PhysicsSolver = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PhysicsSolver", + PhysicsSolver) } } } diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 74e2f71020..1b4d2f39d2 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -111,6 +111,31 @@ void LevelManager::ReadLevelPerformerInfo() this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::Physics(*physics)); + // 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")) + { + auto dartElem = physics->Element()->GetElement("dart"); + + if (dartElem->HasElement("collision_detector")) + { + auto collisionDetector = + dartElem->Get("collision_detector"); + + this->runner->entityCompMgr.CreateComponent(worldEntity, + components::PhysicsCollisionDetector(collisionDetector)); + } + if (dartElem->HasElement("solver") && + dartElem->GetElement("solver")->HasElement("solver_type")) + { + auto solver = + dartElem->GetElement("solver")->Get("solver_type"); + + this->runner->entityCompMgr.CreateComponent(worldEntity, + components::PhysicsSolver(solver)); + } + } + this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::MagneticField(this->runner->sdfWorld->MagneticField())); diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d7c98189c6..40b1be0ae2 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -280,6 +280,31 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) this->dataPtr->ecm->CreateComponent(worldEntity, components::Physics(*physics)); + // 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")) + { + auto dartElem = physics->Element()->GetElement("dart"); + + if (dartElem->HasElement("collision_detector")) + { + auto collisionDetector = + dartElem->Get("collision_detector"); + + this->dataPtr->ecm->CreateComponent(worldEntity, + components::PhysicsCollisionDetector(collisionDetector)); + } + if (dartElem->HasElement("solver") && + dartElem->GetElement("solver")->HasElement("solver_type")) + { + auto solver = + dartElem->GetElement("solver")->Get("solver_type"); + + this->dataPtr->ecm->CreateComponent(worldEntity, + components::PhysicsSolver(solver)); + } + } + // MagneticField this->dataPtr->ecm->CreateComponent(worldEntity, components::MagneticField(_world->MagneticField())); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 30467ea5f2..3255416565 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -49,8 +49,11 @@ #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerAffinity.hh" #include "ignition/gazebo/components/Physics.hh" +#include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" +#include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" @@ -631,12 +634,40 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::PhysicsCollisionDetector::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } + else if (typeId == components::PhysicsSolver::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::Pose::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); if (comp) setData(item, comp->Data()); } + else if (typeId == components::RenderEngineGuiPlugin::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } + else if (typeId == components::RenderEngineServerPlugin::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::Static::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c290b547f3..789ef6b904 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -111,6 +112,7 @@ #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh" #include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" @@ -395,6 +397,18 @@ class ignition::gazebo::systems::PhysicsPrivate CollisionFeatureList, physics::mesh::AttachMeshShapeFeature>{}; + ////////////////////////////////////////////////// + // Collision detector + /// \brief Feature list for setting and getting the collision detector + public: struct CollisionDetectorFeatureList : ignition::physics::FeatureList< + ignition::physics::CollisionDetector>{}; + + ////////////////////////////////////////////////// + // Solver + /// \brief Feature list for setting and getting the solver + public: struct SolverFeatureList : ignition::physics::FeatureList< + ignition::physics::Solver>{}; + ////////////////////////////////////////////////// // Nested Models @@ -410,7 +424,9 @@ class ignition::gazebo::systems::PhysicsPrivate MinimumFeatureList, CollisionFeatureList, ContactFeatureList, - NestedModelFeatureList>; + NestedModelFeatureList, + CollisionDetectorFeatureList, + SolverFeatureList>; /// \brief A map between world entity ids in the ECM to World Entities in /// ign-physics. @@ -657,6 +673,58 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) auto worldPtrPhys = this->engine->ConstructWorld(world); this->entityWorldMap.AddEntity(_entity, worldPtrPhys); + // Optional world features + auto collisionDetectorComp = + _ecm.Component(_entity); + if (collisionDetectorComp) + { + auto collisionDetectorFeature = + this->entityWorldMap.EntityCast( + _entity); + if (!collisionDetectorFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[CollisionDetectorFeature]. Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + collisionDetectorFeature->SetCollisionDetector( + collisionDetectorComp->Data()); + } + } + + auto solverComp = + _ecm.Component(_entity); + if (solverComp) + { + auto solverFeature = + this->entityWorldMap.EntityCast( + _entity); + if (!solverFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[SolverFeature]. Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + solverFeature->SetSolver(solverComp->Data()); + } + } + return true; }); @@ -1927,6 +1995,42 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, { IGN_PROFILE("PhysicsPrivate::UpdateSim"); + // Populate world components with default values + _ecm.EachNew( + [&](const Entity &_entity, + const components::World *)->bool + { + // If not provided by ECM, create component with values from physics if + // those features are available + auto collisionDetectorComp = + _ecm.Component(_entity); + if (!collisionDetectorComp) + { + auto collisionDetectorFeature = + this->entityWorldMap.EntityCast( + _entity); + if (collisionDetectorFeature) + { + _ecm.CreateComponent(_entity, components::PhysicsCollisionDetector( + collisionDetectorFeature->GetCollisionDetector())); + } + } + + auto solverComp = _ecm.Component(_entity); + if (!solverComp) + { + auto solverFeature = + this->entityWorldMap.EntityCast(_entity); + if (solverFeature) + { + _ecm.CreateComponent(_entity, + components::PhysicsSolver(solverFeature->GetSolver())); + } + } + + return true; + }); + IGN_PROFILE_BEGIN("Models"); // make sure we have an up-to-date mapping of canonical links to their models diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index d1c5236013..2877bef288 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index e1ac1762ec..d1e34607a2 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -53,6 +53,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" @@ -1373,3 +1374,87 @@ TEST_F(PhysicsSystemFixture, NestedModelIndividualCanonicalLinks) EXPECT_DOUBLE_EQ(model11WorldPose.Z(), model10It->second.Z()); EXPECT_DOUBLE_EQ(model12WorldPose.Z(), model10It->second.Z()); } + +///////////////////////////////////////////////// +TEST_F(PhysicsSystemFixture, DefaultPhysicsOptions) +{ + ignition::gazebo::ServerConfig serverConfig; + + bool checked{false}; + + // Create a system to check components + test::Relay testSystem; + testSystem.OnPostUpdate( + [&checked](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, const components::World *, + const components::PhysicsCollisionDetector *_collisionDetector, + const components::PhysicsSolver *_solver)->bool + { + EXPECT_NE(nullptr, _collisionDetector); + if (_collisionDetector) + { + EXPECT_EQ("ode", _collisionDetector->Data()); + } + EXPECT_NE(nullptr, _solver); + if (_solver) + { + EXPECT_EQ("DantzigBoxedLcpSolver", _solver->Data()); + } + checked = true; + return true; + }); + }); + + gazebo::Server server(serverConfig); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); + + EXPECT_TRUE(checked); +} + +///////////////////////////////////////////////// +TEST_F(PhysicsSystemFixture, PhysicsOptions) +{ + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "physics_options.sdf")); + + bool checked{false}; + + // Create a system to check components + test::Relay testSystem; + testSystem.OnPostUpdate( + [&checked](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, const components::World *, + const components::PhysicsCollisionDetector *_collisionDetector, + const components::PhysicsSolver *_solver)->bool + { + EXPECT_NE(nullptr, _collisionDetector); + if (_collisionDetector) + { + EXPECT_EQ("bullet", _collisionDetector->Data()); + } + EXPECT_NE(nullptr, _solver); + if (_solver) + { + EXPECT_EQ("pgs", _solver->Data()); + } + checked = true; + return true; + }); + }); + + gazebo::Server server(serverConfig); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); + + EXPECT_TRUE(checked); +} diff --git a/test/worlds/physics_options.sdf b/test/worlds/physics_options.sdf new file mode 100644 index 0000000000..d4fcae60e1 --- /dev/null +++ b/test/worlds/physics_options.sdf @@ -0,0 +1,17 @@ + + + + + + bullet + + pgs + + + + + + + diff --git a/tutorials/physics.md b/tutorials/physics.md index c86ecb07a7..4ff2a32097 100644 --- a/tutorials/physics.md +++ b/tutorials/physics.md @@ -84,6 +84,18 @@ serverConfig.SetPhysicsEngine("CustomEngine"); ignition::gazebo::Server server(serverConfig); ``` +## Engine configuration + +Gazebo supports the following physics engine configurations through SDF. +These options are available to all physics engines, but not all engines +may support them. The default physics engine, DART, supports all these options. + +* [//physics/dart/collision_detector](http://sdformat.org/spec?ver=1.8&elem=physics#dart_collision_detector) + * Options supported by DART: `ode` (default), `bullet`, `fcl`, `dart`. + +* [//physics/dart/solver/solver_type](http://sdformat.org/spec?ver=1.8&elem=physics#solver_solver_type) + * Options supported by DART: `dantzig` (default), `pgs` + ## Troubleshooting > Failed to find plugin [libCustomEngine.so]. Have you checked the