Skip to content

Commit

Permalink
Add support for getting PID parameters from loaded parameters (#374)
Browse files Browse the repository at this point in the history
Co-authored-by: Christoph Fröhlich <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
3 people authored Sep 16, 2024
1 parent fc97344 commit 6a4cc84
Show file tree
Hide file tree
Showing 6 changed files with 357 additions and 50 deletions.
35 changes: 35 additions & 0 deletions doc/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,30 @@ Where the parameters are as follows:

The same definitions apply to the ``vel_*`` parameters.

The PID parameters can be defined for ``position`` or ``position_pid`` and ``velocity`` or ``velocity_pid`` command interfaces as explained above, or defining them in a YAML file and loading it in the ``gazebo_ros2_control`` plugin as below:

.. code-block:: yaml
gazebo_ros2_control:
ros__parameters:
pid_gains:
position_pid: # (or) position
slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0}
.. code-block:: xml
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
...
<ros>
<argument>--ros-args</argument>
<argument>--params-file</argument>
<argument>Path to the configuration file</argument>
</ros>
</plugin>
</gazebo>
Add the gazebo_ros2_control plugin
==========================================

Expand Down Expand Up @@ -386,10 +410,21 @@ The following examples shows a vertical cart control by a PID joint using positi

.. code-block:: shell
ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pids_in_yaml.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
.. code-block:: shell
ros2 run gazebo_ros2_control_demos example_position_pid
ros2 run gazebo_ros2_control_demos example_velocity
The ``vertical_cart_example_position_pids_in_yaml.launch.py`` example uses a YAML file as following to set the PID gains:

.. code-block:: yaml
gazebo_ros2_control:
ros__parameters:
pid_gains:
position:
slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0}
Original file line number Diff line number Diff line change
Expand Up @@ -90,9 +90,12 @@ 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);
bool extractPID(
const std::string & prefix,
const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid);

bool extractPIDFromParameters(
const std::string & control_mode, const std::string & joint_name, control_toolbox::Pid & pid);

/// \brief Private data class
std::unique_ptr<GazeboSystemPrivate> dataPtr;
Expand Down
182 changes: 135 additions & 47 deletions gazebo_ros2_control/src/gazebo_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <cmath>
#include <limits>
#include <map>
#include <memory>
Expand Down Expand Up @@ -110,6 +111,12 @@ class gazebo_ros2_control::GazeboSystemPrivate

// Should hold the joints if no control_mode is active
bool hold_joints_ = true;

// Current position and velocity control method
GazeboSystemInterface::ControlMethod_ position_control_method_ =
GazeboSystemInterface::ControlMethod_::POSITION;
GazeboSystemInterface::ControlMethod_ velocity_control_method_ =
GazeboSystemInterface::ControlMethod_::VELOCITY;
};

namespace gazebo_ros2_control
Expand Down Expand Up @@ -157,7 +164,8 @@ bool GazeboSystem::initSim(
this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
}
RCLCPP_DEBUG_STREAM(
this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl);
this->nh_->get_logger(),
"hold_joints (system): " << std::boolalpha << this->dataPtr->hold_joints_ << std::endl);

registerJoints(hardware_info, parent_model);
registerSensors(hardware_info, parent_model);
Expand All @@ -170,9 +178,9 @@ bool GazeboSystem::initSim(
return true;
}

control_toolbox::Pid GazeboSystem::extractPID(
std::string prefix,
hardware_interface::ComponentInfo joint_info)
bool GazeboSystem::extractPID(
const std::string & prefix,
const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid)
{
double kp;
double ki;
Expand All @@ -181,52 +189,127 @@ control_toolbox::Pid GazeboSystem::extractPID(
double min_integral_error;
bool antiwindup = false;

bool are_pids_set = false;
if (joint_info.parameters.find(prefix + "kp") != joint_info.parameters.end()) {
kp = std::stod(joint_info.parameters.at(prefix + "kp"));
are_pids_set = true;
} else {
kp = 0.0;
}

if (joint_info.parameters.find(prefix + "ki") != joint_info.parameters.end()) {
ki = std::stod(joint_info.parameters.at(prefix + "ki"));
are_pids_set = true;
} else {
ki = 0.0;
}

if (joint_info.parameters.find(prefix + "kd") != joint_info.parameters.end()) {
kd = std::stod(joint_info.parameters.at(prefix + "kd"));
are_pids_set = true;
} 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 (are_pids_set) {
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;
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;
}
}

RCLCPP_INFO_STREAM(
this->nh_->get_logger(),
"Setting kp = " << kp << "\t"
<< " ki = " << ki << "\t"
<< " kd = " << kd << "\t"
<< " max_integral_error = " << max_integral_error << "\t"
<< "antiwindup =" << std::boolalpha << antiwindup);

pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
}
return are_pids_set;
}

RCLCPP_INFO_STREAM(
this->nh_->get_logger(),
"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);
bool GazeboSystem::extractPIDFromParameters(
const std::string & control_mode, const std::string & joint_name,
control_toolbox::Pid & pid)
{
const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name;
auto get_pid_entry = [this, parameter_prefix](const std::string & entry, double & value) -> bool {
try {
// Check if the parameter is declared, if not, declare the default value NaN
if (!this->nh_->has_parameter(parameter_prefix + "." + entry)) {
this->nh_->declare_parameter<double>(
parameter_prefix + "." + entry,
std::numeric_limits<double>::quiet_NaN());
}
value = this->nh_->get_parameter(parameter_prefix + "." + entry).as_double();
} catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
RCLCPP_ERROR(
this->nh_->get_logger(),
"Parameter '%s' not declared, with error %s", entry.c_str(), ex.what());
return false;
} catch (rclcpp::exceptions::InvalidParameterTypeException & ex) {
RCLCPP_ERROR(
this->nh_->get_logger(),
"Parameter '%s' has wrong type: %s", entry.c_str(), ex.what());
return false;
}
return std::isfinite(value);
};
bool are_pids_set = true;
double kp, ki, kd, max_integral_error, min_integral_error;
bool antiwindup;
are_pids_set &= get_pid_entry("kp", kp);
are_pids_set &= get_pid_entry("ki", ki);
are_pids_set &= get_pid_entry("kd", kd);
if (are_pids_set) {
get_pid_entry("max_integral_error", max_integral_error);
get_pid_entry("min_integral_error", min_integral_error);
const std::string antiwindup_str = "antiwindup";
if (!this->nh_->has_parameter(parameter_prefix + "." + antiwindup_str)) {
this->nh_->declare_parameter(
parameter_prefix + "." + antiwindup_str,
false);
}
try {
antiwindup = this->nh_->get_parameter(parameter_prefix + "." + antiwindup_str).as_bool();
} catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
RCLCPP_ERROR(
this->nh_->get_logger(),
"Parameter '%s' not declared, with error %s", antiwindup_str.c_str(), ex.what());
} catch (rclcpp::ParameterTypeException & ex) {
RCLCPP_ERROR(
this->nh_->get_logger(),
"Parameter '%s' has wrong type: %s", antiwindup_str.c_str(), ex.what());
}
RCLCPP_INFO_STREAM(
this->nh_->get_logger(),
"Setting kp = " << kp << "\t"
<< " ki = " << ki << "\t"
<< " kd = " << kd << "\t"
<< " max_integral_error = " << max_integral_error << "\t"
<< "antiwindup =" << std::boolalpha << antiwindup << " from node parameters");
pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
}

return are_pids_set;
}

void GazeboSystem::registerJoints(
Expand All @@ -251,7 +334,7 @@ void GazeboSystem::registerJoints(

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;
const 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);
Expand Down Expand Up @@ -352,20 +435,24 @@ void GazeboSystem::registerJoints(
if (has_already_registered_pos) {
RCLCPP_ERROR_STREAM(
this->nh_->get_logger(),
"can't have position and position"
<< "pid command_interface at same time !!!");
"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);

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->is_pos_pid[j] = this->extractPID(
POSITION_PID_PARAMS_PREFIX, joint_info,
this->dataPtr->pos_pid[j]) || this->extractPIDFromParameters(
joint_info.command_interfaces[i].name, joint_name, this->dataPtr->pos_pid[j]);

if (this->dataPtr->is_pos_pid[j]) {
RCLCPP_INFO_STREAM(
this->nh_->get_logger(), "Found position PIDs for joint: " << joint_name << "!");
this->dataPtr->position_control_method_ = POSITION_PID;
}

this->dataPtr->command_interfaces_.emplace_back(
Expand Down Expand Up @@ -395,10 +482,17 @@ void GazeboSystem::registerJoints(
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->is_vel_pid[j] = this->extractPID(
VELOCITY_PID_PARAMS_PREFIX, joint_info,
this->dataPtr->vel_pid[j]) || this->extractPIDFromParameters(
joint_info.command_interfaces[i].name, joint_name, this->dataPtr->vel_pid[j]);

if (this->dataPtr->is_vel_pid[j]) {
RCLCPP_INFO_STREAM(
this->nh_->get_logger(), "Found velocity PIDs for joint: " << joint_name << "!");
this->dataPtr->velocity_control_method_ = VELOCITY_PID;
}

this->dataPtr->command_interfaces_.emplace_back(
joint_name,
hardware_interface::HW_IF_VELOCITY,
Expand Down Expand Up @@ -641,19 +735,13 @@ GazeboSystem::perform_command_mode_switch(
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 {
this->dataPtr->joint_control_methods_[j] |= POSITION_PID;
}
this->dataPtr->joint_control_methods_[j] |=
static_cast<ControlMethod_>(this->dataPtr->position_control_method_);
} 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;
}
this->dataPtr->joint_control_methods_[j] |=
static_cast<ControlMethod_>(this->dataPtr->velocity_control_method_);
} else if (interface_name == // NOLINT
(this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
{
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
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

gazebo_ros2_control:
ros__parameters:
pid_gains:
position:
slider_to_cart: {kp: 100.0, kd: 10.0, ki: 1.0, max_integral_error: 10000.0}
Loading

0 comments on commit 6a4cc84

Please sign in to comment.