Skip to content

Commit

Permalink
apply_joint_force: Simplified using ComponentDefault
Browse files Browse the repository at this point in the history
Signed-off-by: Martin Pecka <[email protected]>
  • Loading branch information
peci1 authored Sep 16, 2024
1 parent 3765579 commit e41eb49
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 17 deletions.
15 changes: 3 additions & 12 deletions src/systems/apply_joint_force/ApplyJointForce.cc
Original file line number Diff line number Diff line change
Expand Up @@ -156,23 +156,14 @@ void ApplyJointForce::PreUpdate(const UpdateInfo &_info,

// Update joint force
//! [jointForceComponent]
auto force = _ecm.Component<components::JointForceCmd>(
this->dataPtr->jointEntity);
auto force = _ecm.ComponentDefault<components::JointForceCmd>(
this->dataPtr->jointEntity, {0.0});
//! [jointForceComponent]

std::lock_guard<std::mutex> lock(this->dataPtr->jointForceCmdMutex);

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

Expand Down
12 changes: 7 additions & 5 deletions tutorials/component_jointforcecmd.md
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,12 @@ 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),
[joint integration test](https://github.com/gazebosim/gz-sim/blob/gz-sim9/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)),
([source code](https://github.com/gazebosim/gz-sim/tree/gz-sim9/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:
The corresponding world SDF is [`apply_joint_force.sdf`](https://github.com/gazebosim/gz-sim/blob/gz-sim9/examples/worlds/apply_joint_force.sdf), which you can look at in Gazebo:

```bash
gz sim apply_joint_force.sdf
Expand Down Expand Up @@ -56,11 +56,13 @@ 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:
In `PreUpdate()`, look for the component. If it does not exist yet, create it,
setting its default value to `{0.0}` (which means, in this particular case,
a 1-element `std::vector<double>` with value 0.0 in its only element):

\snippet src/systems/apply_joint_force/ApplyJointForce.cc jointForceComponent

Create it if it does not exist yet, and modify it:
Now modify the value stored in the component:

\snippet src/systems/apply_joint_force/ApplyJointForce.cc modifyComponent

Expand Down

0 comments on commit e41eb49

Please sign in to comment.