Add example about using joint control components

examples/plugin/joint_control_components/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR)
+
+project(JointControlPlugins)
+
+find_package(gz-plugin3 REQUIRED COMPONENTS register)
+set(GZ_PLUGIN_VER ${gz-plugin3_VERSION_MAJOR})
+
+find_package(gz-sim9 REQUIRED)
+set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR})
+
+add_library(PulseJointVelocityCommand SHARED PulseJointVelocityCommand.cc)
+target_link_libraries(PulseJointVelocityCommand
+  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  PRIVATE gz-sim${GZ_SIM_VER}::core)
+
+add_library(ResetJointVelocityNearPosition SHARED ResetJointVelocityNearPosition.cc)
+target_link_libraries(ResetJointVelocityNearPosition
+  PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+  PRIVATE gz-sim${GZ_SIM_VER}::core)

examples/plugin/joint_control_components/PulseJointVelocityCommand.cc

@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ */
+
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <gz/sim/Joint.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/System.hh>
+#include "gz/sim/components/JointForceCmd.hh"
+#include <gz/plugin/Register.hh>
+
+using namespace gz::sim;
+namespace joint_control
+{
+class PulseJointVelocityCommand: public System,
+                                 public ISystemConfigure,
+                                 public ISystemPreUpdate
+{
+  void Configure(const Entity &_entity,
+                 const std::shared_ptr<const sdf::Element> &_sdf,
+                 EntityComponentManager &_ecm, EventManager &) override
+  {
+    this->model = Model(_entity);
+
+    if (_sdf->HasElement("pulse_seconds"))
+    {
+      this->pulse_seconds = _sdf->Get<double>("pulse_seconds");
+    }
+
+    if (_sdf->HasElement("velocity_command"))
+    {
+      this->velocity_command = _sdf->Get<double>("velocity_command");
+    }
+  }
+
+  void PreUpdate(const UpdateInfo &_info,
+                 EntityComponentManager &_ecm) override
+  {
+    double time = std::chrono::duration<double>(_info.simTime).count();
+    bool pulse = static_cast<int>(std::floor(time / pulse_seconds)) % 2;
+
+    for (auto jointEntity : this->model.Joints(_ecm))
+    {
+      Joint joint(jointEntity);
+      if (pulse)
+      {
+        joint.SetVelocity(_ecm, {this->velocity_command});
+        _ecm.RemoveComponent<components::JointForceCmd>(jointEntity);
+      }
+      else
+      {
+        // Setting the JointForceCmd is required to disengage the velocity
+        // controller, otherwise the velocity controller remains engaged with a
+        // JointVelocityCmd of 0.
+        joint.SetForce(_ecm, {0.0});
+      }
+    }
+  }
+
+  private: Model model;
+  private: double pulse_seconds = 2.0;
+  // Internal variable stored in rad/s
+  private: double velocity_command = 0.4;
+};
+}  // namespace joint_control
+
+GZ_ADD_PLUGIN(joint_control::PulseJointVelocityCommand,
+              gz::sim::System,
+              joint_control::PulseJointVelocityCommand::ISystemConfigure,
+              joint_control::PulseJointVelocityCommand::ISystemPreUpdate)

examples/plugin/joint_control_components/README.md

@@ -0,0 +1,60 @@
+# Joint control components
+
+This example shows how to use three different
+[components](https://gazebosim.org/api/sim/9/namespacegz_1_1sim_1_1components.html)
+to create joint motion:
+
+* `JointForceCmd`: this component applies an effort to each degree of freedom
+  of the specified joint (force for translational joint axes and torque for
+  rotational joint axes).
+* `JointVelocityCmd`: this component specifies a desired velocity that a
+  controller attempts to match subject to joint effort limits.
+* `JointVelocityReset`: this component rewrites the joint velocity state,
+  in a similar way to setting a new initial condition.
+
+These components are demonstrated with the following plugins:
+
+* `ResetJointVelocityNearPosition`: this plugin sets the `JointVelocityReset`
+  component to a fixed value when the measured joint position is near a trigger
+  position. This can create a bouncing behavior of a pendulum if the trigger
+  position is the bottom stable equilibrium.
+* `PulseJointVelocityCommand`: this plugin applies pulses of joint velocity
+  commands as a square wave with a specified time period between pulses.
+
+## Build
+
+~~~
+cd examples/plugin/joint_control_components
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+This will build the `ResetJointVelocityNearPosition` and
+`PulseJointVelocityCommand` libraries under `build`.
+
+## Run
+
+Add the library to the path:
+
+~~~
+cd examples/plugin/joint_control_components
+export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/build
+~~~
+
+Then run a world that loads examples of these plugins.
+
+    gz sim -r -v 4 joint_control_components.sdf
+
+There should be 3 pendulums loaded.
+
+* On the far left, the pendulum has only the `ResetJointVelocityNearPosition`
+  with a trigger position at the downward stable eqilibrium point. This exhibits
+  a bouncing behavior.
+* On the far right, the pendulum has only the `PulseJointVelocityCommand`.
+  It starts out stationary, then alternates between moving at constant velocity
+  and swinging passively.
+* In the center, the pendulum has both plugins enabled and experiences pulses
+  of constant velocity alternating with passive swinging and bouncing at the
+  bottom.

examples/plugin/joint_control_components/ResetJointVelocityNearPosition.cc

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2025 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.
+ *
+ */
+
+#include <memory>
+#include <gz/sim/Joint.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/System.hh>
+#include <gz/plugin/Register.hh>
+
+using namespace gz::sim;
+namespace joint_control
+{
+class ResetJointVelocityNearPosition: public System,
+                                      public ISystemConfigure,
+                                      public ISystemPreUpdate
+{
+  void Configure(const Entity &_entity,
+                 const std::shared_ptr<const sdf::Element> &_sdf,
+                 EntityComponentManager &_ecm, EventManager &) override
+  {
+    this->model = Model(_entity);
+
+    for (auto jointEntity : this->model.Joints(_ecm))
+    {
+      Joint joint(jointEntity);
+      joint.EnablePositionCheck(_ecm, true);
+    }
+
+    if (_sdf->HasElement("position_tolerance"))
+    {
+      this->position_tolerance = _sdf->Get<double>("position_tolerance");
+    }
+
+    if (_sdf->HasElement("trigger_position"))
+    {
+      this->trigger_position = _sdf->Get<double>("trigger_position");
+    }
+
+    if (_sdf->HasElement("velocity_after_reset"))
+    {
+      this->velocity_after_reset = _sdf->Get<double>("velocity_after_reset");
+    }
+  }
+
+  void PreUpdate(const UpdateInfo &_info,
+                 EntityComponentManager &_ecm) override
+  {
+    auto jointEntities = this->model.Joints(_ecm);
+
+    for (auto jointEntity : jointEntities)
+    {
+      Joint joint(jointEntity);
+
+      auto optPositions = joint.Position(_ecm);
+      if (!optPositions.has_value() || optPositions->size() != 1)
+      {
+        // error message
+        continue;
+      }
+      double position = optPositions->at(0);
+
+      if (fabs(position - this->trigger_position) < position_tolerance)
+      {
+        joint.ResetVelocity(_ecm, {this->velocity_after_reset});
+      }
+    }
+  }
+
+  private: Model model;
+  // Internal variables stored in rad, rad/s
+  private: double trigger_position = 0.0;
+  private: double position_tolerance = 0.01;
+  private: double velocity_after_reset = 0.25;
+};
+}  // namespace joint_control
+
+GZ_ADD_PLUGIN(joint_control::ResetJointVelocityNearPosition,
+              gz::sim::System,
+              joint_control::ResetJointVelocityNearPosition::ISystemConfigure,
+              joint_control::ResetJointVelocityNearPosition::ISystemPreUpdate)