Implement a custom feature

Prerequisites

• Installation
• Understanding the physics plugin
• Loading physics plugins
• Implement a physics feature

Implement a custom feature in DART plugin

In the last Implement a physics feature tutorial, we know how to implement a dummy physics engine as a plugin and load it using Gazebo Physics API. In this tutorial, we will look deeper into the structure of a physics engine plugin, for example, the available DART physics engine in gz-physics repository and how to define a custom Feature for the plugin.

Folder structure of the plugins

Below is the general structure of the gz-physics repository:

gz-physics
├── dartsim                   Files for dartsim plugin component.
├── tpe                       Files for tpe plugin component.
├── heightmap                 Files for heightmap component.
├── include/gz/physics  Header files.
├── mesh                      Files for mesh component.
├── resources                 Model and mesh resource files used by tests.
├── sdf                       Files for sdf component.
├── src                       Source files and unit tests.
├── test
├── tutorials                 Tutorials, written in markdown.
├── Changelog.md              Changelog.
└── CMakeLists.txt            CMake build script.

As shown above, there are two physics engines available:

• DART: gz-physics-dartsim-plugin.
• TPE: gz-physics-tpe-plugin.

and their plugin folders are placed just below the top level of gz-physics.

Looking closer to a plugin folder, for example, the dartsim (DART) plugin:

dartsim
├── worlds                            Example SDF files for testing dartsim plugin functionalities.
├── src                               Main implementation files of the plugin features interfacing the physics engines API
└── CMakeLists.txt                    CMake plugin build script.

Basically, new implementation of Feature or FeatureList, which is corresponded to a functionality of the external physics engine can be defined as a header in src folder. The custom feature could be added in a FeatureList and implemented its functionalities in src folder.

See the Understanding the physics plugin tutorial for details on physics engines.

Plugin and feature requirements

In general, the minimum set of features that any physics engine plugin must implement to be supported by Gazebo is as below:

• FindFreeGroupFeature
• SetFreeGroupWorldPose
• FreeGroupFrameSemantics
• LinkFrameSemantics
• ForwardStep
• RemoveEntities
• ConstructSdfLink
• ConstructSdfModel
• ConstructSdfWorld

This list defines the minimum requirements for the simulation capability of a physics engine plugin and also maintains backward compatibility with downstream physics plugins.

For custom feature requirements, there are two main component classes in the general structure of a custom feature:

• Entity corresponds to the "proxy object" where the Feature is implemented. These are the most common "proxy objects" that are inherited from Entity class:

  • Engine: Placeholder class for the Engine API. This class serves metadata for the physics engine (for example the GetEngineInfo feature). Every Engine feature must inherit this class.
  • Joint: defines physics concept Joint behaviors (for example the GetBasicJointState feature).
  • Link: defines physics concept Link structure.
  • Model: defines physics concept Model structure (for example the GetLinkFromModel feature including both Link and Model objects).
  • Shape: defines physics concept Shape structure (for example the GetShapeKinematicProperties feature).
  • World: defines physics concept Shape structure (for example the RetrieveWorld feature in dartsim plugin).

  Note that these object classes are not mutually exclusive and could be defined in conjunction together to describe the Feature. There are also other uncommon objects defined depending on feature functionality, for example, the FreeGroup object in SetFreeGroupWorldPose feature. For more information about the physics concepts, please refer to Gazebo Physics simulation concepts tutorial.

• Implementation interfaces the actual physics engines API for the custom feature. It has InitiateEngine to trigger physics engine initiation to provide the required functionalities.

Moreover, we can define dependencies between custom Features:

• By default, a blank feature will not require any other features. If the custom feature does require some other set of features, then it should be inherited from FeatureWithRequirements class, and provided a list of the Features required.
• By default, a blank feature will not conflict with any other features. If the custom feature does conflict with some other set of features, then it should be inherited from FeatureWithConflicts class, and provided a list of the conflicting Features. The conflicting Features will not run at the same time when requested.

Define custom feature template

With the requirements and restrictions above, we first need to define a feature template for the custom feature. In this case, this feature will be responsible for retrieving world pointer from the physics engine. The template is placed in World.hh:

template <typename PolicyT, typename FeaturesT>
dart::simulation::WorldPtr RetrieveWorld::World<PolicyT, FeaturesT>
::GetDartsimWorld()
{
  return this->template Interface<RetrieveWorld>()
      ->GetDartsimWorld(this->identity);
}

The RetrieveWorld feature retrieves world pointer from physics engine, so we will use the World entity inherited from Feature::World and declare the member function GetDartsimWorld. Then we substantiate the virtual Implementation member function by overriding in the actual implementation of the custom feature RetrieveWorld later.

Finally, we implement the World entity's member function GetDartsimWorld to call the Implementation class's member function GetDartsimWorld via Entity::Interface convenience function for querying the feature Implementation object.

The newly defined feature template is placed in an /include folder shown in the following structure:

dartsim
├── worlds
├── src
│    ├── CustomFeatures.hh
│    ├── CustomFeatures.cc
|    ├──  World.hh
│    └── ...
└── CMakeLists.txt

We put this custom feature template in dartsim, and the next step is to implement the RetrieveWorld feature function using Dartsim API.

Implement the custom feature

After defining the feature template, we can add it to a custom FeatureList:

using CustomFeatureList = FeatureList<
  RetrieveWorld
>;

The custom feature RetrieveWorld is added to CustomFeatureList, other custom features could also be added here. The CustomFeatures "FeatureList" here uses data structures and classes from:

• Base.hh, which defines structures that contain information to create Model, Joint, Link, and Shape objects in Dartsim API. They act as an interface between Gazebo Physics Library and the actual physics engine.
• Implements3d for implementing the custom feature with FeaturePolicy3d ("FeaturePolicy" of 3 dimensions and scalar type double).

We will then implement the actual function with Dartsim API in CustomFeatures.cc to override the member function declared in the custom feature header file:

dart::simulation::WorldPtr CustomFeatures::GetDartsimWorld(
    const Identity &_worldID)
{
  return this->worlds.at(_worldID);
}

Here, we implement the behavior of GetDartsimWorld with Dartsim API to return the world pointer from EntityStorage object storing world pointers of dartsim in Base class.

In the end, we add the implemented CustomFeatures "FeatureList" together with other FeatureList to final DartsimFeatures "FeatureList" as in dartsim/src/plugin.cc (please see the Implement a physics feature tutorial for registering the plugin to Gazebo Physics).

The folder structure is shown below:

dartsim
├── worlds
├── src
│    ├── CustomFeatures.hh
│    ├── CustomFeatures.cc
│    ├── ...
└── CMakeLists.txt