From 225490241587f330ab5a953107e68cdd925e410d Mon Sep 17 00:00:00 2001
From: chameau5050 <54971185+chameau5050@users.noreply.github.com>
Date: Thu, 21 Mar 2024 22:44:27 -0400
Subject: [PATCH 01/11] add PID controller to control joint using effort

- add PID Implementation
- add effort base Position Controller
- add effort base Velocity Controller
---
 gazebo_ros2_control/src/gazebo_system.cpp | 169 +++++++++++++++++++++-
 1 file changed, 161 insertions(+), 8 deletions(-)

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index 2b76adc9..86ed80be 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -34,6 +34,73 @@ struct MimicJoint
   double multiplier = 1.0;
 };
 
+class PID
+{
+  public: 
+    PID(double kp=0.0, double ki=0.0, double kd=0.0, double max_integral_error=std::numeric_limits<double>::max())
+    {
+      this->kp = kp;
+      this->ki = ki;
+      this->kd = kd;
+      this->max_integral_error = max_integral_error;
+      this->integral_error = 0;
+      this->lastError = 0;
+      this->init = false;
+    };
+
+    double getCMD(double state, double goal, double dt)
+    {
+        double error = goal-state;
+        double e_prime = (error-this->lastError)/dt;
+        this->lastError = error;
+        
+        this->integral_error += error*dt;
+
+        if(this->integral_error > this->max_integral_error)
+        {
+          this->integral_error = this->max_integral_error;
+        }
+
+        if(this->integral_error < -this->max_integral_error)
+        {
+          this->integral_error = -this->max_integral_error;
+        }
+       
+        double kd_cmd = this->kd*(0-e_prime);
+        double kp_cmd = this->kp*error;
+        double ki_cmd = this->ki*this->integral_error;
+
+        if(init)
+        {
+          return kd_cmd+kp_cmd+ki_cmd;
+        }
+        else
+        {
+          return kp_cmd+ki_cmd;
+        }
+        
+        // return kp_cmd+ki_cmd;
+    };
+    
+    bool isUse()
+    {
+      return kp!= 0.0 || ki != 0.0 || kd != 0.0 ;
+    }
+
+  private:
+    double kp;
+    double ki;
+    double kd;
+    double max_integral_error;
+    double integral_error;
+    double lastError;
+    bool init;
+};
+
+
+
+
+
 class gazebo_ros2_control::GazeboSystemPrivate
 {
 public:
@@ -83,6 +150,9 @@ class gazebo_ros2_control::GazeboSystemPrivate
   /// \brief vector with the current cmd joint effort
   std::vector<double> joint_effort_cmd_;
 
+  /// \brief Joint PID utils
+  std::vector<PID> pid;
+
   /// \brief handles to the imus from within Gazebo
   std::vector<gazebo::sensors::ImuSensorPtr> sim_imu_sensors_;
 
@@ -154,6 +224,8 @@ 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->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 +242,51 @@ void GazeboSystem::registerJoints(
 
     // Accept this joint and continue configuration
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
+    
+    double kp;
+    double ki;
+    double kd;
+    double max_integral_error;
+
+    if (joint_info.parameters.find("kp") != joint_info.parameters.end()){
+      kp = std::stod(joint_info.parameters.at("kp"));
+    }
+    else
+    {
+      kp = 0.0;
+    }
+
+    if (joint_info.parameters.find("ki") != joint_info.parameters.end()){
+      ki = std::stod(joint_info.parameters.at("ki"));
+    }
+    else
+    {
+      ki = 0.0;
+    }
+
+    if (joint_info.parameters.find("kd") != joint_info.parameters.end()){
+      kd = std::stod(joint_info.parameters.at("kd"));
+    }
+    else
+    {
+      kd = 0.0;
+    }
+
+    if (joint_info.parameters.find("max_integral_error") != joint_info.parameters.end()){
+      max_integral_error = std::stod(joint_info.parameters.at("max_integral_error"));
+    }
+    else
+    {
+      max_integral_error = std::numeric_limits<double>::max();
+    }
+
+    this->dataPtr->pid[j] = PID(kp,ki,kd,max_integral_error);
+
+    if(this->dataPtr->pid[j].isUse())
+    {
+      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "setting kp=" << kp <<" ki=" << ki <<" kd=" << kd <<" max_integral_error=" << max_integral_error);
+    }
+    
 
     std::string suffix = "";
 
@@ -603,16 +720,52 @@ hardware_interface::return_type GazeboSystem::write(
     }
   }
 
+  double dt = sim_period.seconds();
+
   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
-        this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
-      } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT
+    if (this->dataPtr->sim_joints_[j]) 
+    {
+      if (this->dataPtr->joint_control_methods_[j] & POSITION) 
+      {
+        double pos_goal = this->dataPtr->joint_position_cmd_[j];
+        if(this->dataPtr->pid[j].isUse())
+        {
+          double pos = this->dataPtr->sim_joints_[j]->Position(0);
+          double cmd = this->dataPtr->pid[j].getCMD(pos,pos_goal,dt);
+       
+          this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
+        }
+        else
+        {
+          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) 
+      { 
+        
+        double vel_goal = this->dataPtr->joint_velocity_cmd_[j];
+
+        if(this->dataPtr->pid[j].isUse())
+        {
+          double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0);
+          double cmd = this->dataPtr->pid[j].getCMD(vel,vel_goal,dt);
+       
+          this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
+        }
+        else
+        {
+          this->dataPtr->sim_joints_[j]->SetVelocity(0, vel_goal);
+        }
+        
+      } 
+      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->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);
       }

From 68f70667ebcb274289f147d7cbdf3dbee22a11a0 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Mon, 1 Apr 2024 14:32:41 -0400
Subject: [PATCH 02/11] clean PID implementation

---
 gazebo_ros2_control/CMakeLists.txt            |   5 +-
 .../gazebo_ros2_control/gazebo_system.hpp     |   7 +
 .../gazebo_system_interface.hpp               |   2 +
 gazebo_ros2_control/package.xml               |   1 +
 gazebo_ros2_control/src/gazebo_system.cpp     | 326 +++++++++---------
 5 files changed, 180 insertions(+), 161 deletions(-)

diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt
index cfba1298..1031d1f2 100644
--- a/gazebo_ros2_control/CMakeLists.txt
+++ b/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()
\ No newline at end of file
diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
index 8230a4ec..14d02f63 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
+++ b/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;
 };
diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp
index 8a4cf1fb..8bded30c 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp
+++ b/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;
diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml
index 9740676b..4f32857f 100644
--- a/gazebo_ros2_control/package.xml
+++ b/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>
diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index 86ed80be..931a0e48 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -34,73 +34,6 @@ struct MimicJoint
   double multiplier = 1.0;
 };
 
-class PID
-{
-  public: 
-    PID(double kp=0.0, double ki=0.0, double kd=0.0, double max_integral_error=std::numeric_limits<double>::max())
-    {
-      this->kp = kp;
-      this->ki = ki;
-      this->kd = kd;
-      this->max_integral_error = max_integral_error;
-      this->integral_error = 0;
-      this->lastError = 0;
-      this->init = false;
-    };
-
-    double getCMD(double state, double goal, double dt)
-    {
-        double error = goal-state;
-        double e_prime = (error-this->lastError)/dt;
-        this->lastError = error;
-        
-        this->integral_error += error*dt;
-
-        if(this->integral_error > this->max_integral_error)
-        {
-          this->integral_error = this->max_integral_error;
-        }
-
-        if(this->integral_error < -this->max_integral_error)
-        {
-          this->integral_error = -this->max_integral_error;
-        }
-       
-        double kd_cmd = this->kd*(0-e_prime);
-        double kp_cmd = this->kp*error;
-        double ki_cmd = this->ki*this->integral_error;
-
-        if(init)
-        {
-          return kd_cmd+kp_cmd+ki_cmd;
-        }
-        else
-        {
-          return kp_cmd+ki_cmd;
-        }
-        
-        // return kp_cmd+ki_cmd;
-    };
-    
-    bool isUse()
-    {
-      return kp!= 0.0 || ki != 0.0 || kd != 0.0 ;
-    }
-
-  private:
-    double kp;
-    double ki;
-    double kd;
-    double max_integral_error;
-    double integral_error;
-    double lastError;
-    bool init;
-};
-
-
-
-
-
 class gazebo_ros2_control::GazeboSystemPrivate
 {
 public:
@@ -150,8 +83,17 @@ class gazebo_ros2_control::GazeboSystemPrivate
   /// \brief vector with the current cmd joint effort
   std::vector<double> joint_effort_cmd_;
 
-  /// \brief Joint PID utils
-  std::vector<PID> pid;
+  /// \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_;
@@ -209,83 +151,106 @@ bool GazeboSystem::initSim(
   return true;
 }
 
-void GazeboSystem::registerJoints(
-  const hardware_interface::HardwareInfo & hardware_info,
-  gazebo::physics::ModelPtr parent_model)
+control_toolbox::Pid GazeboSystem::extractPID(std::string prefix, hardware_interface::ComponentInfo joint_info)
 {
-  this->dataPtr->n_dof_ = hardware_info.joints.size();
-
-  this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_);
-  this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_);
-  this->dataPtr->is_joint_actuated_.resize(this->dataPtr->n_dof_);
-  this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_);
-  this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_);
-  this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_);
-  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->pid.resize(this->dataPtr->n_dof_);
-
-
-  for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
-    auto & joint_info = hardware_info.joints[j];
-    std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
-
-    gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
-    this->dataPtr->sim_joints_.push_back(simjoint);
-    if (!simjoint) {
-      RCLCPP_WARN_STREAM(
-        this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name <<
-          "' which is not in the gazebo model.");
-      continue;
-    }
-
-    // Accept this joint and continue configuration
-    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
-    
     double kp;
     double ki;
     double kd;
     double max_integral_error;
+    double min_integral_error;
+    bool antiwindup;
 
-    if (joint_info.parameters.find("kp") != joint_info.parameters.end()){
-      kp = std::stod(joint_info.parameters.at("kp"));
+    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("ki") != joint_info.parameters.end()){
-      ki = std::stod(joint_info.parameters.at("ki"));
+    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("kd") != joint_info.parameters.end()){
-      kd = std::stod(joint_info.parameters.at("kd"));
+    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("max_integral_error") != joint_info.parameters.end()){
-      max_integral_error = std::stod(joint_info.parameters.at("max_integral_error"));
+    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();
     }
 
-    this->dataPtr->pid[j] = PID(kp,ki,kd,max_integral_error);
+    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(this->dataPtr->pid[j].isUse())
+    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
     {
-      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "setting kp=" << kp <<" ki=" << ki <<" kd=" << kd <<" max_integral_error=" << max_integral_error);
+      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)
+{
+  this->dataPtr->n_dof_ = hardware_info.joints.size();
+
+  this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->is_joint_actuated_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_);
+  this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_);
+  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];
+    std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
+
+    gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
+    this->dataPtr->sim_joints_.push_back(simjoint);
+    if (!simjoint) {
+      RCLCPP_WARN_STREAM(
+        this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name <<
+          "' which is not in the gazebo model.");
+      continue;
     }
+
+    // Accept this joint and continue configuration
+    RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
+    
     
 
     std::string suffix = "";
@@ -381,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,
@@ -606,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;
       }
@@ -720,49 +735,39 @@ hardware_interface::return_type GazeboSystem::write(
     }
   }
 
-  double dt = sim_period.seconds();
+  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) 
-      {
-        double pos_goal = this->dataPtr->joint_position_cmd_[j];
-        if(this->dataPtr->pid[j].isUse())
-        {
-          double pos = this->dataPtr->sim_joints_[j]->Position(0);
-          double cmd = this->dataPtr->pid[j].getCMD(pos,pos_goal,dt);
-       
-          this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
-        }
-        else
         {
           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) 
       { 
-        
-        double vel_goal = this->dataPtr->joint_velocity_cmd_[j];
-
-        if(this->dataPtr->pid[j].isUse())
-        {
-          double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0);
-          double cmd = this->dataPtr->pid[j].getCMD(vel,vel_goal,dt);
-       
-          this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
-        }
-        else
-        {
-          this->dataPtr->sim_joints_[j]->SetVelocity(0, vel_goal);
-        }
-        
+        this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
       } 
       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->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]) 
       {
@@ -781,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)
+  
\ No newline at end of file

From 50b00db5d861fa25991260a4f3f4ba21f320cf80 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Thu, 4 Apr 2024 17:50:50 -0400
Subject: [PATCH 03/11] add exemple for velocity_pid and position_pid

---
 gazebo_ros2_control_demos/CMakeLists.txt      |  7 ++
 .../config/cart_controller_position.yaml      | 15 ++++
 .../examples/example_position_pid.cpp         | 58 +++++++++++++
 ...rtical_cart_example_position_pid.launch.py | 85 ++++++++++++++++++
 ...rtical_cart_example_velocity_pid.launch.py | 86 +++++++++++++++++++
 ...test_vertical_cart_position_pid.xacro.urdf | 77 +++++++++++++++++
 ...test_vertical_cart_velocity_pid.xacro.urdf | 77 +++++++++++++++++
 7 files changed, 405 insertions(+)
 create mode 100644 gazebo_ros2_control_demos/config/cart_controller_position.yaml
 create mode 100644 gazebo_ros2_control_demos/examples/example_position_pid.cpp
 create mode 100644 gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
 create mode 100644 gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
 create mode 100644 gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
 create mode 100644 gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf

diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt
index 6823378a..50488e59 100644
--- a/gazebo_ros2_control_demos/CMakeLists.txt
+++ b/gazebo_ros2_control_demos/CMakeLists.txt
@@ -54,6 +54,12 @@ ament_target_dependencies(example_gripper
   std_msgs
 )
 
+add_executable(example_position_pid examples/example_position_pid.cpp)
+ament_target_dependencies(example_position_pid
+  rclcpp
+  std_msgs
+)
+
 add_executable(example_diff_drive examples/example_diff_drive.cpp)
 ament_target_dependencies(example_diff_drive
   rclcpp
@@ -77,6 +83,7 @@ endif()
 install(
   TARGETS
     example_position
+    example_position_pid
     example_velocity
     example_effort
     example_diff_drive
diff --git a/gazebo_ros2_control_demos/config/cart_controller_position.yaml b/gazebo_ros2_control_demos/config/cart_controller_position.yaml
new file mode 100644
index 00000000..1aa6ea4c
--- /dev/null
+++ b/gazebo_ros2_control_demos/config/cart_controller_position.yaml
@@ -0,0 +1,15 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+position_controller:
+  ros__parameters:
+    joints:
+      - slider_to_cart
+
diff --git a/gazebo_ros2_control_demos/examples/example_position_pid.cpp b/gazebo_ros2_control_demos/examples/example_position_pid.cpp
new file mode 100644
index 00000000..654d7653
--- /dev/null
+++ b/gazebo_ros2_control_demos/examples/example_position_pid.cpp
@@ -0,0 +1,58 @@
+// Copyright 2020 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.
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/float64_multi_array.hpp"
+
+std::shared_ptr<rclcpp::Node> node;
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  node = std::make_shared<rclcpp::Node>("velocity_test_node");
+
+  auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
+    "/position_controller/commands", 10);
+
+  RCLCPP_INFO(node->get_logger(), "node created");
+
+  std_msgs::msg::Float64MultiArray commands;
+
+  using namespace std::chrono_literals;
+
+  commands.data.push_back(0);
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 1;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = -1;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 0;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
new file mode 100644
index 00000000..ca3f6686
--- /dev/null
+++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
@@ -0,0 +1,85 @@
+# Copyright 2020 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_vertical_cart_position_pid.xacro.urdf')
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'cart'],
+                        output='screen')
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_joint_trajectory_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'position_controller'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_broadcaster,
+                on_exit=[load_joint_trajectory_controller],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])
diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
new file mode 100644
index 00000000..2236935e
--- /dev/null
+++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
@@ -0,0 +1,86 @@
+# Copyright 2020 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_vertical_cart_velocity_pid.xacro.urdf')
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'cart'],
+                        output='screen')
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_velocity_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'velocity_controller'],
+        output='screen'
+    )
+
+
+    return LaunchDescription([
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_broadcaster,
+                on_exit=[load_velocity_controller],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])
diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
new file mode 100644
index 00000000..56999cb3
--- /dev/null
+++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
@@ -0,0 +1,77 @@
+<?xml version="1.0" ?>
+<robot name="cart">
+  <link name="world"/>
+  
+  <link name="slideBar">
+    <visual>
+      <geometry>
+        <box size="0.05 0.05 5"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="green">
+        <color rgba="0 0.8 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  
+  <link name="cart">
+    <visual>
+      <geometry>
+        <box size="0.5 0.5 0.2"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="blue">
+        <color rgba="0 0 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="world_to_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 2.5"/>
+    <parent link="world"/>
+    <child link="slideBar"/>
+  </joint>
+
+  <joint name="slider_to_cart" type="prismatic">
+    <axis xyz="0 0 1"/>
+    <origin xyz="0.0 0.0 0.0"/>
+    <parent link="slideBar"/>
+    <child link="cart"/>
+    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="30"/>
+    <dynamics damping="1.0" friction="0.0"/>
+  </joint>
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+    <joint name="slider_to_cart">
+
+      <param name="pos_kp">100</param>
+      <param name="pos_ki">1</param>
+      <param name="pos_kd">10</param>
+      <param name="pos_max_integral_error">10000</param>
+
+      <command_interface name="position_pid"/>
+      
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <parameters>$(find gazebo_ros2_control_demos)/config/cart_controller_position.yaml</parameters>
+    </plugin>
+  </gazebo>
+</robot>
diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf
new file mode 100644
index 00000000..0c079e18
--- /dev/null
+++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf
@@ -0,0 +1,77 @@
+<?xml version="1.0" ?>
+<robot name="cart">
+  <link name="world"/>
+  
+  <link name="slideBar">
+    <visual>
+      <geometry>
+        <box size="0.05 0.05 5"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="green">
+        <color rgba="0 0.8 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  
+  <link name="cart">
+    <visual>
+      <geometry>
+        <box size="0.5 0.5 0.2"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="blue">
+        <color rgba="0 0 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="world_to_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 2.5"/>
+    <parent link="world"/>
+    <child link="slideBar"/>
+  </joint>
+
+  <joint name="slider_to_cart" type="prismatic">
+    <axis xyz="0 0 1"/>
+    <origin xyz="0.0 0.0 0.0"/>
+    <parent link="slideBar"/>
+    <child link="cart"/>
+    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="30"/>
+    <dynamics damping="1.0" friction="0.0"/>
+  </joint>
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+    <joint name="slider_to_cart">
+
+      <param name="vel_kp">10</param>
+      <param name="vel_ki">10</param>
+      <param name="vel_kd">0.1</param>
+      <param name="vel_max_integral_error">10000</param>
+
+      <command_interface name="velocity_pid"/>
+      
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <parameters>$(find gazebo_ros2_control_demos)/config/cart_controller_velocity.yaml</parameters>
+    </plugin>
+  </gazebo>
+</robot>

From ab7d0a784bad228e0695cbc2a96e662bb7d1e12e Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Thu, 4 Apr 2024 19:30:37 -0400
Subject: [PATCH 04/11] add documentation for PID joint control

---
 doc/index.rst | 52 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/doc/index.rst b/doc/index.rst
index e24a2308..87162caa 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -131,6 +131,58 @@ We should include:
   </joint>
 
 
+Using PID control joints
+-----------------------------------------------------------
+
+To use PID control joints in gazebo_ros2_control, you should define their parameters inside the ``<joint>`` tag 
+within the ``<ros2_control>`` tag. These PID joints can be controlled either in position or velocity.
+
+- To control a joint with velocity PID, simply set its ``command_interface`` to ``velocity_PID``.
+- To control a joint with position PID, set its ``command_interface`` to ``position_PID``.
+
+.. note::
+    You cannot have both command interfaces set to position and position_PID for the same joint. The same restriction applies to velocity (and velocity_PID).
+
+To create a system with one joint that can be controlled using both position_PID and velocity_PID, follow this example:
+
+.. code-block:: xml
+
+ <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+    <joint name="slider_to_cart">
+
+      <param name="pos_kp">10</param>
+      <param name="pos_ki">1</param>
+      <param name="pos_kd">2</param>
+      <param name="pos_max_integral_error">10000</param>
+
+      <param name="vel_kp">10</param>
+      <param name="vel_ki">5</param>
+      <param name="vel_kd">2</param>
+      <param name="vel_max_integral_error">10000</param>
+
+      <command_interface name="position_PID"/>
+      <command_interface name="velocity_PID"/>
+
+      <state_interface name="position">
+        <param name="initial_value">1.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+
+Where the parameters are as follows:
+
+- ``pos_kp``: Proportional gain
+- ``pos_ki``: Integral gain
+- ``pos_kd``: Derivative gain
+- ``pos_max_integral_error``: Maximum summation of the error
+
+The same definitions apply to the ``vel_*`` parameters.
+
 Add the gazebo_ros2_control plugin
 ==========================================
 

From 6acca413e8324ea52a2d2542ae5c44c2569bef67 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Fri, 5 Apr 2024 10:13:28 -0400
Subject: [PATCH 05/11] fix selection problem with position PID

---
 gazebo_ros2_control/src/gazebo_system.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index 931a0e48..aeaa021b 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -359,20 +359,22 @@ void GazeboSystem::registerJoints(
         }
         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");
+        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << joint_info.command_interfaces[i].name);        
 
         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);
         }
+        else
+        {
+          this->dataPtr->is_pos_pid[j]=false;
+        }
 
         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;

From d97d8393b64d7888017b7805783f62a66f42d415 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Fri, 5 Apr 2024 15:10:55 -0400
Subject: [PATCH 06/11] fix lint warning

---
 gazebo_ros2_control/CMakeLists.txt            |   6 +-
 .../gazebo_ros2_control/gazebo_system.hpp     |   7 +-
 gazebo_ros2_control/package.xml               |   4 +-
 gazebo_ros2_control/src/gazebo_system.cpp     | 210 ++++++++----------
 4 files changed, 96 insertions(+), 131 deletions(-)

diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt
index 1031d1f2..fb5f3a4a 100644
--- a/gazebo_ros2_control/CMakeLists.txt
+++ b/gazebo_ros2_control/CMakeLists.txt
@@ -4,10 +4,10 @@ project(gazebo_ros2_control)
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
 find_package(controller_manager REQUIRED)
+find_package(control_toolbox 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)
@@ -34,12 +34,12 @@ add_library(${PROJECT_NAME} SHARED
 ament_target_dependencies(${PROJECT_NAME}
   angles
   controller_manager
+  control_toolbox
   gazebo_dev
   gazebo_ros
   hardware_interface
   pluginlib
   rclcpp
-  control_toolbox
   yaml_cpp_vendor
 )
 
@@ -48,9 +48,9 @@ add_library(gazebo_hardware_plugins SHARED
 )
 ament_target_dependencies(gazebo_hardware_plugins
   angles
+  control_toolbox
   gazebo_dev
   hardware_interface
-  control_toolbox
   rclcpp
 )
 
diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
index 14d02f63..64b45341 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
+++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
@@ -26,6 +26,7 @@
 #include "angles/angles.h"
 
 #include "control_toolbox/pid.hpp"
+
 #include "gazebo_ros2_control/gazebo_system_interface.hpp"
 
 #include "std_msgs/msg/bool.hpp"
@@ -42,7 +43,7 @@ class GazeboSystemPrivate;
 
 class GazeboSystem : public GazeboSystemInterface
 {
-public:
+ public:
   // Documentation Inherited
   CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info)
   override;
@@ -81,7 +82,7 @@ class GazeboSystem : public GazeboSystemInterface
     const hardware_interface::HardwareInfo & hardware_info,
     sdf::ElementPtr sdf) override;
 
-private:
+ private:
   void registerJoints(
     const hardware_interface::HardwareInfo & hardware_info,
     gazebo::physics::ModelPtr parent_model);
@@ -90,7 +91,7 @@ class GazeboSystem : public GazeboSystemInterface
     const hardware_interface::HardwareInfo & hardware_info,
     gazebo::physics::ModelPtr parent_model);
 
-  control_toolbox::Pid extractPID(std::string prefix, 
+  control_toolbox::Pid extractPID(std::string prefix,
       hardware_interface::ComponentInfo joint_info);
 
   /// \brief Private data class
diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml
index 4f32857f..fb99381e 100644
--- a/gazebo_ros2_control/package.xml
+++ b/gazebo_ros2_control/package.xml
@@ -19,15 +19,15 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>angles</depend>
+  <depend>controller_manager</depend>
+  <depend>control_toolbox</depend>
   <depend>gazebo_dev</depend>
   <depend>gazebo_ros</depend>
-  <depend>controller_manager</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <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>
diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index aeaa021b..400bb184 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -34,9 +34,8 @@ struct MimicJoint
   double multiplier = 1.0;
 };
 
-class gazebo_ros2_control::GazeboSystemPrivate
-{
-public:
+class gazebo_ros2_control::GazeboSystemPrivate{
+ public:
   GazeboSystemPrivate() = default;
 
   ~GazeboSystemPrivate() = default;
@@ -88,8 +87,8 @@ class gazebo_ros2_control::GazeboSystemPrivate
 
   /// \brief Joint position PID utils
   std::vector<control_toolbox::Pid> pos_pid;
-  
-  //  \brief control flag
+
+  // \brief control flag
   std::vector<bool> is_pos_pid;
 
   //  \brief control flag
@@ -151,7 +150,9 @@ bool GazeboSystem::initSim(
   return true;
 }
 
-control_toolbox::Pid GazeboSystem::extractPID(std::string prefix, hardware_interface::ComponentInfo joint_info)
+control_toolbox::Pid GazeboSystem::extractPID(
+  std::string prefix,
+  hardware_interface::ComponentInfo joint_info)
 {
     double kp;
     double ki;
@@ -160,59 +161,52 @@ control_toolbox::Pid GazeboSystem::extractPID(std::string prefix, hardware_inter
     double min_integral_error;
     bool antiwindup;
 
-    if (joint_info.parameters.find(prefix+"kp") != joint_info.parameters.end()){
+    if (joint_info.parameters.find(prefix+"kp") != joint_info.parameters.end()) {
       kp = std::stod(joint_info.parameters.at(prefix+"kp"));
-    }
-    else
-    {
+    } else {
       kp = 0.0;
     }
 
-    if (joint_info.parameters.find(prefix+"ki") != joint_info.parameters.end()){
+    if (joint_info.parameters.find(prefix+"ki") != joint_info.parameters.end()) {
       ki = std::stod(joint_info.parameters.at(prefix+"ki"));
-    }
-    else
-    {
+    } else {
       ki = 0.0;
     }
 
-    if (joint_info.parameters.find(prefix+"kd") != joint_info.parameters.end()){
+    if (joint_info.parameters.find(prefix+"kd") != joint_info.parameters.end()) {
       kd = std::stod(joint_info.parameters.at(prefix+"kd"));
-    }
-    else
-    {
+    } else {
       kd = 0.0;
     }
 
-    if (joint_info.parameters.find(prefix+"max_integral_error") != joint_info.parameters.end()){
+    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
-    {
+    } else {
       max_integral_error = std::numeric_limits<double>::max();
     }
 
-    if (joint_info.parameters.find(prefix+"min_integral_error") != joint_info.parameters.end()){
+    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
-    {
+    } 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")
-      {
+    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
-    {
+    } 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);
+    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(
@@ -250,8 +244,6 @@ void GazeboSystem::registerJoints(
 
     // Accept this joint and continue configuration
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
-    
-    
 
     std::string suffix = "";
 
@@ -346,29 +338,27 @@ 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;
+    bool has_already_registered_vel = false;
+    bool has_already_registered_pos = false;
 
     for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
-      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 !!!");
+      if (joint_info.command_interfaces[i].name == "position" ||
+        joint_info.command_interfaces[i].name == "position_pid") {
+        if (has_already_registered_pos) {
+          RCLCPP_ERROR_STREAM(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);        
+        has_already_registered_pos = true;
+        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t "
+                                                    << joint_info.command_interfaces[i].name);
 
-        if(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);
-        }
-        else
-        {
-          this->dataPtr->is_pos_pid[j]=false;
+        } else {
+          this->dataPtr->is_pos_pid[j] = false;
         }
 
         this->dataPtr->command_interfaces_.emplace_back(
@@ -379,29 +369,27 @@ void GazeboSystem::registerJoints(
         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" || 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 !!!");
+      if (joint_info.command_interfaces[i].name == "velocity" ||
+          joint_info.command_interfaces[i].name == "velocity_pid") {
+        if (has_already_registered_vel) {
+          RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "can't have velocity and velocity_pid "
+                                                << "command_interface at same time !!!");
           continue;
         }
-        has_already_register_vel = true; 
+        has_already_registered_vel = true;
 
-        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << joint_info.command_interfaces[i].name);
+        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;
+        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,
@@ -610,16 +598,15 @@ 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))
-      {
-        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 & POSITION_PID & EFFORT);
-      } 
-      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
-      {
+      if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) {  // NOLINT
+        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)) {  // NOLINT
+        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)) {  // NOLINT
         this->dataPtr->joint_control_methods_[j] &=
           static_cast<ControlMethod_>(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY);
       }
@@ -627,31 +614,20 @@ GazeboSystem::perform_command_mode_switch(
 
     // 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(!this->dataPtr->is_pos_pid[j])
-      {
+      if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) {  // NOLINT
+        if (!this->dataPtr->is_pos_pid[j]) {
         this->dataPtr->joint_control_methods_[j] |= POSITION;
-        }
-        else
-        {
+        } 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
-        {
+      }
+      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) {  // NOLINT
+        if (!this->dataPtr->is_vel_pid[j]) {
+          this->dataPtr->joint_control_methods_[j] |= VELOCITY;
+        } else {
           this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID;
         }
-      } 
-      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
-      {
+      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) {  // NOLINT
         this->dataPtr->joint_control_methods_[j] |= EFFORT;
       }
     }
@@ -718,20 +694,17 @@ hardware_interface::return_type GazeboSystem::write(
   // set values of all mimic joints with respect to mimicked joint
   for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
     if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION &&
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION)
-    {
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) {
       this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
         mimic_joint.multiplier *
         this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
     } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY)
-    {
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) {
       this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
         mimic_joint.multiplier *
         this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
     } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT)
-    {
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) {
       this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
         mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
     }
@@ -740,39 +713,30 @@ 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);
+    if (this->dataPtr->sim_joints_[j]) {
+      if (this->dataPtr->joint_control_methods_[j] & POSITION) {
+          double cmd = this->dataPtr->joint_position_cmd_[j];
+          this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true);
           this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
-      } 
-      else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) 
-      { 
+
+      } 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) 
-      { 
+
+      } 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->joint_control_methods_[j] & VELOCITY_PID) 
-      { 
-        
+
+      } 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);
+        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) 
-      { 
+      } 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);
+        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]) 
-      {
+      } 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);
       }

From 39874571571854e87e3068085998083339e36f30 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Sun, 7 Apr 2024 15:36:46 -0400
Subject: [PATCH 07/11] fix all uncrustify and cpplint error

---
 .../gazebo_ros2_control/gazebo_system.hpp     |   9 +-
 gazebo_ros2_control/src/gazebo_system.cpp     | 176 ++++++++++--------
 ...rtical_cart_example_velocity_pid.launch.py |   1 -
 3 files changed, 104 insertions(+), 82 deletions(-)

diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
index 64b45341..d3de0eef 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
+++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
@@ -43,7 +43,7 @@ class GazeboSystemPrivate;
 
 class GazeboSystem : public GazeboSystemInterface
 {
- public:
+public:
   // Documentation Inherited
   CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info)
   override;
@@ -82,7 +82,7 @@ class GazeboSystem : public GazeboSystemInterface
     const hardware_interface::HardwareInfo & hardware_info,
     sdf::ElementPtr sdf) override;
 
- private:
+private:
   void registerJoints(
     const hardware_interface::HardwareInfo & hardware_info,
     gazebo::physics::ModelPtr parent_model);
@@ -91,8 +91,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);
+  control_toolbox::Pid extractPID(
+    std::string prefix,
+    hardware_interface::ComponentInfo joint_info);
 
   /// \brief Private data class
   std::unique_ptr<GazeboSystemPrivate> dataPtr;
diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index 400bb184..92030e91 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -34,8 +34,9 @@ struct MimicJoint
   double multiplier = 1.0;
 };
 
-class gazebo_ros2_control::GazeboSystemPrivate{
- public:
+class gazebo_ros2_control::GazeboSystemPrivate
+{
+public:
   GazeboSystemPrivate() = default;
 
   ~GazeboSystemPrivate() = default;
@@ -154,59 +155,61 @@ 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;
-    }
+  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 + "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 + "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 + "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 + "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;
+  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);
+  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);
+  return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
 }
 
 void GazeboSystem::registerJoints(
@@ -343,18 +346,21 @@ void GazeboSystem::registerJoints(
 
     for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) {
       if (joint_info.command_interfaces[i].name == "position" ||
-        joint_info.command_interfaces[i].name == "position_pid") {
+        joint_info.command_interfaces[i].name == "position_pid")
+      {
         if (has_already_registered_pos) {
-          RCLCPP_ERROR_STREAM(this->nh_->get_logger(),
-                                            "can't have position and position"
-                                         << "pid command_interface at same time !!!");
+          RCLCPP_ERROR_STREAM(
+            this->nh_->get_logger(),
+            "can't have position and position"
+              << "pid command_interface at same time !!!");
           continue;
         }
         has_already_registered_pos = true;
-        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t "
-                                                    << joint_info.command_interfaces[i].name);
+        RCLCPP_INFO_STREAM(
+          this->nh_->get_logger(), "\t\t "
+            << joint_info.command_interfaces[i].name);
 
-        if (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);
         } else {
@@ -375,16 +381,19 @@ void GazeboSystem::registerJoints(
         this->dataPtr->sim_joints_[j]->SetPosition(0, initial_position, true);
       }
       if (joint_info.command_interfaces[i].name == "velocity" ||
-          joint_info.command_interfaces[i].name == "velocity_pid") {
+        joint_info.command_interfaces[i].name == "velocity_pid")
+      {
         if (has_already_registered_vel) {
-          RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "can't have velocity and velocity_pid "
-                                                << "command_interface at same time !!!");
+          RCLCPP_ERROR_STREAM(
+            this->nh_->get_logger(), "can't have velocity and velocity_pid "
+              << "command_interface at same time !!!");
           continue;
         }
         has_already_registered_vel = true;
 
-        RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t "
-                            << joint_info.command_interfaces[i].name);
+        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;
@@ -598,15 +607,21 @@ 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)) {  // NOLINT
+      if (interface_name == // NOLINT
+        (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION))
+      {
         this->dataPtr->joint_control_methods_[j] &=
-        static_cast<ControlMethod_>(VELOCITY & VELOCITY_PID & EFFORT);
+          static_cast<ControlMethod_>(VELOCITY & VELOCITY_PID & EFFORT);
 
-      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) {  // NOLINT
+      } else if (interface_name == // NOLINT
+        (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY))
+      {
         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)) {  // NOLINT
+      } else if (interface_name == // NOLINT
+        (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
+      {
         this->dataPtr->joint_control_methods_[j] &=
           static_cast<ControlMethod_>(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY);
       }
@@ -614,20 +629,25 @@ GazeboSystem::perform_command_mode_switch(
 
     // 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)) {  // NOLINT
+      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;
+          this->dataPtr->joint_control_methods_[j] |= POSITION;
         } else {
           this->dataPtr->joint_control_methods_[j] |= POSITION_PID;
         }
-      }
-      else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) {  // NOLINT
+      } else if (interface_name == // NOLINT
+        (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 {
           this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID;
         }
-      } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) {  // NOLINT
+      } else if (interface_name == // NOLINT
+        (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
+      {
         this->dataPtr->joint_control_methods_[j] |= EFFORT;
       }
     }
@@ -694,17 +714,20 @@ hardware_interface::return_type GazeboSystem::write(
   // set values of all mimic joints with respect to mimicked joint
   for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
     if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION &&
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) {
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION)
+    {
       this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
         mimic_joint.multiplier *
         this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
     } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) {
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY)
+    {
       this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
         mimic_joint.multiplier *
         this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
     } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT
-      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) {
+      this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT)
+    {
       this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
         mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
     }
@@ -715,9 +738,9 @@ hardware_interface::return_type GazeboSystem::write(
   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) {
-          double cmd = this->dataPtr->joint_position_cmd_[j];
-          this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true);
-          this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
+        double cmd = this->dataPtr->joint_position_cmd_[j];
+        this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, 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]);
@@ -728,13 +751,13 @@ hardware_interface::return_type GazeboSystem::write(
       } 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);
+        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);
+        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)
@@ -752,4 +775,3 @@ hardware_interface::return_type GazeboSystem::write(
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(
   gazebo_ros2_control::GazeboSystem, gazebo_ros2_control::GazeboSystemInterface)
-  
\ No newline at end of file
diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
index 2236935e..049537b7 100644
--- a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
+++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
@@ -66,7 +66,6 @@ def generate_launch_description():
         output='screen'
     )
 
-
     return LaunchDescription([
         RegisterEventHandler(
             event_handler=OnProcessExit(

From f4306e131626a4425072a011addded43d77e7b25 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Mon, 8 Apr 2024 08:21:52 -0400
Subject: [PATCH 08/11] fix CI error and update for code review

---
 doc/index.rst                                         |  2 +-
 gazebo_ros2_control/CMakeLists.txt                    |  2 +-
 .../include/gazebo_ros2_control/gazebo_system.hpp     |  2 +-
 gazebo_ros2_control/src/gazebo_system.cpp             | 11 ++++++-----
 .../config/cart_controller_position.yaml              |  1 -
 .../examples/example_position_pid.cpp                 |  2 +-
 .../vertical_cart_example_position_pid.launch.py      |  2 +-
 .../vertical_cart_example_velocity_pid.launch.py      |  2 +-
 .../urdf/test_vertical_cart_position_pid.xacro.urdf   |  8 ++++----
 .../urdf/test_vertical_cart_velocity_pid.xacro.urdf   |  8 ++++----
 10 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/doc/index.rst b/doc/index.rst
index 87162caa..7136a8dd 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -134,7 +134,7 @@ We should include:
 Using PID control joints
 -----------------------------------------------------------
 
-To use PID control joints in gazebo_ros2_control, you should define their parameters inside the ``<joint>`` tag 
+To use PID control joints in gazebo_ros2_control, you should define their parameters inside the ``<joint>`` tag
 within the ``<ros2_control>`` tag. These PID joints can be controlled either in position or velocity.
 
 - To control a joint with velocity PID, simply set its ``command_interface`` to ``velocity_PID``.
diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt
index fb5f3a4a..01a40ee2 100644
--- a/gazebo_ros2_control/CMakeLists.txt
+++ b/gazebo_ros2_control/CMakeLists.txt
@@ -96,4 +96,4 @@ install(DIRECTORY include/${PROJECT_NAME}/
 
 pluginlib_export_plugin_description_file(gazebo_ros2_control gazebo_hardware_plugins.xml)
 
-ament_package()
\ No newline at end of file
+ament_package()
diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
index d3de0eef..7f5d93cf 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
+++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
@@ -25,7 +25,7 @@
 
 #include "angles/angles.h"
 
-#include "control_toolbox/pid.hpp"
+#include <control_toolbox/pid.hpp>
 
 #include "gazebo_ros2_control/gazebo_system_interface.hpp"
 
diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index c84a8937..f4c41e7b 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -19,6 +19,8 @@
 #include <vector>
 #include <utility>
 
+#include <control_toolbox/pid.hpp>
+
 #include "gazebo_ros2_control/gazebo_system.hpp"
 #include "gazebo/sensors/ImuSensor.hh"
 #include "gazebo/sensors/ForceTorqueSensor.hh"
@@ -204,10 +206,10 @@ control_toolbox::Pid GazeboSystem::extractPID(
 
   RCLCPP_INFO_STREAM(
     this->nh_->get_logger(),
-    "setting kp=" << kp
-                  << " ki=" << ki
-                  << " kd=" << kd
-                  << " max_integral_error=" << max_integral_error);
+    "Setting kp = " << kp << "\t"
+                    << " ki = " << ki << "\t"
+                    << " kd = " << kd << "\t"
+                    << " max_integral_error = " << max_integral_error);
 
   return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
 }
@@ -371,7 +373,6 @@ void GazeboSystem::registerJoints(
           joint_name + suffix,
           hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joint_position_cmd_[j]);
-
         if (!std::isnan(initial_position)) {
           this->dataPtr->joint_position_cmd_[j] = initial_position;
         }
diff --git a/gazebo_ros2_control_demos/config/cart_controller_position.yaml b/gazebo_ros2_control_demos/config/cart_controller_position.yaml
index 1aa6ea4c..d12a2295 100644
--- a/gazebo_ros2_control_demos/config/cart_controller_position.yaml
+++ b/gazebo_ros2_control_demos/config/cart_controller_position.yaml
@@ -12,4 +12,3 @@ position_controller:
   ros__parameters:
     joints:
       - slider_to_cart
-
diff --git a/gazebo_ros2_control_demos/examples/example_position_pid.cpp b/gazebo_ros2_control_demos/examples/example_position_pid.cpp
index 654d7653..07ea680d 100644
--- a/gazebo_ros2_control_demos/examples/example_position_pid.cpp
+++ b/gazebo_ros2_control_demos/examples/example_position_pid.cpp
@@ -1,4 +1,4 @@
-// Copyright 2020 Open Source Robotics Foundation, Inc.
+// Copyright 2024 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.
diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
index ca3f6686..d708dbfe 100644
--- a/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
+++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2020 Open Source Robotics Foundation, Inc.
+# Copyright 2024 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.
diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
index 049537b7..ae16fd1c 100644
--- a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
+++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2020 Open Source Robotics Foundation, Inc.
+# Copyright 2024 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.
diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
index 56999cb3..cc3ce1cc 100644
--- a/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
+++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <robot name="cart">
   <link name="world"/>
-  
+
   <link name="slideBar">
     <visual>
       <geometry>
@@ -17,7 +17,7 @@
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
-  
+
   <link name="cart">
     <visual>
       <geometry>
@@ -61,12 +61,12 @@
       <param name="pos_max_integral_error">10000</param>
 
       <command_interface name="position_pid"/>
-      
+
       <state_interface name="position"/>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
     </joint>
-    
+
   </ros2_control>
 
   <gazebo>
diff --git a/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf
index 0c079e18..58661be1 100644
--- a/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf
+++ b/gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <robot name="cart">
   <link name="world"/>
-  
+
   <link name="slideBar">
     <visual>
       <geometry>
@@ -17,7 +17,7 @@
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
-  
+
   <link name="cart">
     <visual>
       <geometry>
@@ -61,12 +61,12 @@
       <param name="vel_max_integral_error">10000</param>
 
       <command_interface name="velocity_pid"/>
-      
+
       <state_interface name="position"/>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
     </joint>
-    
+
   </ros2_control>
 
   <gazebo>

From 8f143b54e71233af7ab72629163bad0a2e037b67 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 29 Apr 2024 13:55:46 +0200
Subject: [PATCH 09/11] Update gazebo_ros2_control/src/gazebo_system.cpp

---
 gazebo_ros2_control/src/gazebo_system.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index f4c41e7b..8ffc1f2a 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -19,8 +19,7 @@
 #include <vector>
 #include <utility>
 
-#include <control_toolbox/pid.hpp>
-
+#include "control_toolbox/pid.hpp"
 #include "gazebo_ros2_control/gazebo_system.hpp"
 #include "gazebo/sensors/ImuSensor.hh"
 #include "gazebo/sensors/ForceTorqueSensor.hh"

From 5b93baec3c4bf5fc4f877c7b456a165934014b0c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 29 Apr 2024 13:56:45 +0200
Subject: [PATCH 10/11] Update
 gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp

---
 .../include/gazebo_ros2_control/gazebo_system.hpp              | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
index 7f5d93cf..8473b9b6 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
+++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
@@ -25,8 +25,7 @@
 
 #include "angles/angles.h"
 
-#include <control_toolbox/pid.hpp>
-
+#include "control_toolbox/pid.hpp"
 #include "gazebo_ros2_control/gazebo_system_interface.hpp"
 
 #include "std_msgs/msg/bool.hpp"

From 749b1490ec7480a918857615f72d1bfbb49d6cf2 Mon Sep 17 00:00:00 2001
From: chameau5050 <demo2502@usherbrooke.ca>
Date: Wed, 1 May 2024 19:55:51 -0400
Subject: [PATCH 11/11] update docs

---
 doc/index.rst | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/doc/index.rst b/doc/index.rst
index 7136a8dd..07d89c49 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -183,6 +183,31 @@ Where the parameters are as follows:
 
 The same definitions apply to the ``vel_*`` parameters.
 
+To try the position PID example, run the following command:
+
+.. code-block:: shell
+
+  ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pid.launch.py
+
+When the Gazebo world is launched you can run the following commands to move the cart :
+
+.. code-block:: shell
+
+  ros2 run gazebo_ros2_control_demos example_position_pid
+
+
+Similarly, for the velocity PID example, execute the following command:
+
+.. code-block:: shell
+
+  ros2 launch gazebo_ros2_control_demos vertical_cart_example_velocity_pid.launch.py
+
+When the Gazebo world is launched you can run the following commands to move the cart :
+
+.. code-block:: shell
+
+  ros2 run gazebo_ros2_control_demos example_velocity
+
 Add the gazebo_ros2_control plugin
 ==========================================
 
@@ -286,6 +311,8 @@ You can run some of the configuration running the following commands:
   ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py
   ros2 launch gazebo_ros2_control_demos diff_drive.launch.py
   ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py
+  ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pid.launch.py
+  ros2 launch gazebo_ros2_control_demos vertical_cart_example_velocity_pid.launch.py
 
 
 When the Gazebo world is launched you can run some of the following commands to move the cart.
@@ -293,6 +320,7 @@ When the Gazebo world is launched you can run some of the following commands to
 .. code-block:: shell
 
   ros2 run gazebo_ros2_control_demos example_position
+  ros2 run gazebo_ros2_control_demos example_position_pid
   ros2 run gazebo_ros2_control_demos example_velocity
   ros2 run gazebo_ros2_control_demos example_effort
   ros2 run gazebo_ros2_control_demos example_diff_drive