From 286a14f450d3a9d88ab40c3dab8f4cbe6432e47c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 8 Feb 2023 13:23:14 -0600 Subject: [PATCH 1/5] Don't initialize to zero --- ign_ros2_control/src/ign_system.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 95244cba..b8999c1c 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -589,7 +589,7 @@ hardware_interface::return_type IgnitionSystem::write( { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); + ignition::gazebo::components::JointVelocityCmd({this->dataPtr->joints_[i].joint_velocity_cmd})); } else { const auto jointVelCmd = this->dataPtr->ecm->Component( @@ -606,16 +606,18 @@ hardware_interface::return_type IgnitionSystem::write( // Calculate target velcity double target_vel = -this->dataPtr->position_proportional_gain_ * error; - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); - - if (vel == nullptr) { + if (!this->dataPtr->ecm->Component( + 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( + 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( @@ -623,7 +625,7 @@ hardware_interface::return_type IgnitionSystem::write( { 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( From 6a9123cf4960a4315bd7895017a5943adfc180d2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 8 Feb 2023 14:32:45 -0600 Subject: [PATCH 2/5] Delete duplicated code --- ign_ros2_control/src/ign_system.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index b8999c1c..cea61355 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -644,10 +644,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; + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( + {target_vel}); } } } From 57a8a08c09b4e58c1a7cf20a9d255554544ada12 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 9 Feb 2023 15:52:19 -0600 Subject: [PATCH 3/5] Add full PID control --- ign_ros2_control/CMakeLists.txt | 11 ++-- ign_ros2_control/package.xml | 7 ++- ign_ros2_control/src/ign_system.cpp | 89 ++++++++++++++++++++++------- 3 files changed, 76 insertions(+), 31 deletions(-) diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index f11c290f..54f0832d 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -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) @@ -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 ) ######### @@ -74,15 +75,15 @@ 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 @@ -90,7 +91,6 @@ install(TARGETS RUNTIME DESTINATION bin ) -# Testing and linting if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -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() diff --git a/ign_ros2_control/package.xml b/ign_ros2_control/package.xml index 8fba1d44..4a33543c 100644 --- a/ign_ros2_control/package.xml +++ b/ign_ros2_control/package.xml @@ -15,13 +15,14 @@ ignition-gazebo3 ignition-gazebo5 ignition-gazebo6 + control_toolbox + controller_manager + hardware_interface ignition-plugin pluginlib + rclcpp_lifecycle rclcpp yaml_cpp_vendor - rclcpp_lifecycle - hardware_interface - controller_manager ament_lint_auto ament_lint_common diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index cea61355..27fc665d 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -38,7 +38,7 @@ #include - +#include #include struct jointData @@ -149,8 +149,9 @@ class ign_ros2_control::IgnitionSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector 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 joint_position_pids_; }; namespace ign_ros2_control @@ -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); - } - if (this->dataPtr->n_dof_ == 0) { RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "There is no joint available"); return false; @@ -371,6 +361,49 @@ bool IgnitionSystem::initSim( registerSensors(hardware_info); + // Initialize PID controllers for converting position commands to Gazebo joint velocities + double proportional_gain = 1.0; + double integral_gain = 0.1; + double derivative_gain = 0.1; + 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; + if (!this->nh_->get_parameter_or( + joint_name + "/position_proportional_gain", 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); + } + + if (!this->nh_->get_parameter_or( + joint_name + "/position_integral_gain", 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); + } + + if (!this->nh_->get_parameter_or( + joint_name + "/position_derivative_gain", 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; } @@ -476,13 +509,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); } @@ -589,7 +626,9 @@ hardware_interface::return_type IgnitionSystem::write( { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({this->dataPtr->joints_[i].joint_velocity_cmd})); + ignition::gazebo::components::JointVelocityCmd( + {this->dataPtr->joints_[i]. + joint_velocity_cmd})); } else { const auto jointVelCmd = this->dataPtr->ecm->Component( @@ -599,12 +638,17 @@ 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)); + + double e_p, e_i, e_d; + this->dataPtr->joint_position_pids_.at(i).getCurrentPIDErrors(e_p, e_i, e_d); if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) @@ -625,7 +669,8 @@ hardware_interface::return_type IgnitionSystem::write( { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({this->dataPtr->joints_[i].joint_effort_cmd})); + ignition::gazebo::components::JointForceCmd( + {this->dataPtr->joints_[i].joint_effort_cmd})); } else { const auto jointEffortCmd = this->dataPtr->ecm->Component( From 94a6c4a953f9450191ef3a72267f565e8e058e32 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 10 Feb 2023 08:01:31 -0600 Subject: [PATCH 4/5] Kd is unstable --- ign_ros2_control/src/ign_system.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 27fc665d..5c99f276 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -362,9 +362,9 @@ bool IgnitionSystem::initSim( registerSensors(hardware_info); // Initialize PID controllers for converting position commands to Gazebo joint velocities - double proportional_gain = 1.0; - double integral_gain = 0.1; - double derivative_gain = 0.1; + 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; @@ -387,7 +387,9 @@ bool IgnitionSystem::initSim( "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. +/* if (!this->nh_->get_parameter_or( joint_name + "/position_derivative_gain", derivative_gain, derivative_gain)) @@ -397,7 +399,7 @@ bool IgnitionSystem::initSim( "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, From 0c4da2204c27acdf97a3002a14507c39561d22fe Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 12 Feb 2023 09:37:06 -0600 Subject: [PATCH 5/5] Fix parameter declaration --- ign_ros2_control/src/ign_system.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 5c99f276..e14f17bb 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -366,11 +366,12 @@ bool IgnitionSystem::initSim( 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++) { + for (unsigned int j = 0; j < this->dataPtr->n_dof_; ++j) { const std::string joint_name = this->dataPtr->joints_[j].name; - if (!this->nh_->get_parameter_or( - joint_name + "/position_proportional_gain", proportional_gain, - proportional_gain)) + + this->nh_->declare_parameter(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(), @@ -378,9 +379,9 @@ bool IgnitionSystem::initSim( ", defaulting to: " << proportional_gain); } - if (!this->nh_->get_parameter_or( - joint_name + "/position_integral_gain", integral_gain, - integral_gain)) + this->nh_->declare_parameter(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(), @@ -390,9 +391,9 @@ bool IgnitionSystem::initSim( // TODO(andyz): a nonzero derivative gain seems to be destabilizing. Investigate in the // control_toolbox. /* - if (!this->nh_->get_parameter_or( - joint_name + "/position_derivative_gain", derivative_gain, - derivative_gain)) + this->nh_->declare_parameter(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(),