Case Study: Using the JointForceCmd Component

We will show how to use one of the components, 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, the gz::sim::systems::ApplyJointForce system (source code), and others.

The corresponding world SDF is apply_joint_force.sdf, which you can look at in Gazebo:

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 gz::sim::Joint entity in this case. It is declared as a member variable:

  public: Entity jointEntity;

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 <model> in the SDF, so at the time Configure() is called, it has access to a model entity from which we can extract a gz::sim::Model:

  public: Model model{kNullEntity};

void ApplyJointForce::Configure(const Entity &_entity,
    const std::shared_ptr<const sdf::Element> &_sdf,
    EntityComponentManager &_ecm,
    EventManager &/*_eventMgr*/)
{
  this->dataPtr->model = Model(_entity);

Using the Model object, we can find the joint by its name, when PreUpdate() is called. That gives us a Joint entity:

  if (this->dataPtr->jointEntity == kNullEntity)
  {
    this->dataPtr->jointEntity =
        this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName);
  }

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:

  auto force = _ecm.Component<components::JointForceCmd>(
      this->dataPtr->jointEntity);

Create it if it does not exist yet, and modify it:

  if (force == nullptr)
  {
    _ecm.CreateComponent(
        this->dataPtr->jointEntity,
        components::JointForceCmd({this->dataPtr->jointForceCmd}));
  }
  else
  {
    force->Data()[0] += this->dataPtr->jointForceCmd;
  }

where the scalar joint force command is declared as a member variable:

  public: double jointForceCmd;

and a callback function allows the user to specify a force on a topic:

  auto topic = transport::TopicUtils::AsValidTopic("/model/" +
      this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
      "/cmd_force");

  this->dataPtr->node.Subscribe(topic, &ApplyJointForcePrivate::OnCmdForce,
                                this->dataPtr.get());

void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg)
{
  std::lock_guard<std::mutex> lock(this->jointForceCmdMutex);
  this->jointForceCmd = _msg.data();
}

You can test this by issuing a force command to the topic:

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.