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/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 diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py index aba4c61ca9..61068c3fa3 100755 --- a/python/test/sensor_TEST.py +++ b/python/test/sensor_TEST.py @@ -33,11 +33,11 @@ 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_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_post_update(on_post_update_cb) + fixture.on_update(on_update_cb) + fixture.on_pre_update(on_pre_update_cb) fixture.finalize() server = fixture.server() 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/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 28f78e9032..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 @@ -432,7 +433,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) 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" 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); 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) {