CMakeLists.txt

@@ -38,3 +38,5 @@ target_include_directories(control_loop PUBLIC
   $<INSTALL_INTERFACE:include>)
 target_link_libraries(control_loop gz_transport_hw_interface)
 
+
+add_subdirectory(plugin)

cmake @ eb7e4240

-Subproject commit 2ede15d1cb9d66401ba96788444ad64c44ffccd8
+Subproject commit eb7e42406ef48d66acb3a789917688363a0bf0b0

plugin/ApplyJointsForces.cc

+/*
+ * Copyright (C) 2019 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 "ApplyJointsForces.hh"
+
+#include <gz/msgs/double.pb.h>
+
+#include <string>
+
+#include <gz/common/Profiler.hh>
+#include <gz/plugin/Register.hh>
+#include <gz/transport/Node.hh>
+
+#include "gz/sim/components/JointForceCmd.hh"
+#include "gz/sim/components/Name.hh"
+#include "gz/sim/components/Joint.hh"
+#include "gz/sim/components/JointType.hh"
+#include "gz/sim/Model.hh"
+#include "gz/sim/Util.hh"
+
+#include <gz/msgs/map_named_joint_forces.pb.h>
+
+using namespace gz;
+using namespace sim;
+using namespace systems;
+
+
+/// \brief A single 1-axis joint that is controlled by JointTrajectoryController
+/// plugin
+class ActuatedJoint
+{
+  /// \brief Default contructor
+  public: ActuatedJoint() = default;
+
+  /// \brief Constructor that properly configures the actuated joint
+  /// \param[in] _entity Entity of the joint
+  /// \param[in] _params All parameters of the joint required for its
+  /// configuration
+  public: ActuatedJoint(const Entity &_entity);
+
+  /// \brief Setup components required for control of this joint
+  /// \param[in,out] _ecm Gazebo Entity Component Manager
+  public: void SetupComponents(
+              gz::sim::EntityComponentManager &_ecm) const;
+
+  /// \brief Set Torque of the joint that the controller will attempt to reach
+  /// \param[in] _jointIndex Index of the joint, used to determine what index
+  /// of `_targetPoint` to use
+  public: void SetJointForce(
+              const gz::msgs::Double &JointTorqueForce);
+
+  /// \brief Update command force that is applied on the joint
+  /// \param[in,out] _ecm Gazebo Entity Component Manager
+  /// \param[in] _dt Time difference to update for
+  public: void Update(gz::sim::EntityComponentManager &_ecm,
+                      const std::chrono::steady_clock::duration &_dt);
+
+  /// \brief Entity of the joint
+  public: Entity entity;
+
+  /// \brief Joint name
+  public: std::string jointName;
+
+  /// \brief The jointForceCmd.
+  double jointForceCmd;
+
+};
+
+class gz::sim::systems::ApplyJointsForcesPrivate
+{
+
+    /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no
+  /// joint names are specified in the plugin configuration, all valid 1-axis
+  /// joints are returned
+  /// \param[in] _entity Entity of the model that the plugin is being
+  /// configured for
+  /// \param[in] _sdf SDF reference used to determine enabled joints
+  /// \param[in] _ecm Gazebo Entity Component Manager
+  /// \return List of entities containinig all enabled joints
+  public: std::vector<Entity> GetEnabledJoints(
+              const Entity &_entity,
+              const std::shared_ptr<const sdf::Element> &_sdf,
+              EntityComponentManager &_ecm) const;
+
+
+  /// \brief Reset internals of the plugin, without affecting already created
+  /// components
+  public: void Reset();
+
+  /// \brief Callback for joint force subscription
+  /// \param[in] _msg Joint force message
+ public: void CallbackCmdForce(const gz::msgs::MapNamedJointForces &_msg);
+
+  /// \brief Gazebo communication node.
+  public: transport::Node node;
+
+  //! [jointEntityDeclaration]
+
+  /// \brief Commanded joint force
+  //! [forceDeclaration]
+  public: std::map<std::string, ActuatedJoint> actuatedJoints;
+  //! [forceDeclaration]
+
+  /// \brief mutex to protect jointForceCmd
+  public: std::mutex jointForceCmdMutex;
+
+  /// \brief Model interface
+  //! [modelDeclaration]
+  public: Model model{kNullEntity};
+  //! [modelDeclaration]<q
+
+  public: 
+  template <typename T>
+    std::vector<T> Parse(
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    const std::string &_parameterName) const;
+
+};
+
+//////////////////////////////////////////////////
+ApplyJointsForces::ApplyJointsForces()
+  : dataPtr(std::make_unique<ApplyJointsForcesPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+template <typename T>
+std::vector<T> ApplyJointsForcesPrivate::Parse(
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    const std::string &_parameterName) const
+{
+  std::vector<T> output;
+
+  if (_sdf->HasElement(_parameterName))
+  {
+    auto param = _sdf->FindElement(_parameterName);
+    while (param)
+    {
+      output.push_back(param->Get<T>());
+      param = param->GetNextElement(_parameterName);
+    }
+  }
+  return output;
+}
+
+//////////////////////////////////////////////////
+//! [Configure]
+void ApplyJointsForces::Configure(const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &_ecm,
+    EventManager &/*_eventMgr*/)
+{
+  this->dataPtr->model = Model(_entity);
+  //! [Configure]
+
+  if (!this->dataPtr->model.Valid(_ecm))
+  {
+    gzerr << "ApplyJoinstForces plugin should be attached to a model entity. "
+           << "Failed to initialize." << std::endl;
+    return;
+  }
+  gzmsg << "[ApplyJointsForces] Setting up controller for ["
+         << this->dataPtr->model.Name(_ecm) << "(Entity=" << _entity << ")].\n";
+
+  auto sdfClone = _sdf->Clone();
+
+  // Get list of enabled joints
+  const auto enabledJoints = this->dataPtr->GetEnabledJoints(_entity,
+                                                             _sdf,
+                                                             _ecm);
+
+
+  // Iterate over all enabled joints and create/configure them
+  for (const auto &jointEntity : enabledJoints)
+  {
+    const auto jointName =
+        _ecm.Component<components::Name>(jointEntity)->Data();
+    this->dataPtr->actuatedJoints[jointName] =
+        ActuatedJoint(jointEntity);
+    gzmsg << "[ApplyJointsForces] Configured joint ["
+           << jointName << "(Entity=" << jointEntity << ")].\n";
+  }
+
+  // Make sure at least one joint is configured
+  if (this->dataPtr->actuatedJoints.empty())
+  {
+    gzerr << "[JointTrajectoryController] Failed to initialize because ["
+           << this->dataPtr->model.Name(_ecm) << "(Entity=" << _entity
+           << ")] has no supported joints.\n";
+    return;
+  }
+
+
+  // Subscribe to commands
+  //! [cmdTopic]
+  auto topic = _sdf->Get<std::string>("topic");
+  //! [cmdTopic]
+  if (topic.empty())
+  {
+    transport::TopicUtils::AsValidTopic("/model/" +
+      this->dataPtr->model.Name(_ecm) + "/joints/cmd_forces");
+  }
+
+  // Make sure the topic is valid
+  const auto validApplyJointsForcesTopic = transport::TopicUtils::AsValidTopic(
+      topic);
+  if (validApplyJointsForcesTopic.empty())
+  {
+    gzerr << "[ApplyJointsForces] Cannot subscribe to invalid topic ["
+           << topic << "].\n";
+    return;
+  }
+ // Subscribe
+  gzmsg << "[ApplyJointsForces] Subscribing to ApplyJointsForces"
+            " commands on topic [" << validApplyJointsForcesTopic << "].\n";
+ 
+  //! [cmdSub]
+  this->dataPtr->node.Subscribe(validApplyJointsForcesTopic,
+                                &ApplyJointsForcesPrivate::CallbackCmdForce,
+                                this->dataPtr.get());
+  //! [cmdSub]
+
+  gzmsg << "ApplyJointsForces subscribing to Double messages on [" << topic
+         << "]" << std::endl;
+}
+
+//////////////////////////////////////////////////
+void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
+    EntityComponentManager &_ecm)
+{
+  GZ_PROFILE("ApplyJointsForces::PreUpdate");
+
+  // \TODO(anyone) Support rewind
+  if (_info.dt < std::chrono::steady_clock::duration::zero())
+  {
+    gzwarn << "Detected jump back in time ["
+           << std::chrono::duration<double>(_info.dt).count()
+           << "s]. System may not work properly." << std::endl;
+
+  }
+
+
+  // Iterator over all the actuated joints
+  for (auto an_actuated_joint = this->dataPtr->actuatedJoints.begin();
+       an_actuated_joint != this->dataPtr->actuatedJoints.end();
+       an_actuated_joint++)
+  {
+    // If the joint hasn't been identified yet, look for it
+    //! [findJoint]
+    if (an_actuated_joint->second.entity == kNullEntity)
+    {
+      an_actuated_joint->second.entity =
+          this->dataPtr->model.JointByName(_ecm, an_actuated_joint->first);
+    }
+    //! [findJoint]
+    
+    if (an_actuated_joint->second.entity == kNullEntity)
+      continue;
+    
+
+    // Update joint force
+    //! [jointForceComponent]
+    auto force = _ecm.Component<components::JointForceCmd>(
+        an_actuated_joint->second.entity);
+    //! [jointForceComponent]
+    
+    std::lock_guard<std::mutex> lock(this->dataPtr->jointForceCmdMutex);
+    
+    //! [modifyComponent]
+    if (force == nullptr)
+    {
+      _ecm.CreateComponent(
+          an_actuated_joint->second.entity,
+          components::JointForceCmd({an_actuated_joint->second.jointForceCmd}));
+    }
+    else
+    {
+      force->Data()[0] += an_actuated_joint->second.jointForceCmd;
+    }
+  }
+    //! [modifyComponent]
+}
+
+//////////////////////////////////////////////////
+//! [setForce]
+void ApplyJointsForcesPrivate::CallbackCmdForce(
+    const gz::msgs::MapNamedJointForces &mnjf_msg)
+{
+  std::lock_guard<std::mutex> lock(this->jointForceCmdMutex);
+  for ( auto mnjf_it =  mnjf_msg.jointforces().begin();
+        mnjf_it !=  mnjf_msg.jointforces().end();
+        mnjf_it++)
+  {
+    actuatedJoints[mnjf_it->first].jointForceCmd = mnjf_it->second;
+  }
+}
+//! [setForce]
+
+std::vector<Entity> ApplyJointsForcesPrivate::GetEnabledJoints(
+    const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &_ecm) const
+{
+  std::vector<Entity> output;
+
+  // Get list of user-enabled joint names. If empty, enable all 1-axis joints
+  const auto enabledJoints = Parse<std::string>(_sdf,
+                                                "joint_name");
+
+  // Get list of joint entities of the model
+  // If there are joints explicitely enabled by the user, get only those
+  std::vector<Entity> jointEntities;
+  if (!enabledJoints.empty())
+  {
+    for (const auto &enabledJointName : enabledJoints)
+    {
+      auto enabledJointEntity = _ecm.ChildrenByComponents(
+          _entity, components::Joint(), components::Name(enabledJointName));
+      // Check that model has exactly one joint that matches the name
+      if (enabledJointEntity.empty())
+      {
+        gzerr << "[ApplyJointsForces] Model does not contain joint ["
+               << enabledJointName << "], which was explicitly enabled.\n";
+        continue;
+      }
+      else if (enabledJointEntity.size() > 1)
+      {
+        gzwarn << "[ApplyJointsForces] Model has "
+                << enabledJointEntity.size() << " duplicate joints named ["
+                << enabledJointName << "]. Only the first (Entity="
+                << enabledJointEntity[0] << ") will be configured.\n";
+      }
+      // Add entity to the list of enabled joints
+      jointEntities.push_back(enabledJointEntity[0]);
+    }
+  }
+  else
+  {
+    jointEntities = _ecm.ChildrenByComponents(_entity, components::Joint());
+  }
+
+  // Iterate over all joints and verify whether they can be enabled or not
+  for (const auto &jointEntity : jointEntities)
+  {
+    const auto jointName = _ecm.Component<components::Name>(
+                                   jointEntity)->Data();
+
+    // Ignore duplicate joints
+    for (const auto &actuatedJoint : this->actuatedJoints)
+    {
+      if (actuatedJoint.second.entity == jointEntity)
+      {
+        gzwarn << "[ApplyJointsForces] Ignoring duplicate joint ["
+                << jointName << "(Entity=" << jointEntity << ")].\n";
+        continue;
+      }
+    }
+
+    // Make sure the joint type is supported, i.e. it has exactly one
+    // actuated axis
+    const auto *jointType = _ecm.Component<components::JointType>(jointEntity);
+    switch (jointType->Data())
+    {
+      case sdf::JointType::PRISMATIC:
+      case sdf::JointType::REVOLUTE:
+      case sdf::JointType::CONTINUOUS:
+      case sdf::JointType::GEARBOX:
+      {
+        // Supported joint type
+        break;
+      }
+      case sdf::JointType::FIXED:
+      {
+        gzdbg << "[ApplyJointsForces] Fixed joint [" << jointName
+               << "(Entity=" << jointEntity << ")] is skipped.\n";
+        continue;
+      }
+      case sdf::JointType::REVOLUTE2:
+      case sdf::JointType::SCREW:
+      case sdf::JointType::BALL:
+      case sdf::JointType::UNIVERSAL:
+      {
+        gzwarn << "[ApplyJointsForces] Joint [" << jointName
+                << "(Entity=" << jointEntity
+                << ")] is of unsupported type. Only joints with a single axis"
+                   " are supported.\n";
+        continue;
+      }
+      default:
+      {
+        gzwarn << "[ApplyJointsForces] Joint [" << jointName
+                << "(Entity=" << jointEntity << ")] is of unknown type.\n";
+        continue;
+      }
+    }
+    output.push_back(jointEntity);
+  }
+
+  return output;
+}
+
+    
+
+GZ_ADD_PLUGIN(ApplyJointsForces,
+                    System,
+                    ApplyJointsForces::ISystemConfigure,
+                    ApplyJointsForces::ISystemPreUpdate)
+
+GZ_ADD_PLUGIN_ALIAS(ApplyJointsForces,
+                    "gz::sim::systems::ApplyJointsForces")

plugin/ApplyJointsForces.hh

+/*
+ * Copyright (C) 2019 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.
+ *
+ */
+#ifndef GZ_SIM_SYSTEMS_APPLYJOINTSFORCES_HH_
+#define GZ_SIM_SYSTEMS_APPLYJOINTSFORCES_HH_
+
+#include <gz/sim/System.hh>
+#include <memory>
+#include <vector>
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems
+{
+  // Forward declaration
+  class ApplyJointsForcesPrivate;
+
+  /// \brief This system applies a force to the first axis of a specified joint.
+  ///
+  /// ## Components
+  ///
+  /// This system uses the following components:
+  ///
+  /// - gz::sim::components::JointForceCmd: A std::vector of force commands
+  ///   of type `double`. Only element `[0]` is used, for applying force to
+  ///   the specified joint.
+  class ApplyJointsForces
+      : public System,
+        public ISystemConfigure,
+        public ISystemPreUpdate
+  {
+    /// \brief Constructor
+    public: ApplyJointsForces();
+
+    /// \brief Destructor
+    public: ~ApplyJointsForces() override = default;
+
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) override;
+
+    // Documentation inherited
+    public: void PreUpdate(
+                const gz::sim::UpdateInfo &_info,
+                gz::sim::EntityComponentManager &_ecm) override;
+
+    /// \brief Private data pointer
+    private: std::unique_ptr<ApplyJointsForcesPrivate> dataPtr;
+
+  };
+  }
+}
+}
+}
+
+#endif

plugin/CMakeLists.txt

+add_subdirectory(msgs)
+
+find_package(gz_sim_vendor REQUIRED)
+find_package(gz-sim REQUIRED)
+
+find_package(gz_plugin_vendor REQUIRED)
+find_package(gz-plugin REQUIRED)
+
+set(gz_gep_tools_plugin ApplyJointsForces)
+
+add_library(${gz_gep_tools_plugin}-system SHARED
+  ApplyJointsForces.cc)
+
+target_link_libraries(${gz_gep_tools_plugin}-system PRIVATE
+  gz-sim::gz-sim
+  gz-plugin::register
+  gz_gep_tools-msgs
+)
+
+# Install directories
+install(TARGETS ${gz_gep_tools_plugin}-system
+  DESTINATION lib
+)

plugin/msgs/CMakeLists.txt

+#============================================================================
+# Find gz-cmake
+#============================================================================
+find_package(gz-cmake3 REQUIRED)
+
+find_package(gz-msgs10 REQUIRED)
+# Define a variable 'GZ_MSGS_VER' holding the version number
+set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
+
+# Define a variable 'MSGS_PROTOS' listing the .proto files
+set(MSGS_PROTOS
+   ${CMAKE_CURRENT_SOURCE_DIR}/gz/msgs/map_named_joint_forces.proto
+)
+
+# Call 'gz_msgs_generate_messages()' to process the .proto files
+gz_msgs_generate_messages(
+  # The cmake target to be generated for libraries/executables to link
+  TARGET msgs
+  # The protobuf package to generate (Typically based on the path)
+  PROTO_PACKAGE "gz.msgs"
+  # The path to the base directory of the proto files
+  # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto)
+  MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}
+  # List of proto files to process
+  MSGS_PROTOS ${MSGS_PROTOS}
+  # Depenency on gz-msgs
+  DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER}
+)
+
+# install(TARGETS 
+#          LIBRARY DESTINATION lib
+#          ARCHIVE DESTINATION lib
+#          RUNTIME DESTINATION bin
+# )

plugin/msgs/gz/msgs/map_named_joint_forces.proto

+syntax = "proto3";
+
+package gz.msgs;
+
+message MapNamedJointForces {
+  map<string, double> JointForces= 1;
+}
\ No newline at end of file