diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index 4bd269d8ac..19add069df 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -43,19 +43,25 @@ class gz::sim::systems::ApplyJointForcePrivate public: transport::Node node; /// \brief Joint Entity + //! [jointEntityDeclaration] public: Entity jointEntity; + //! [jointEntityDeclaration] /// \brief Joint name public: std::string jointName; /// \brief Commanded joint force + //! [forceDeclaration] public: double jointForceCmd; + //! [forceDeclaration] /// \brief mutex to protect jointForceCmd public: std::mutex jointForceCmdMutex; /// \brief Model interface + //! [modelDeclaration] public: Model model{kNullEntity}; + //! [modelDeclaration] }; ////////////////////////////////////////////////// @@ -65,12 +71,14 @@ ApplyJointForce::ApplyJointForce() } ////////////////////////////////////////////////// +//! [Configure] void ApplyJointForce::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) { this->dataPtr->model = Model(_entity); + //! [Configure] if (!this->dataPtr->model.Valid(_ecm)) { @@ -96,17 +104,21 @@ void ApplyJointForce::Configure(const Entity &_entity, } // Subscribe to commands + //! [cmdTopic] auto topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + "/cmd_force"); + //! [cmdTopic] if (topic.empty()) { gzerr << "Failed to create valid topic for [" << this->dataPtr->jointName << "]" << std::endl; return; } + //! [cmdSub] this->dataPtr->node.Subscribe(topic, &ApplyJointForcePrivate::OnCmdForce, this->dataPtr.get()); + //! [cmdSub] gzmsg << "ApplyJointForce subscribing to Double messages on [" << topic << "]" << std::endl; @@ -127,11 +139,13 @@ void ApplyJointForce::PreUpdate(const UpdateInfo &_info, } // If the joint hasn't been identified yet, look for it + //! [findJoint] if (this->dataPtr->jointEntity == kNullEntity) { this->dataPtr->jointEntity = this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); } + //! [findJoint] if (this->dataPtr->jointEntity == kNullEntity) return; @@ -141,11 +155,14 @@ void ApplyJointForce::PreUpdate(const UpdateInfo &_info, return; // Update joint force + //! [jointForceComponent] auto force = _ecm.Component( this->dataPtr->jointEntity); + //! [jointForceComponent] std::lock_guard lock(this->dataPtr->jointForceCmdMutex); + //! [modifyComponent] if (force == nullptr) { _ecm.CreateComponent( @@ -156,14 +173,17 @@ void ApplyJointForce::PreUpdate(const UpdateInfo &_info, { force->Data()[0] += this->dataPtr->jointForceCmd; } + //! [modifyComponent] } ////////////////////////////////////////////////// +//! [setForce] void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg) { std::lock_guard lock(this->jointForceCmdMutex); this->jointForceCmd = _msg.data(); } +//! [setForce] GZ_ADD_PLUGIN(ApplyJointForce, System, diff --git a/src/systems/apply_joint_force/ApplyJointForce.hh b/src/systems/apply_joint_force/ApplyJointForce.hh index 0c93a28c20..88ada1ec84 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.hh +++ b/src/systems/apply_joint_force/ApplyJointForce.hh @@ -32,6 +32,14 @@ namespace systems class ApplyJointForcePrivate; /// \brief This system applies a force to the first axis of a specified joint. + /// + /// ## Components + /// + /// This system uses the following components: + /// + /// - gz::sim::components::JointForceCmd: A std::vector of force commands + /// of type `double`. Only element `[0]` is used, for applying force to + /// the specified joint. class ApplyJointForce : public System, public ISystemConfigure, diff --git a/test/integration/joint.cc b/test/integration/joint.cc index 12044025c6..423d9b6e4e 100644 --- a/test/integration/joint.cc +++ b/test/integration/joint.cc @@ -280,7 +280,7 @@ TEST_F(JointIntegrationTest, SetForce) std::vector forceCmd{10}; joint.SetForce(ecm, forceCmd); - // velocity cmd should exist + // force cmd should exist EXPECT_NE(nullptr, ecm.Component(eJoint)); EXPECT_EQ(forceCmd, ecm.Component(eJoint)->Data()); diff --git a/tutorials.md.in b/tutorials.md.in index 88b45c89f0..646bb238eb 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -64,6 +64,7 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. ## Developers * \subpage createsystemplugins "Create System Plugins": Programmatically access simulation using C++ plugins. +* \subpage usingcomponents "Using components": Using components in a system plugin. * \subpage rendering_plugins "Rendering plugins": Write plugins that use Gazebo Rendering on the server and client. * \subpage test_fixture "Test Fixture": Writing automated CI tests diff --git a/tutorials/component_jointforcecmd.md b/tutorials/component_jointforcecmd.md new file mode 100644 index 0000000000..e3a86134ac --- /dev/null +++ b/tutorials/component_jointforcecmd.md @@ -0,0 +1,83 @@ +\page jointforcecmdcomponent Case Study: Using the JointForceCmd Component + +We will show how to use one of the components, +\ref gz::sim::components::JointForceCmd, in a system. +This component allows us to set the force command on a joint. + +Programmatic usage of this component can be found in the source code for +systems and integration tests, such as the +[joint integration test](https://github.com/gazebosim/gz-sim/blob/gz-sim8/test/integration/joint.cc), +the \ref gz::sim::systems::ApplyJointForce system +([source code](https://github.com/gazebosim/gz-sim/tree/gz-sim8/src/systems/apply_joint_force)), +and others. + +The corresponding world SDF is [`apply_joint_force.sdf`](https://github.com/gazebosim/gz-sim/blob/gz-sim8/examples/worlds/apply_joint_force.sdf), which you can look at in Gazebo: + +```bash +gz sim apply_joint_force.sdf +``` + +We will walk through the relevant lines of source code in `ApplyJointForce` +that interact with `JointForceCmd`. + +### Find the entity of interest + +First, we will need access to an entity, the \ref gz::sim::Joint entity in this +case. It is declared as a member variable: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc jointEntityDeclaration + +An entity may have been created at the time the world is loaded, or you may +create an entity at runtime if it does not exist yet. +For joints, most likely they were defined in the SDF file that specifies the +world, and all we have to do at runtime is to look for the joint by its name. + +`ApplyJointForce` happens to be a system meant to be specified under `` +in the SDF, so at the time `Configure()` is called, it has access to a model +entity from which we can extract a \ref gz::sim::Model: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc modelDeclaration +\snippet src/systems/apply_joint_force/ApplyJointForce.cc Configure + +Using the Model object, we can find the joint by its name, when `PreUpdate()` +is called. +That gives us a Joint entity: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc findJoint + +### Modify the component + +Once we have the handle to an entity, we can modify components associated with +it. +A component may have been created at the time the world is loaded, or you may +create a component at runtime if it does not exist yet. + +In this case, we use the joint entity found above to look for and modify its +`JointForceCmd` component. +This will apply a force command to the joint. + +In `PreUpdate()`, look for the component: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc jointForceComponent + +Create it if it does not exist yet, and modify it: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc modifyComponent + +where the scalar joint force command is declared as a member variable: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc forceDeclaration + +and a callback function allows the user to specify a force on a topic: + +\snippet src/systems/apply_joint_force/ApplyJointForce.cc cmdTopic +\snippet src/systems/apply_joint_force/ApplyJointForce.cc cmdSub +\snippet src/systems/apply_joint_force/ApplyJointForce.cc setForce + +You can test this by issuing a force command to the topic: + +```bash +gz topic -t /model/joint_force_example/joint/j1/cmd_force \ + -m gz.msgs.Double -p 'data: 1.0' +``` +This should move the model that the joint is attached to. diff --git a/tutorials/terminology.md b/tutorials/terminology.md index 8b812fbf37..7103fee7ec 100644 --- a/tutorials/terminology.md +++ b/tutorials/terminology.md @@ -22,7 +22,7 @@ to developers touching the source code. can also create their own by inheriting from the \ref gz::sim::components::BaseComponent "BaseComponent" class or instantiating a template of - \ref gz::sim::components::Component "Component" + \ref gz::sim::components::Component "Component". * **System**: Logic that operates on all entities that have a given set of components. Systems are plugins that can be loaded at runtime. diff --git a/tutorials/using_components.md b/tutorials/using_components.md new file mode 100644 index 0000000000..717beea566 --- /dev/null +++ b/tutorials/using_components.md @@ -0,0 +1,74 @@ +\page usingcomponents Using Components in a System Plugin + +Gazebo uses the entity component system (ECS) software architecture. +See the [Terminology](./terminology.html) page for the definitions of entity, +component, system, and entity component manager (ECM) used in this tutorial. +In short, a simulation world consists of many entities, each of which is +associated with a set of components. + +System plugins can modify the simulation world by manipulating components. +The basic structure of a system plugin is outlined in the +[tutorial on creating system plugins](./createsystemplugins.html). + +Here, we will explain how a system can use components to modify the world. +You can view the list of \ref gz::sim::components in the API. + +Programmatic usage of components can be found in +[built-in systems](https://github.com/gazebosim/gz-sim/tree/gz-sim8/src/systems) +and [integration tests](https://github.com/gazebosim/gz-sim/blob/gz-sim8/test/integration) +in the source code. +Most of the built-in systems have a corresponding example SDF world. +You can find all the example worlds [here](https://github.com/gazebosim/gz-sim/tree/gz-sim8/examples/worlds). + +## Prerequisites + +This is a tutorial for developers or advanced users. +It assumes that you are familiar with basic usage of Gazebo. + +Prior to starting this tutorial, these other tutorials will help with +understanding: +- [Terminology](./terminology.html) +- [Create system plugins](./createsystemplugins.html) + +## Resources + +Quick access to resources mentioned in this tutorial: +- List of \ref gz::sim::components in the API +- List of \ref gz::sim::systems in the API +- Source code of [built-in systems](https://github.com/gazebosim/gz-sim/tree/gz-sim8/src/systems) +- Source code of [example worlds](https://github.com/gazebosim/gz-sim/tree/gz-sim8/examples/worlds) +- Source code of [integration tests](https://github.com/gazebosim/gz-sim/blob/gz-sim8/test/integration) + +## Entity Component Manager (ECM) + +The gateway to interact with entities is through the +\ref gz::sim::EntityComponentManager +([source code](https://github.com/gazebosim/gz-sim/blob/gz-sim8/include/gz/sim/EntityComponentManager.hh)), +ECM for short. +The ECM gives us access to all the entities, each of which gives us access +to its associated components. + +An ECM object is passed into all of the interfaces in a system, including +`ISystemConfigure()` and `ISystem*Update()`. +The signatures of those interfaces are specified in +\ref gz/sim/System.hh and explained in the +[tutorial on creating system plugins](./createsystemplugins.html). +We will assume that these interfaces are implemented in functions called +`Configure()` and `*Update()` in a system. + +Note that when `Configure()` is called, all the elements in the parent element +of the plugin have been loaded. +For example, if the plugin is attached to a ``, all the elements in that +`` would have been loaded. +Similarly for ``. +However, if you need to access entities outside the plugin's parent element, +they may not have finished loading at the time the plugin's `Configure()` is +called. +Then you may need to access those entities later, in `*Update()`. + +## Case studies + +The rest of the tutorial is case studies that walk through the usage of +specific components. + +- \subpage jointforcecmdcomponent "JointForceCmd"