Actuators message for JointPositionController.

src/systems/joint_position_controller/JointPositionController.cc

@@ -18,6 +18,7 @@
 
 #include "JointPositionController.hh"
 
+#include <gz/msgs/actuators.pb.h>
 #include <gz/msgs/double.pb.h>
 
 #include <string>
@@ -29,6 +30,7 @@
 #include <gz/plugin/Register.hh>
 #include <gz/transport/Node.hh>
 
+#include "gz/sim/components/Actuators.hh"
 #include "gz/sim/components/Joint.hh"
 #include "gz/sim/components/JointForceCmd.hh"
 #include "gz/sim/components/JointVelocityCmd.hh"
@@ -46,6 +48,10 @@ class gz::sim::systems::JointPositionControllerPrivate
   /// \param[in] _msg Position message
   public: void OnCmdPos(const msgs::Double &_msg);
 
+  /// \brief Callback for actuator position subscription
+  /// \param[in] _msg Position message
+  public: void OnActuatorPos(const msgs::Actuators &_msg);
+
   /// \brief Gazebo communication node.
   public: transport::Node node;
 
@@ -58,12 +64,18 @@ class gz::sim::systems::JointPositionControllerPrivate
   /// \brief Commanded joint position
   public: double jointPosCmd{0.0};
 
+  /// \brief Index of position actuator.
+  public: int actuatorNumber = 0;
+
   /// \brief mutex to protect joint commands
   public: std::mutex jointCmdMutex;
 
   /// \brief Model interface
   public: Model model{kNullEntity};
 
+  /// \brief True if using Actuator msg to control joint position.
+  public: bool useActuatorMsg{false};
+
   /// \brief Position PID controller.
   public: math::PID posPid;
 
@@ -190,9 +202,26 @@ void JointPositionController::Configure(const Entity &_entity,
     this->dataPtr->jointPosCmd = _sdf->Get<double>("initial_position");
   }
 
+  if (_sdf->HasElement("use_actuator_msg") &&
+    _sdf->Get<bool>("use_actuator_msg"))
+  {
+    if (_sdf->HasElement("actuator_number"))
+    {
+      this->dataPtr->actuatorNumber =
+        _sdf->Get<int>("actuator_number");
+      this->dataPtr->useActuatorMsg = true;
+    }
+    else
+    {
+      gzerr << "Please specify an actuator_number" <<
+        "to use Actuator position message control." << std::endl;
+    }
+  }
+
   // Subscribe to commands
   std::string topic;
-  if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")))
+  if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))
+    && (!this->dataPtr->useActuatorMsg))
   {
     topic = transport::TopicUtils::AsValidTopic("/model/" +
         this->dataPtr->model.Name(_ecm) + "/joint/" +
@@ -206,6 +235,18 @@ void JointPositionController::Configure(const Entity &_entity,
       return;
     }
   }
+  if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic"))
+    && (this->dataPtr->useActuatorMsg))
+  {
+    topic = transport::TopicUtils::AsValidTopic("/actuators");
+    if (topic.empty())
+    {
+      gzerr << "Failed to create Actuator topic for joint ["
+            << this->dataPtr->jointNames[0]
+            << "]" << std::endl;
+      return;
+    }
+  }
   if (_sdf->HasElement("sub_topic"))
   {
     topic = transport::TopicUtils::AsValidTopic("/model/" +
@@ -235,8 +276,24 @@ void JointPositionController::Configure(const Entity &_entity,
       return;
     }
   }
-  this->dataPtr->node.Subscribe(
-      topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get());
+  if (this->dataPtr->useActuatorMsg)
+  {
+    this->dataPtr->node.Subscribe(topic,
+      &JointPositionControllerPrivate::OnActuatorPos,
+      this->dataPtr.get());
+
+    gzmsg << "JointPositionController subscribing to Actuator messages on ["
+      << topic << "]" << std::endl;
+  }
+  else
+  {
+    this->dataPtr->node.Subscribe(topic,
+      &JointPositionControllerPrivate::OnCmdPos,
+      this->dataPtr.get());
+
+    gzmsg << "JointPositionController subscribing to Double messages on ["
+      << topic << "]" << std::endl;
+  }
 
   gzdbg << "[JointPositionController] system parameters:" << std::endl;
   gzdbg << "p_gain: ["     << p         << "]"            << std::endl;
@@ -426,6 +483,20 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg)
   this->jointPosCmd = _msg.data();
 }
 
+void JointPositionControllerPrivate::OnActuatorPos(const msgs::Actuators &_msg)
+{
+  std::lock_guard<std::mutex> lock(this->jointCmdMutex);
+  if (this->actuatorNumber > _msg.position_size() - 1)
+  {
+    gzerr << "You tried to access index " << this->actuatorNumber
+      << " of the Actuator position array which is of size "
+      << _msg.position_size() << std::endl;
+    return;
+  }
+
+  this->jointPosCmd = static_cast<double>(_msg.position(this->actuatorNumber));
+}
+
 GZ_ADD_PLUGIN(JointPositionController,
                     System,
                     JointPositionController::ISystemConfigure,

src/systems/joint_position_controller/JointPositionController.hh

@@ -52,6 +52,13 @@ namespace systems
   /// `<joint_index>` Axis of the joint to control. Optional parameter.
   ///  The default value is 0.
   ///
+  /// `<use_actuator_msg>` True to enable the use of actutor message
+  /// for position command. Relies on `<actuator_number>` for the
+  /// index of the position actuator and defaults to topic "/actuators".
+  ///
+  /// `<actuator_number>` used with `<use_actuator_commands>` to set
+  /// the index of the position actuator.
+  ///
   /// `<p_gain>` The proportional gain of the PID. Optional parameter.
   ///  The default value is 1.
   ///

test/integration/joint_position_controller_system.cc

@@ -19,6 +19,7 @@
 #include <gtest/gtest.h>
 
 #include <gz/msgs/double.pb.h>
+#include <gz/msgs/actuators.pb.h>
 
 #include <gz/common/Console.hh>
 #include <gz/common/Util.hh>
@@ -128,6 +129,84 @@ TEST_F(JointPositionControllerTestFixture,
   EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
 }
 
+/////////////////////////////////////////////////
+// Tests that the JointPositionController accepts joint position commands
+// See https://github.com/gazebosim/gz-sim/issues/1175
+TEST_F(JointPositionControllerTestFixture,
+       GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionActuatorsCommand))
+{
+  using namespace std::chrono_literals;
+
+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(common::joinPaths(
+      PROJECT_SOURCE_PATH, "test", "worlds",
+      "joint_position_controller_actuators.sdf"));
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  server.SetUpdatePeriod(0ns);
+
+  const std::string jointName = "j1";
+
+  test::Relay testSystem;
+  std::vector<double> currentPosition;
+  testSystem.OnPreUpdate(
+      [&](const UpdateInfo &, EntityComponentManager &_ecm)
+      {
+        auto joint = _ecm.EntityByComponents(components::Joint(),
+                                             components::Name(jointName));
+        // Create a JointPosition component if it doesn't exist. This signals
+        // physics system to populate the component
+        if (nullptr == _ecm.Component<components::JointPosition>(joint))
+        {
+          _ecm.CreateComponent(joint, components::JointPosition());
+        }
+      });
+
+  testSystem.OnPostUpdate([&](const UpdateInfo &,
+                              const EntityComponentManager &_ecm)
+      {
+        _ecm.Each<components::Joint, components::Name,
+                  components::JointPosition>(
+            [&](const Entity &,
+                const components::Joint *,
+                const components::Name *_name,
+                const components::JointPosition *_position) -> bool
+            {
+              EXPECT_EQ(_name->Data(), jointName);
+              currentPosition = _position->Data();
+              return true;
+            });
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+
+  const std::size_t initIters = 10;
+  server.Run(true, initIters, false);
+  EXPECT_NEAR(0, currentPosition.at(0), TOL);
+
+  // Publish command and check that the joint position is set
+  transport::Node node;
+  auto pub = node.Advertise<msgs::Actuators>(
+      "/actuators");
+
+  const double targetPosition{1.0};
+  msgs::Actuators msg;
+  msg.add_position(targetPosition);
+
+  pub.Publish(msg);
+  // Wait for the message to be published
+  std::this_thread::sleep_for(100ms);
+
+  const std::size_t testIters = 3000;
+  server.Run(true, testIters , false);
+
+  EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
+}
+
 /////////////////////////////////////////////////
 // Tests that the JointPositionController accepts joint position commands
 TEST_F(JointPositionControllerTestFixture,

test/worlds/joint_position_controller_actuators.sdf

@@ -0,0 +1,123 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <physics name="fast" type="ignored">
+      <real_time_factor>0</real_time_factor>
+    </physics>
+
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="joint_position_controller_test">
+      <pose>0 0 0.005 0 0 0</pose>
+      <link name="base_link">
+        <pose>0.0 0.0 0.0 0 0 0</pose>
+        <inertial>
+          <inertia>
+            <ixx>2.501</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>2.501</iyy>
+            <iyz>0</iyz>
+            <izz>5</izz>
+          </inertia>
+          <mass>120.0</mass>
+        </inertial>
+        <visual name="base_visual">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.01</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name="base_collision">
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 0.5 0.01</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+      <link name="rotor">
+        <pose>0.0 0.0 1.0 0.0 0 0</pose>
+        <inertial>
+          <pose>0.0 0.0 0.0 0 0 0</pose>
+          <inertia>
+            <ixx>0.032</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.032</iyy>
+            <iyz>0</iyz>
+            <izz>0.00012</izz>
+          </inertia>
+          <mass>0.6</mass>
+        </inertial>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.25 0.25 0.05</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.25 0.25 0.05</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <joint name="j1" type="revolute">
+        <pose>0 0 -0.5 0 0 0</pose>
+        <parent>base_link</parent>
+        <child>rotor</child>
+        <axis>
+          <xyz>0 0 1</xyz>
+        </axis>
+      </joint>
+      <plugin
+        filename="gz-sim-joint-position-controller-system"
+        name="gz::sim::systems::JointPositionController">
+        <joint_name>j1</joint_name>
+        <use_actuator_msg>true</use_actuator_msg>
+        <actuator_number>0</actuator_number>
+      </plugin>
+    </model>
+  </world>
+</sdf>