PID effort control of joints (+mimic)

ign_ros2_control/CMakeLists.txt

@@ -20,10 +20,15 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(controller_manager REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
+generate_parameter_library(ign_ros2_control_parameters
+  src/ign_ros2_control_parameters.yaml
+)
+
 if("$ENV{IGNITION_VERSION}" STREQUAL "citadel")
   find_package(ignition-gazebo3 REQUIRED)
   set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR})
@@ -39,10 +44,30 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
   set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
   message(STATUS "Compiling against Ignition Fortress")
 
+  find_package(ignition-cmake2 REQUIRED)
+  find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+  set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+  add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
+  target_link_libraries(mimic-joint-system
+    PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+    PRIVATE ignition-gazebo6::ignition-gazebo6)
+  install(TARGETS mimic-joint-system
+          DESTINATION lib)
+
 else()
   find_package(ignition-gazebo6 REQUIRED)
   set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
   message(STATUS "Compiling against Ignition Fortress")
+
+  find_package(ignition-cmake2 REQUIRED)
+  find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+  set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+  add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
+  target_link_libraries(mimic-joint-system
+    ignition-gazebo${IGN_GAZEBO_VER}::core
+    ignition-plugin${IGN_PLUGIN_VER}::register)
+  install(TARGETS mimic-joint-system
+          DESTINATION lib)
 endif()
 
 find_package(ignition-plugin1 REQUIRED)
@@ -58,6 +83,7 @@ target_link_libraries(${PROJECT_NAME}-system
   ignition-gazebo${IGN_GAZEBO_VER}::core
   ignition-plugin${IGN_PLUGIN_VER}::register
 )
+target_link_libraries(${PROJECT_NAME}-system ign_ros2_control_parameters)
 ament_target_dependencies(${PROJECT_NAME}-system
   ament_index_cpp
   controller_manager
@@ -73,6 +99,8 @@ ament_target_dependencies(${PROJECT_NAME}-system
 add_library(ign_hardware_plugins SHARED
   src/ign_system.cpp
 )
+target_include_directories(ign_hardware_plugins PRIVATE include)
+target_link_libraries(ign_hardware_plugins ign_ros2_control_parameters)
 ament_target_dependencies(ign_hardware_plugins
   rclcpp_lifecycle
   hardware_interface
@@ -82,9 +110,14 @@ target_link_libraries(ign_hardware_plugins
   ignition-gazebo${IGN_GAZEBO_VER}::core
 )
 
+install(DIRECTORY include/
+  DESTINATION include
+)
+
 ## Install
 install(TARGETS
-  ign_hardware_plugins
+  ign_hardware_plugins ign_ros2_control_parameters
+  EXPORT export_ign_ros2_control
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -98,7 +131,9 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins)
-
+ament_export_targets(
+  export_ign_ros2_control HAS_LIBRARY_TARGET
+)
 # Install directories
 install(TARGETS ${PROJECT_NAME}-system
   DESTINATION lib

ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh

@@ -0,0 +1,93 @@
+/*
+ * Copyright 2023 Open Source Robotics Foundation, Inc.
+ *
+ * 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.
+ *
+*/
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Lovro Ivanov [email protected]
+ * \date    2023-03-15
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_
+#define IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_
+
+//! [header]
+#include <memory>
+
+#include <ignition/gazebo/System.hh>
+
+// This is an example to go in your urdf (is that correct?)
+//! <plugin filename="mimic-joint-system" name="ign_ros2_control::MimicJointSystem">
+//!     <joint_name>joint_name</joint_name>
+//!     <mimic_joint_name>mimic_joint_name</mimic_joint_name>
+//!     <multiplier>1.0</multiplier>
+//!     <offset>0.0</offset>
+//!     <joint_index>0</joint_index>
+//!     <mimic_joint_index>0</mimic_joint_index>
+//!     <p_gain>100.0</p_gain>
+//!     <i_gain>0.1</i_gain>
+//!     <d_gain>0.0</d_gain>
+//!     <i_max>5.0</i_max>
+//!     <i_min>-5.0</i_min>
+//!     <cmd_max>500.0</cmd_max>
+//!     <cmd_min>-500.0</cmd_min>
+//!     <cmd_offset>0.0</cmd_offset>
+//!     <dead_zone>0.001</dead_zone>
+//!     <use_velocity_commands>false</use_velocity_commands>
+//! </plugin>
+
+namespace ign_ros2_control
+{
+  class MimicJointSystemPrivate;
+
+  class MimicJointSystem:
+    // This class is a system.
+    public ignition::gazebo::System,
+    public ignition::gazebo::ISystemConfigure,
+    // This class also implements the ISystemPreUpdate, ISystemUpdate,
+    // and ISystemPostUpdate interfaces.
+    public ignition::gazebo::ISystemPreUpdate,
+    public ignition::gazebo::ISystemUpdate,
+    public ignition::gazebo::ISystemPostUpdate
+  {
+    public: MimicJointSystem();
+
+    // Documentation inherited
+    public:  void Configure(const ignition::gazebo::Entity &_entity,
+                          const std::shared_ptr<const sdf::Element> &_sdf,
+                          ignition::gazebo::EntityComponentManager &_ecm,
+                          ignition::gazebo::EventManager &_eventMgr) override;
+
+    public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+                ignition::gazebo::EntityComponentManager &_ecm) override;
+
+    public: void Update(const ignition::gazebo::UpdateInfo &_info,
+                ignition::gazebo::EntityComponentManager &_ecm) override;
+
+    public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+                const ignition::gazebo::EntityComponentManager &_ecm) override;
+
+  private:
+      /// \brief Private data pointer
+      private: std::unique_ptr<MimicJointSystemPrivate> dataPtr;
+  };
+}
+//! [header]
+
+#endif

ign_ros2_control/include/ign_ros2_control/ign_system.hpp

@@ -22,9 +22,13 @@
 #include <vector>
 
 #include "ign_ros2_control/ign_system_interface.hpp"
+#include "ign_ros2_control_parameters.hpp"
+
+#include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
+
 namespace ign_ros2_control
 {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@@ -88,6 +92,15 @@ class IgnitionSystem : public IgnitionSystemInterface
 
   /// \brief Private data class
   std::unique_ptr<IgnitionSystemPrivate> dataPtr;
+
+  // Parameters from ROS for ign_ros2_control
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  rclcpp::Node::SharedPtr param_node_;
+  std::thread spin_thread_;
+  std::atomic<bool> stop_spin_ = false;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr exec_;
 };
 
 }  // namespace ign_ros2_control

ign_ros2_control/package.xml

@@ -22,6 +22,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>hardware_interface</depend>
   <depend>controller_manager</depend>
+  <depend>generate_parameter_library</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>