From 79ab869af051043bf5c38444d9ee20e1b3b6bee3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Aug 2024 14:59:03 -0700 Subject: [PATCH 1/7] Model.hh: add ModelByName and ModelCount APIs Backport nested model accessor APIs from #2494. Signed-off-by: Steve Peters --- include/gz/sim/Model.hh | 14 ++++++++++++++ src/Model.cc | 16 ++++++++++++++++ test/integration/model.cc | 24 ++++++++++++++++++++++++ 3 files changed, 54 insertions(+) diff --git a/include/gz/sim/Model.hh b/include/gz/sim/Model.hh index 9de49ea0a1..ddb0e8130d 100644 --- a/include/gz/sim/Model.hh +++ b/include/gz/sim/Model.hh @@ -137,6 +137,14 @@ namespace gz public: sim::Entity LinkByName(const EntityComponentManager &_ecm, const std::string &_name) const; + /// \brief Get the ID of a nested model entity which is an immediate + /// child of this model. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Nested model name. + /// \return Nested model entity. + public: sim::Entity ModelByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + /// \brief Get all joints which are immediate children of this model. /// \param[in] _ecm Entity-component manager. /// \return All joints in this model. @@ -167,6 +175,12 @@ namespace gz /// \return Number of links in this model. public: uint64_t LinkCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of nested models which are immediate children + /// of this model. + /// \param[in] _ecm Entity-component manager. + /// \return Number of nested models in this model. + public: uint64_t ModelCount(const EntityComponentManager &_ecm) const; + /// \brief Set a command to change the model's pose. /// \param[in] _ecm Entity-component manager. /// \param[in] _pose New model pose. diff --git a/src/Model.cc b/src/Model.cc index a23779981f..01f4538fb7 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -148,6 +148,16 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm, components::Link()); } +////////////////////////////////////////////////// +Entity Model::ModelByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Model()); +} + ////////////////////////////////////////////////// std::vector Model::Joints(const EntityComponentManager &_ecm) const { @@ -184,6 +194,12 @@ uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const return this->Links(_ecm).size(); } +////////////////////////////////////////////////// +uint64_t Model::ModelCount(const EntityComponentManager &_ecm) const +{ + return this->Models(_ecm).size(); +} + ////////////////////////////////////////////////// void Model::SetWorldPoseCmd(EntityComponentManager &_ecm, const math::Pose3d &_pose) diff --git a/test/integration/model.cc b/test/integration/model.cc index f878088631..f4a0f00b2c 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -164,6 +164,30 @@ TEST_F(ModelIntegrationTest, SourceFilePath) EXPECT_EQ("/tmp/path", model.SourceFilePath(ecm)); } +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, ModelByName) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + Model model(eModel); + EXPECT_EQ(eModel, model.Entity()); + EXPECT_EQ(0u, model.ModelCount(ecm)); + + // Nested Model + auto eNestedModel = ecm.CreateEntity(); + ecm.CreateComponent(eNestedModel, components::Model()); + ecm.CreateComponent(eNestedModel, + components::ParentEntity(eModel)); + ecm.CreateComponent(eNestedModel, + components::Name("nested_model_name")); + + // Check model + EXPECT_EQ(eNestedModel, model.ModelByName(ecm, "nested_model_name")); + EXPECT_EQ(1u, model.ModelCount(ecm)); +} + ////////////////////////////////////////////////// TEST_F(ModelIntegrationTest, LinkByName) { From 774f21c15c69c6dec4ef0230df160d8c4db2c121 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Aug 2024 15:00:44 -0700 Subject: [PATCH 2/7] Add WrenchMeasured component Signed-off-by: Steve Peters --- include/gz/sim/components/WrenchMeasured.hh | 57 +++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 include/gz/sim/components/WrenchMeasured.hh diff --git a/include/gz/sim/components/WrenchMeasured.hh b/include/gz/sim/components/WrenchMeasured.hh new file mode 100644 index 0000000000..74c350514b --- /dev/null +++ b/include/gz/sim/components/WrenchMeasured.hh @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_ +#define GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_ + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { + +namespace components +{ +/// \brief Wrench measured by a ForceTorqueSensor in SI units (Nm for torque, +/// N for force). +/// The wrench is expressed in the Sensor frame and the force component is +/// applied at the sensor origin. +/// \note The term Wrench is used here to mean a pair of 3D vectors representing +/// torque and force quantities expressed in a given frame and where the force +/// is applied at the origin of the frame. This is different from the Wrench +/// used in screw theory. +/// \note The value of force_offset in msgs::Wrench is ignored for this +/// component. The force is assumed to be applied at the origin of the sensor +/// frame. +using WrenchMeasured = + Component; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.WrenchMeasured", + WrenchMeasured) +} // namespace components +} +} +} + +#endif From a518a2276564ffc316445d64739c4422360346fc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Aug 2024 15:01:15 -0700 Subject: [PATCH 3/7] Physics.cc: fix typo in comment Signed-off-by: Steve Peters --- src/systems/physics/Physics.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 28f78e9032..01f56fdc63 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -432,7 +432,7 @@ class gz::sim::systems::PhysicsPrivate } return true; }}; - /// \brief msgs::Contacts equality comparison function. + /// \brief msgs::Wrench equality comparison function. public: std::function wrenchEql{ [](const msgs::Wrench &_a, const msgs::Wrench &_b) From 501d155ecc1c9584ea20462aac05ffd15d1fd141 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 14:50:19 -0700 Subject: [PATCH 4/7] sensor_TEST.py: move PreUpdate callback to Update The test's PreUpdate callback assumes that it executes after the ForceTorque::PreUpdate, so just move it to Update to gurantee it. Also fix a spelling error in the callback variable names. Signed-off-by: Steve Peters --- python/test/sensor_TEST.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py index aba4c61ca9..156459e11e 100755 --- a/python/test/sensor_TEST.py +++ b/python/test/sensor_TEST.py @@ -36,8 +36,8 @@ def test_model(self): def on_post_udpate_cb(_info, _ecm): self.post_iterations += 1 - def on_pre_udpate_cb(_info, _ecm): - self.pre_iterations += 1 + def on_update_cb(_info, _ecm): + self.iterations += 1 world_e = world_entity(_ecm) self.assertNotEqual(K_NULL_ENTITY, world_e) w = World(world_e) @@ -53,19 +53,19 @@ def on_pre_udpate_cb(_info, _ecm): # Pose Test self.assertEqual(Pose3d(0, 1, 0, 0, 0, 0), sensor.pose(_ecm)) # Topic Test - if self.pre_iterations <= 1: + if self.iterations <= 1: self.assertEqual(None, sensor.topic(_ecm)) else: self.assertEqual('sensor_topic_test', sensor.topic(_ecm)) # Parent Test self.assertEqual(j.entity(), sensor.parent(_ecm)) - def on_udpate_cb(_info, _ecm): - self.iterations += 1 + def on_pre_update_cb(_info, _ecm): + self.pre_iterations += 1 fixture.on_post_update(on_post_udpate_cb) - fixture.on_update(on_udpate_cb) - fixture.on_pre_update(on_pre_udpate_cb) + fixture.on_update(on_update_cb) + fixture.on_pre_update(on_pre_update_cb) fixture.finalize() server = fixture.server() From cd0b6373ab920954a26b7cc66e58cb0d2c5ae60d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 2 Aug 2024 14:51:34 -0700 Subject: [PATCH 5/7] fix spelling in on_post_update_cb Signed-off-by: Steve Peters --- python/test/sensor_TEST.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py index 156459e11e..61068c3fa3 100755 --- a/python/test/sensor_TEST.py +++ b/python/test/sensor_TEST.py @@ -33,7 +33,7 @@ def test_model(self): file_path = os.path.dirname(os.path.realpath(__file__)) fixture = TestFixture(os.path.join(file_path, 'joint_test.sdf')) - def on_post_udpate_cb(_info, _ecm): + def on_post_update_cb(_info, _ecm): self.post_iterations += 1 def on_update_cb(_info, _ecm): @@ -63,7 +63,7 @@ def on_update_cb(_info, _ecm): def on_pre_update_cb(_info, _ecm): self.pre_iterations += 1 - fixture.on_post_update(on_post_udpate_cb) + fixture.on_post_update(on_post_update_cb) fixture.on_update(on_update_cb) fixture.on_pre_update(on_pre_update_cb) fixture.finalize() From 279a0a7a587de6657a575ef5efae658abee4878a Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Aug 2024 15:05:51 -0700 Subject: [PATCH 6/7] backport minor fixes to force-torque test Signed-off-by: Steve Peters --- test/integration/force_torque_system.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index dd4684e627..23ff6cbbb4 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -43,7 +43,7 @@ class ForceTorqueTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight)) +TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightTopic)) { using namespace std::chrono_literals; // Start server @@ -59,8 +59,8 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight)) // Having iters exactly in sync with update rate can lead to a race condition // in the test between simulation and transport - size_t iters = 999u; - size_t updates = 100u; + const size_t iters = 999u; + const size_t updates = 100u; std::vector wrenches; wrenches.reserve(updates); From 4e36f47de087a90a830830628d61600a39d5af67 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Aug 2024 15:10:40 -0700 Subject: [PATCH 7/7] Include System.hh from some systems, add comment Signed-off-by: Steve Peters --- src/SystemManager.cc | 1 + src/systems/force_torque/ForceTorque.cc | 1 + src/systems/physics/Physics.cc | 1 + src/systems/user_commands/UserCommands.cc | 1 + 4 files changed, 4 insertions(+) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 9d86af1241..a2b85cd4c0 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -307,6 +307,7 @@ const std::vector& SystemManager::SystemsConfigure() return this->systemsConfigure; } +////////////////////////////////////////////////// const std::vector& SystemManager::SystemsConfigureParameters() { diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 8792f514d5..4fa376433d 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -45,6 +45,7 @@ #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/World.hh" #include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" using namespace gz; diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 01f56fdc63..eafbfe0aed 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -93,6 +93,7 @@ #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Model.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" // Components diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8f9ba3102c..02e5d7dee8 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -80,6 +80,7 @@ #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Model.hh" #include "gz/sim/SdfEntityCreator.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" #include "gz/sim/World.hh" #include "gz/sim/components/ContactSensorData.hh"