Add PID controller to control joint using effort

gazebo_ros2_control/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(controller_manager REQUIRED)
 find_package(gazebo_dev REQUIRED)
 find_package(gazebo_ros REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(control_toolbox REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
@@ -38,6 +39,7 @@ ament_target_dependencies(${PROJECT_NAME}
   hardware_interface
   pluginlib
   rclcpp
+  control_toolbox
   yaml_cpp_vendor
 )
 
@@ -48,6 +50,7 @@ ament_target_dependencies(gazebo_hardware_plugins
   angles
   gazebo_dev
   hardware_interface
+  control_toolbox
   rclcpp
 )
 
@@ -93,4 +96,4 @@ install(DIRECTORY include/${PROJECT_NAME}/
 
 pluginlib_export_plugin_description_file(gazebo_ros2_control gazebo_hardware_plugins.xml)
 
-ament_package()
+ament_package()

gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp

@@ -16,12 +16,16 @@
 #ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
 #define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
 
+#define VELOCITY_PID_PARAMS_PREFIX "vel_"
+#define POSITION_PID_PARAMS_PREFIX "pos_"
+
 #include <memory>
 #include <string>
 #include <vector>
 
 #include "angles/angles.h"
 
+#include "control_toolbox/pid.hpp"
 #include "gazebo_ros2_control/gazebo_system_interface.hpp"
 
 #include "std_msgs/msg/bool.hpp"
@@ -86,6 +90,9 @@ class GazeboSystem : public GazeboSystemInterface
     const hardware_interface::HardwareInfo & hardware_info,
     gazebo::physics::ModelPtr parent_model);
 
+  control_toolbox::Pid extractPID(std::string prefix, 
+      hardware_interface::ComponentInfo joint_info);
+
   /// \brief Private data class
   std::unique_ptr<GazeboSystemPrivate> dataPtr;
 };

gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp

@@ -79,6 +79,8 @@ class GazeboSystemInterface
     POSITION  = (1 << 0),
     VELOCITY  = (1 << 1),
     EFFORT    = (1 << 2),
+    VELOCITY_PID    = (1 << 3),
+    POSITION_PID    = (1 << 4),
   };
 
   typedef SafeEnum<enum ControlMethod_> ControlMethod;

gazebo_ros2_control/package.xml

@@ -27,6 +27,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>control_toolbox</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

gazebo_ros2_control/src/gazebo_system.cpp

@@ -83,6 +83,18 @@ class gazebo_ros2_control::GazeboSystemPrivate
   /// \brief vector with the current cmd joint effort
   std::vector<double> joint_effort_cmd_;
 
+  /// \brief Joint velocity PID utils
+  std::vector<control_toolbox::Pid> vel_pid;
+
+  /// \brief Joint position PID utils
+  std::vector<control_toolbox::Pid> pos_pid;
+
+  //  \brief control flag
+  std::vector<bool> is_pos_pid;
+
+  //  \brief control flag
+  std::vector<bool> is_vel_pid;
+
   /// \brief handles to the imus from within Gazebo
   std::vector<gazebo::sensors::ImuSensorPtr> sim_imu_sensors_;
 
@@ -139,6 +151,70 @@ bool GazeboSystem::initSim(
   return true;
 }
 
+control_toolbox::Pid GazeboSystem::extractPID(std::string prefix, hardware_interface::ComponentInfo joint_info)
+{
+    double kp;
+    double ki;
+    double kd;
+    double max_integral_error;
+    double min_integral_error;
+    bool antiwindup;
+
+    if (joint_info.parameters.find(prefix+"kp") != joint_info.parameters.end()){
+      kp = std::stod(joint_info.parameters.at(prefix+"kp"));
+    }
+    else
+    {
+      kp = 0.0;
+    }
+
+    if (joint_info.parameters.find(prefix+"ki") != joint_info.parameters.end()){
+      ki = std::stod(joint_info.parameters.at(prefix+"ki"));
+    }
+    else
+    {
+      ki = 0.0;
+    }
+
+    if (joint_info.parameters.find(prefix+"kd") != joint_info.parameters.end()){
+      kd = std::stod(joint_info.parameters.at(prefix+"kd"));
+    }
+    else
+    {
+      kd = 0.0;
+    }
+
+    if (joint_info.parameters.find(prefix+"max_integral_error") != joint_info.parameters.end()){
+      max_integral_error = std::stod(joint_info.parameters.at(prefix+"max_integral_error"));
+    }
+    else
+    {
+      max_integral_error = std::numeric_limits<double>::max();
+    }
+
+    if (joint_info.parameters.find(prefix+"min_integral_error") != joint_info.parameters.end()){
+      min_integral_error = std::stod(joint_info.parameters.at(prefix+"min_integral_error"));
+    }
+    else
+    {
+      min_integral_error = std::numeric_limits<double>::min();
+    }
+
+    if (joint_info.parameters.find(prefix+"antiwindup") != joint_info.parameters.end()){
+      if(joint_info.parameters.at(prefix+"antiwindup") == "true" || joint_info.parameters.at(prefix+"antiwindup") == "True")
+      {
+        antiwindup = true;
+      }
+    }
+    else
+    {
+      antiwindup = false;
+    }
+
+    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "setting kp=" << kp <<" ki=" << ki <<" kd=" << kd <<" max_integral_error=" << max_integral_error);
+    return control_toolbox::Pid(kp,ki,kd,max_integral_error,min_integral_error,antiwindup);
+}
+
 void GazeboSystem::registerJoints(
   const hardware_interface::HardwareInfo & hardware_info,
   gazebo::physics::ModelPtr parent_model)
@@ -154,6 +230,10 @@ void GazeboSystem::registerJoints(
   this->dataPtr->joint_position_cmd_.resize(this->dataPtr->n_dof_);
   this->dataPtr->joint_velocity_cmd_.resize(this->dataPtr->n_dof_);
   this->dataPtr->joint_effort_cmd_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->vel_pid.resize(this->dataPtr->n_dof_);
+  this->dataPtr->pos_pid.resize(this->dataPtr->n_dof_);
+  this->dataPtr->is_pos_pid.resize(this->dataPtr->n_dof_);
+  this->dataPtr->is_vel_pid.resize(this->dataPtr->n_dof_);
 
   for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
     auto & joint_info = hardware_info.joints[j];
@@ -170,6 +250,8 @@ void GazeboSystem::registerJoints(
 
     // Accept this joint and continue configuration
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
+
+
 
     std::string suffix = "";
 
@@ -264,23 +346,60 @@ void GazeboSystem::registerJoints(
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:");
 
     // register the command handles
+    bool has_already_register_vel =false;
+    bool has_already_register_pos =false;
+
     for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
-      if (joint_info.command_interfaces[i].name == "position") {
-        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
+      if (joint_info.command_interfaces[i].name == "position" || joint_info.command_interfaces[i].name == "position_pid") 
+      {
+        if(has_already_register_pos)
+        {
+          RCLCPP_ERROR(this->nh_->get_logger(), "can't have position and position_pid command_interface at same time !!!");
+          continue;
+        }
+        has_already_register_pos = true; 
+
+        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << joint_info.command_interfaces[i].name);
+        this->dataPtr->is_pos_pid[j]=(joint_info.command_interfaces[i].name =="position_pid");
+
+        if(joint_info.command_interfaces[i].name =="position_pid")
+        {
+          this->dataPtr->is_pos_pid[j] = true;
+          this->dataPtr->pos_pid[j] = this->extractPID(POSITION_PID_PARAMS_PREFIX, joint_info);
+        }
+
         this->dataPtr->command_interfaces_.emplace_back(
           joint_name + suffix,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joint_position_cmd_[j]);
+          this->dataPtr->is_pos_pid[j]=false;
+
         if (!std::isnan(initial_position)) {
           this->dataPtr->joint_position_cmd_[j] = initial_position;
         }
+
       }
       // independently of existence of command interface set initial value if defined
       if (!std::isnan(initial_position)) {
         this->dataPtr->sim_joints_[j]->SetPosition(0, initial_position, true);
       }
-      if (joint_info.command_interfaces[i].name == "velocity") {
-        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
+      if (joint_info.command_interfaces[i].name == "velocity" || joint_info.command_interfaces[i].name == "velocity_pid") 
+      {        
+        if(has_already_register_vel)
+        {
+          RCLCPP_ERROR(this->nh_->get_logger(), "can't have velocity and velocity_pid command_interface at same time !!!");
+          continue;
+        }
+        has_already_register_vel = true; 
+
+        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << joint_info.command_interfaces[i].name);
+
+        if(joint_info.command_interfaces[i].name =="velocity_pid")
+        {
+          this->dataPtr->is_vel_pid[j]=true;
+          this->dataPtr->vel_pid[j] = this->extractPID(VELOCITY_PID_PARAMS_PREFIX, joint_info);
+        }
+
         this->dataPtr->command_interfaces_.emplace_back(
           joint_name + suffix,
           hardware_interface::HW_IF_VELOCITY,
@@ -489,34 +608,47 @@ GazeboSystem::perform_command_mode_switch(
   for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
     for (const std::string & interface_name : stop_interfaces) {
       // Clear joint control method bits corresponding to stop interfaces
-      if (interface_name == (this->dataPtr->joint_names_[j] + "/" +
-        hardware_interface::HW_IF_POSITION))
+      if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION))
       {
-        this->dataPtr->joint_control_methods_[j] &= static_cast<ControlMethod_>(VELOCITY & EFFORT);
-      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT
-        hardware_interface::HW_IF_VELOCITY))
+        this->dataPtr->joint_control_methods_[j] &= static_cast<ControlMethod_>(VELOCITY & VELOCITY_PID & EFFORT);
+      } 
+      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY))
       {
-        this->dataPtr->joint_control_methods_[j] &= static_cast<ControlMethod_>(POSITION & EFFORT);
-      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT
-        hardware_interface::HW_IF_EFFORT))
+        this->dataPtr->joint_control_methods_[j] &= static_cast<ControlMethod_>(POSITION & POSITION_PID & EFFORT);
+      } 
+      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
       {
         this->dataPtr->joint_control_methods_[j] &=
-          static_cast<ControlMethod_>(POSITION & VELOCITY);
+          static_cast<ControlMethod_>(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY);
       }
     }
 
     // Set joint control method bits corresponding to start interfaces
     for (const std::string & interface_name : start_interfaces) {
-      if (interface_name == (this->dataPtr->joint_names_[j] + "/" +
-        hardware_interface::HW_IF_POSITION))
+      if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION))
+      {
+        if(!this->dataPtr->is_pos_pid[j])
       {
         this->dataPtr->joint_control_methods_[j] |= POSITION;
-      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT
-        hardware_interface::HW_IF_VELOCITY))
+        }
+        else
+        {
+          this->dataPtr->joint_control_methods_[j] |= POSITION_PID;
+        }
+      } 
+      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY))
+      {
+
+        if(!this->dataPtr->is_vel_pid[j])
       {
         this->dataPtr->joint_control_methods_[j] |= VELOCITY;
-      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT
-        hardware_interface::HW_IF_EFFORT))
+        }
+        else
+        {
+          this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID;
+        }
+      } 
+      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
       {
         this->dataPtr->joint_control_methods_[j] |= EFFORT;
       }
@@ -603,16 +735,42 @@ hardware_interface::return_type GazeboSystem::write(
     }
   }
 
+  uint64_t dt = sim_period.nanoseconds();
+
   for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
-    if (this->dataPtr->sim_joints_[j]) {
-      if (this->dataPtr->joint_control_methods_[j] & POSITION) {
-        this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true);
-        this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
-      } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { // NOLINT
+    if (this->dataPtr->sim_joints_[j]) 
+    {
+      if (this->dataPtr->joint_control_methods_[j] & POSITION) 
+        {
+          this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true);
+          this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
+      } 
+      else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) 
+      { 
         this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
-      } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT
+      } 
+      else if (this->dataPtr->joint_control_methods_[j] & EFFORT) 
+      { 
         this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]);
-      } else if (this->dataPtr->is_joint_actuated_[j]) {
+      }
+      else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) 
+      { 
+
+        double vel_goal = this->dataPtr->joint_velocity_cmd_[j];
+        double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0);
+        double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal-vel,dt);
+        this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
+
+      }
+      else if (this->dataPtr->joint_control_methods_[j] & POSITION_PID) 
+      { 
+        double pos_goal = this->dataPtr->joint_position_cmd_[j];
+        double pos = this->dataPtr->sim_joints_[j]->Position(0);
+        double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal-pos,dt);
+        this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
+      } 
+      else if (this->dataPtr->is_joint_actuated_[j]) 
+      {
         // Fallback case is a velocity command of zero (only for actuated joints)
         this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
       }
@@ -628,3 +786,4 @@ hardware_interface::return_type GazeboSystem::write(
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(
   gazebo_ros2_control::GazeboSystem, gazebo_ros2_control::GazeboSystemInterface)
+