Skip to content

Add full PID control, plus small bug fixes #117

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 5 commits into from
Closed

Add full PID control, plus small bug fixes #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
286a14f
Don't initialize to zero
AndyZe Feb 8, 2023
6a9123c
Delete duplicated code
AndyZe Feb 8, 2023
57a8a08
Add full PID control
AndyZe Feb 9, 2023
94a6c4a
Kd is unstable
AndyZe Feb 10, 2023
0c4da22
Fix parameter declaration
AndyZe Feb 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 5 additions & 6 deletions ign_ros2_control/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(controller_manager REQUIRED)
find_package(control_toolbox REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
Expand Down Expand Up @@ -53,19 +54,19 @@ include_directories(include)
add_library(${PROJECT_NAME}-system SHARED
src/ign_ros2_control_plugin.cpp
)

target_link_libraries(${PROJECT_NAME}-system
ignition-gazebo${IGN_GAZEBO_VER}::core
ignition-plugin${IGN_PLUGIN_VER}::register
)
ament_target_dependencies(${PROJECT_NAME}-system
ament_index_cpp
control_toolbox
controller_manager
hardware_interface
pluginlib
rclcpp
yaml_cpp_vendor
rclcpp_lifecycle
yaml_cpp_vendor
)

#########
Expand All @@ -74,23 +75,22 @@ add_library(ign_hardware_plugins SHARED
src/ign_system.cpp
)
ament_target_dependencies(ign_hardware_plugins
rclcpp_lifecycle
control_toolbox
hardware_interface
rclcpp
rclcpp_lifecycle
)
target_link_libraries(ign_hardware_plugins
ignition-gazebo${IGN_GAZEBO_VER}::core
)

## Install
install(TARGETS
ign_hardware_plugins
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)

# Testing and linting
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
Expand All @@ -106,5 +106,4 @@ install(TARGETS ${PROJECT_NAME}-system

pluginlib_export_plugin_description_file(ign_ros2_control ign_hardware_plugins.xml)

# Setup the project
ament_package()
7 changes: 4 additions & 3 deletions ign_ros2_control/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,14 @@
<depend condition="$IGNITION_VERSION == citadel">ignition-gazebo3</depend>
<depend condition="$IGNITION_VERSION == edifice">ignition-gazebo5</depend>
<depend condition="$IGNITION_VERSION == fortress">ignition-gazebo6</depend>
<depend>control_toolbox</depend>
<depend>controller_manager</depend>
<depend>hardware_interface</depend>
<depend>ignition-plugin</depend>
<depend>pluginlib</depend>
<depend>rclcpp_lifecycle</depend>
<depend>rclcpp</depend>
<depend>yaml_cpp_vendor</depend>
<depend>rclcpp_lifecycle</depend>
<depend>hardware_interface</depend>
<depend>controller_manager</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
116 changes: 84 additions & 32 deletions ign_ros2_control/src/ign_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@

#include <ignition/transport/Node.hh>


#include <control_toolbox/pid.hpp>
#include <hardware_interface/hardware_info.hpp>

struct jointData
Expand Down Expand Up @@ -149,8 +149,9 @@ class ign_ros2_control::IgnitionSystemPrivate
/// \brief mapping of mimicked joints to index of joint they mimic
std::vector<MimicJoint> mimic_joints_;

/// \brief Gain which converts position error to a velocity command
double position_proportional_gain_;
/// \brief PID controllers for each joint, used in converting position commands to Gazebo joint
// velocities
std::vector<control_toolbox::Pid> joint_position_pids_;
};

namespace ign_ros2_control
Expand All @@ -175,17 +176,6 @@ bool IgnitionSystem::initSim(

this->dataPtr->joints_.resize(this->dataPtr->n_dof_);

constexpr double default_gain = 0.1;
if (!this->nh_->get_parameter_or(
"position_proportional_gain",
this->dataPtr->position_proportional_gain_, default_gain))
{
RCLCPP_WARN_STREAM(
this->nh_->get_logger(),
"The position_proportional_gain parameter was not defined, defaulting to: " <<
default_gain);
}
Copy link
Contributor Author

@AndyZe AndyZe Feb 9, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I moved this PID initialization stuff to the bottom of the function so that joint names would be available for the debug prints.


if (this->dataPtr->n_dof_ == 0) {
RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "There is no joint available");
return false;
Expand Down Expand Up @@ -371,6 +361,52 @@ bool IgnitionSystem::initSim(

registerSensors(hardware_info);

// Initialize PID controllers for converting position commands to Gazebo joint velocities
double proportional_gain = 10.0;
double integral_gain = 1.0;
double derivative_gain = 0.0;
this->dataPtr->joint_position_pids_.resize(this->dataPtr->n_dof_);
for (unsigned int j = 0; j < this->dataPtr->n_dof_; ++j) {
const std::string joint_name = this->dataPtr->joints_[j].name;

this->nh_->declare_parameter<double>(joint_name + "/position_proportional_gain", proportional_gain);
if (!this->nh_->get_parameter(
joint_name + "/position_proportional_gain", proportional_gain))
{
RCLCPP_WARN_STREAM(
this->nh_->get_logger(),
"The position_proportional_gain parameter was not defined for joint " << joint_name <<
", defaulting to: " << proportional_gain);
}

this->nh_->declare_parameter<double>(joint_name + "/position_integral_gain", integral_gain);
if (!this->nh_->get_parameter(
joint_name + "/position_integral_gain", integral_gain))
{
RCLCPP_WARN_STREAM(
this->nh_->get_logger(),
"The position_integral_gain parameter was not defined for joint " << joint_name <<
", defaulting to: " << integral_gain);
}
// TODO(andyz): a nonzero derivative gain seems to be destabilizing. Investigate in the
// control_toolbox.
/*
this->nh_->declare_parameter<double>(joint_name + "/position_derivative_gain", derivative_gain);
if (!this->nh_->get_parameter(
joint_name + "/position_derivative_gain", derivative_gain))
{
RCLCPP_WARN_STREAM(
this->nh_->get_logger(),
"The position_derivative_gain parameter was not defined for joint " << joint_name <<
", defaulting to: " << derivative_gain);
}
*/
this->dataPtr->joint_position_pids_.at(j) = control_toolbox::Pid(
proportional_gain,
integral_gain,
derivative_gain);
}

return true;
}

Expand Down Expand Up @@ -476,13 +512,17 @@ IgnitionSystem::export_command_interfaces()

CallbackReturn IgnitionSystem::on_activate(const rclcpp_lifecycle::State & previous_state)
{
return CallbackReturn::SUCCESS;
for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
this->dataPtr->joint_position_pids_.at(j).reset();
}
return hardware_interface::SystemInterface::on_activate(previous_state);
}

CallbackReturn IgnitionSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state)
{
return CallbackReturn::SUCCESS;
for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
this->dataPtr->joint_position_pids_.at(j).reset();
}
return hardware_interface::SystemInterface::on_deactivate(previous_state);
}

Expand Down Expand Up @@ -589,7 +629,9 @@ hardware_interface::return_type IgnitionSystem::write(
{
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[i].sim_joint,
ignition::gazebo::components::JointVelocityCmd({0}));
Copy link
Contributor Author

@AndyZe AndyZe Feb 9, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't believe it makes sense to initialize this to 0 when we have a command available.

ignition::gazebo::components::JointVelocityCmd(
{this->dataPtr->joints_[i].
joint_velocity_cmd}));
} else {
const auto jointVelCmd =
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
Expand All @@ -599,31 +641,39 @@ hardware_interface::return_type IgnitionSystem::write(
}
} else if (this->dataPtr->joints_[i].joint_control_method & POSITION) {
// Get error in position
double error;
error = (this->dataPtr->joints_[i].joint_position -
this->dataPtr->joints_[i].joint_position_cmd) * *this->dataPtr->update_rate;
double error = this->dataPtr->joints_[i].joint_position_cmd -
this->dataPtr->joints_[i].joint_position;

// Calculate target velcity
double target_vel = -this->dataPtr->position_proportional_gain_ * error;
double target_vel =
this->dataPtr->joint_position_pids_.at(i).computeCommand(
error,
uint64_t(1e9 / *this->dataPtr->update_rate));

auto vel =
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
this->dataPtr->joints_[i].sim_joint);
double e_p, e_i, e_d;
this->dataPtr->joint_position_pids_.at(i).getCurrentPIDErrors(e_p, e_i, e_d);

if (vel == nullptr) {
if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
this->dataPtr->joints_[i].sim_joint))
{
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[i].sim_joint,
ignition::gazebo::components::JointVelocityCmd({target_vel}));
} else if (!vel->Data().empty()) {
vel->Data()[0] = target_vel;
} else {
const auto jointVelCmd =
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
this->dataPtr->joints_[i].sim_joint);
*jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
{target_vel});
}
} else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) {
if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
this->dataPtr->joints_[i].sim_joint))
{
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[i].sim_joint,
ignition::gazebo::components::JointForceCmd({0}));
ignition::gazebo::components::JointForceCmd(
{this->dataPtr->joints_[i].joint_effort_cmd}));
} else {
const auto jointEffortCmd =
this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
Expand All @@ -642,10 +692,12 @@ hardware_interface::return_type IgnitionSystem::write(
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[i].sim_joint,
ignition::gazebo::components::JointVelocityCmd({target_vel}));
} else if (!vel->Data().empty()) {
vel->Data()[0] = target_vel;
} else if (!vel->Data().empty()) {
vel->Data()[0] = target_vel;
Comment on lines -647 to -648
Copy link
Contributor Author

@AndyZe AndyZe Feb 9, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Whoops, I duplicated this block of code. Plus, I think it is safer to use:

        const auto jointVelCmd =
          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
          this->dataPtr->joints_[i].sim_joint);
        *jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
          {target_vel});

Than...

vel->Data()[0] = target_vel;

I might be wrong, though.

} else {
const auto jointVelCmd =
this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
this->dataPtr->joints_[i].sim_joint);
*jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
{target_vel});
}
}
}
Expand Down