From 5c0eaaaee823efa0fee01e667276be5db12a9de3 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 3 Jun 2024 08:20:42 -0700 Subject: [PATCH 1/6] Fix typo in a comment (#2429) Signed-off-by: Nate Koenig --- src/ModelCommandAPI_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index 95f5b8b4a4..673f9c9a8b 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -608,7 +608,7 @@ TEST(ModelCommandAPI, GZ_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraSensor)) // Run without blocking. server.Run(false, 0, false); - // Tested command: gz model -m altimeter_mode -l link -s altimeter_sensor + // Tested command: gz model -m rgbd_camera -l rgbd_camera_link -s rgbd_camera { const std::string cmd = kGzModelCommand + "-m rgbd_camera -l rgbd_camera_link -s rgbd_camera"; From 1b577f1b77a2a93a3cc210de9b33ec9e54550329 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 3 Jun 2024 11:18:18 -0700 Subject: [PATCH 2/6] Use topicFromScopedName in a few systems (#2427) * Use topicFromScopedName in a few systems Signed-off-by: Nate Koenig * Fix topic name generation Signed-off-by: Nate Koenig --------- Signed-off-by: Nate Koenig --- .../LogicalAudioSensorPlugin.cc | 7 +++---- src/systems/pose_publisher/PosePublisher.cc | 3 +-- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index dd14a2d271..c0836401dd 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -405,12 +405,11 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( }; // create services for this source - const auto fullName = scopedName(entity, _ecm); - auto validName = transport::TopicUtils::AsValidTopic(fullName); + const auto validName = topicFromScopedName(entity, _ecm, false); if (validName.empty()) { gzerr << "Failed to create valid topics with entity scoped name [" - << fullName << "]" << std::endl; + << scopedName(entity, _ecm) << "]" << std::endl; return; } if (!this->node.Advertise(validName + "/play", playSrvCb)) @@ -504,7 +503,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( // create the detection publisher for this microphone auto pub = this->node.Advertise( - scopedName(entity, _ecm) + "/detection"); + topicFromScopedName(entity, _ecm, false) + "/detection"); if (!pub) { gzerr << "Error creating a detection publisher for microphone " diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index e75d5789a3..fd167a882e 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -250,8 +250,7 @@ void PosePublisher::Configure(const Entity &_entity, this->dataPtr->usePoseV = _sdf->Get("use_pose_vector_msg", this->dataPtr->usePoseV).first; - std::string poseTopic = scopedName(_entity, _ecm) + "/pose"; - poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); + std::string poseTopic = topicFromScopedName(_entity, _ecm, false) + "/pose"; if (poseTopic.empty()) { poseTopic = "/pose"; From 49ab5c6d0c69abdf0db45b99bcd350f4d90bf7cf Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 5 Jun 2024 07:55:42 -0700 Subject: [PATCH 3/6] Handle sdf::Geometry::EMPTY in conversions (#2430) Signed-off-by: Nate Koenig --- src/Conversions.cc | 4 ++++ src/Conversions_TEST.cc | 10 ++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/Conversions.cc b/src/Conversions.cc index 33548a6843..1ef5df8f82 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -259,6 +259,10 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) } } } + else if (_in.Type() == sdf::GeometryType::EMPTY) + { + out.set_type(msgs::Geometry::EMPTY); + } else { gzerr << "Geometry type [" << static_cast(_in.Type()) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 3d6e3e5861..2e7deecdd0 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -1206,3 +1206,13 @@ TEST(Conversions, MsgsPluginToSdf) EXPECT_EQ(innerXml, sdfPlugins[1].Contents()[0]->ToString("")); EXPECT_EQ(innerXml2, sdfPlugins[1].Contents()[1]->ToString("")); } + +///////////////////////////////////////////////// +TEST(Conversions, GeometryEmpty) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::EMPTY); + + auto geometryMsg = convert(geometry); + EXPECT_EQ(msgs::Geometry::EMPTY, geometryMsg.type()); +} From 45239e9e12077ade60bd01078e7ce92acf353889 Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Thu, 13 Jun 2024 02:32:15 -0500 Subject: [PATCH 4/6] Add support for no gravity link (#2398) Signed-off-by: youhy Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- src/SdfEntityCreator.cc | 8 ++++++++ src/systems/physics/Physics.cc | 18 ++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 6947ff329c..a84343fd42 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -588,6 +588,14 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) linkEntity, components::WindMode(_link->EnableWind())); } + if (!_link->EnableGravity()) + { + // If disable gravity, create a gravity component to the entity + // This gravity will have value 0,0,0 + this->dataPtr->ecm->CreateComponent( + linkEntity, components::Gravity()); + } + // Visuals for (uint64_t visualIndex = 0; visualIndex < _link->VisualCount(); ++visualIndex) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 544f16d54e..c61279205f 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1160,6 +1160,24 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) link.SetInertial(inertial->Data()); } + // get link gravity + const components::Gravity *gravity = + _ecm.Component(_entity); + if (nullptr != gravity) + { + // Entity has a gravity component that is all zeros when + // is set to false + // See SdfEntityCreator::CreateEntities() + if (gravity->Data() == math::Vector3d::Zero) + { + link.SetEnableGravity(false); + } + else + { + link.SetEnableGravity(true); + } + } + auto linkPtrPhys = modelPtrPhys->ConstructLink(link); this->entityLinkMap.AddEntity(_entity, linkPtrPhys); this->topLevelModelMap.insert(std::make_pair(_entity, From ca1b62602e282a905c4373da09128a10720d74e1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 25 Jun 2024 19:35:56 -0700 Subject: [PATCH 5/6] Add GravityEnabled boolean component (#2451) This adds a boolean GravityEnabled component to the existing gz/sim/components/Gravity.hh header file, which should be used with Link entities instead of the Vector3 gravity component. --------- Signed-off-by: Steve Peters Signed-off-by: youhy Co-authored-by: youhy --- include/gz/sim/components/Gravity.hh | 5 +++++ src/SdfEntityCreator.cc | 5 ++--- src/systems/physics/Physics.cc | 21 ++++++--------------- 3 files changed, 13 insertions(+), 18 deletions(-) diff --git a/include/gz/sim/components/Gravity.hh b/include/gz/sim/components/Gravity.hh index fe3cf88a9f..713e797acf 100644 --- a/include/gz/sim/components/Gravity.hh +++ b/include/gz/sim/components/Gravity.hh @@ -36,6 +36,11 @@ namespace components /// \brief Store the gravity acceleration. using Gravity = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Gravity", Gravity) + + /// \brief Store the gravity acceleration. + using GravityEnabled = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.GravityEnabled", GravityEnabled) } } } diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index a84343fd42..71c295db9b 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -590,10 +590,9 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) if (!_link->EnableGravity()) { - // If disable gravity, create a gravity component to the entity - // This gravity will have value 0,0,0 + // If disable gravity, create a GravityEnabled component to the entity this->dataPtr->ecm->CreateComponent( - linkEntity, components::Gravity()); + linkEntity, components::GravityEnabled(false)); } // Visuals diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c61279205f..687178a369 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1161,21 +1161,12 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) } // get link gravity - const components::Gravity *gravity = - _ecm.Component(_entity); - if (nullptr != gravity) - { - // Entity has a gravity component that is all zeros when - // is set to false - // See SdfEntityCreator::CreateEntities() - if (gravity->Data() == math::Vector3d::Zero) - { - link.SetEnableGravity(false); - } - else - { - link.SetEnableGravity(true); - } + const components::GravityEnabled *gravityEnabled = + _ecm.Component(_entity); + if (nullptr != gravityEnabled) + { + // gravityEnabled set in SdfEntityCreator::CreateEntities() + link.SetEnableGravity(gravityEnabled->Data()); } auto linkPtrPhys = modelPtrPhys->ConstructLink(link); From a02e891679c09a4850284012506abc7c06da1116 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 26 Jun 2024 23:26:07 +0000 Subject: [PATCH 6/6] Set max contacts for collision pairs (#2270) Signed-off-by: Ian Chen Co-authored-by: Addisu Z. Taddese --- src/systems/physics/Physics.cc | 38 +++++++++++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index b3581ae3e8..0a6564231f 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -617,6 +617,14 @@ class gz::sim::systems::PhysicsPrivate public: struct SolverFeatureList : gz::physics::FeatureList< gz::physics::Solver>{}; + ////////////////////////////////////////////////// + // CollisionPairMaxContacts + /// \brief Feature list for setting and getting the max total contacts for + /// collision pairs + public: struct CollisionPairMaxContactsFeatureList : + gz::physics::FeatureList< + gz::physics::CollisionPairMaxContacts>{}; + ////////////////////////////////////////////////// // Nested Models @@ -642,7 +650,8 @@ class gz::sim::systems::PhysicsPrivate NestedModelFeatureList, CollisionDetectorFeatureList, SolverFeatureList, - WorldModelFeatureList + WorldModelFeatureList, + CollisionPairMaxContactsFeatureList >; /// \brief A map between world entity ids in the ECM to World Entities in @@ -1045,6 +1054,33 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } } + auto physicsComp = + _ecm.Component(_entity); + if (physicsComp) + { + auto maxContactsFeature = + this->entityWorldMap.EntityCast< + CollisionPairMaxContactsFeatureList>(_entity); + if (!maxContactsFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[CollisionPairMaxContacts]. " + << "Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + maxContactsFeature->SetCollisionPairMaxContacts( + physicsComp->Data().MaxContacts()); + } + } + // World Model proxy (used for joints directly under in SDF) auto worldModelFeature = this->entityWorldMap.EntityCast(_entity);