From 55f636440232d12a4c4c33ba0a31df37bfc05ab9 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 23 Feb 2023 17:42:12 +0100 Subject: [PATCH] Introuduce gen param library. --- ign_ros2_control/CMakeLists.txt | 26 ++- .../include/ign_ros2_control/ign_system.hpp | 10 + .../validate_system_parameters.hpp | 13 ++ ign_ros2_control/package.xml | 1 + .../src/ign_ros2_control_parameters.yaml | 51 +++++ ign_ros2_control/src/ign_system.cpp | 200 +++++++++++------- 6 files changed, 227 insertions(+), 74 deletions(-) create mode 100644 ign_ros2_control/include/ign_ros2_control/validate_system_parameters.hpp create mode 100644 ign_ros2_control/src/ign_ros2_control_parameters.yaml diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index f11c290f..d1ea3931 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -23,6 +23,12 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(ign_ros2_control_parameters + src/ign_ros2_control_parameters.yaml + include/ign_ros2_control/validate_system_parameters.hpp +) if("$ENV{IGNITION_VERSION}" STREQUAL "citadel") find_package(ignition-gazebo3 REQUIRED) @@ -82,9 +88,13 @@ target_link_libraries(ign_hardware_plugins ignition-gazebo${IGN_GAZEBO_VER}::core ) +target_link_libraries(ign_hardware_plugins + ign_ros2_control_parameters +) + ## Install -install(TARGETS - ign_hardware_plugins +install(TARGETS ign_hardware_plugins ign_ros2_control_parameters + EXPORT export_ign_ros2_control ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -99,6 +109,9 @@ endif() ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins) +ament_export_targets( + export_ign_ros2_control HAS_LIBRARY_TARGET +) # Install directories install(TARGETS ${PROJECT_NAME}-system DESTINATION lib @@ -106,5 +119,14 @@ install(TARGETS ${PROJECT_NAME}-system pluginlib_export_plugin_description_file(ign_ros2_control ign_hardware_plugins.xml) +ament_export_dependencies( + ament_index_cpp + controller_manager + hardware_interface + pluginlib + rclcpp + yaml_cpp_vendor + rclcpp_lifecycle +) # Setup the project ament_package() diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp index 08dfd847..5ef630a2 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp @@ -25,6 +25,8 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ign_ros2_control_parameters.hpp" + namespace ign_ros2_control { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -88,6 +90,14 @@ class IgnitionSystem : public IgnitionSystemInterface /// \brief Private data class std::unique_ptr dataPtr; + + rcl_interfaces::msg::SetParametersResult myCallback(const std::vector & parameters); + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr cb_; + + rclcpp::Node::SharedPtr node_; + + std::thread spin_thread_; + }; } // namespace ign_ros2_control diff --git a/ign_ros2_control/include/ign_ros2_control/validate_system_parameters.hpp b/ign_ros2_control/include/ign_ros2_control/validate_system_parameters.hpp new file mode 100644 index 00000000..36720d2f --- /dev/null +++ b/ign_ros2_control/include/ign_ros2_control/validate_system_parameters.hpp @@ -0,0 +1,13 @@ +#ifndef IGN_ROS2_CONTROL__VALIDATE_SYSTEM_PARAMETERS_HPP_ +#define IGN_ROS2_CONTROL__VALIDATE_SYSTEM_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" + +namespace parameter_traits +{ + +} // namespace parameter_traits + +#endif // IGN_ROS2_CONTROL__VALIDATE_SYSTEM_PARAMETERS_HPP_ diff --git a/ign_ros2_control/package.xml b/ign_ros2_control/package.xml index 90195285..7d31d748 100644 --- a/ign_ros2_control/package.xml +++ b/ign_ros2_control/package.xml @@ -22,6 +22,7 @@ rclcpp_lifecycle hardware_interface controller_manager + generate_parameter_library ament_lint_auto ament_lint_common diff --git a/ign_ros2_control/src/ign_ros2_control_parameters.yaml b/ign_ros2_control/src/ign_ros2_control_parameters.yaml new file mode 100644 index 00000000..a21f1770 --- /dev/null +++ b/ign_ros2_control/src/ign_ros2_control_parameters.yaml @@ -0,0 +1,51 @@ +ignition_system_interface: + joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller", + validation: { + unique<>: null, + } + } + gains: + __map_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + iMax: { + type: double, + default_value: 0.0, + description: "Integral positive clamp." + } + iMin: { + type: double, + default_value: 0.0, + description: "Integral negative clamp." + } + cmdMax: { + type: double, + default_value: 0.0, + description: "Maximum value for the PID command." + } + cmdMin: { + type: double, + default_value: 0.0, + description: "Maximum value for the PID command." + } + cmdOffset: { + type: double, + default_value: 0.0, + description: "Offset value for the command which is added to the result of the PID controller." + } \ No newline at end of file diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 19eac9ba..bf1c6ef9 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -352,32 +352,41 @@ bool IgnitionSystem::initSim( // init position PID // assuming every joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); // PID parameters - double p_gain = 100.0; - double i_gain = 0.0; - double d_gain = 50.0; + double effort_range = jointAxis->Data().Effort() * 2; + double position_range = jointAxis->Data().Upper() - jointAxis->Data().Lower(); + double p_gain = 10*effort_range / position_range; //500.0; + double i_gain = p_gain*0.0; + double d_gain = p_gain/2.0; // set integral max and min component to 10 percent of the max effort - double iMax = std::isnan(jointAxis->Data().Effort()) ? 1.0 : jointAxis->Data().Effort() * 0.1; - double iMin = std::isnan(jointAxis->Data().Effort()) ? 1.0 : -jointAxis->Data().Effort() * 0.1; - double cmdMax = std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); - double cmdMin = std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); + double iMax = std::isnan(jointAxis->Data().Effort()) ? 1.0 : jointAxis->Data().Effort() * + 0.5; + double iMin = std::isnan(jointAxis->Data().Effort()) ? 1.0 : -jointAxis->Data().Effort() * + 0.5; + double cmdMax = + std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); + double cmdMin = + std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); double cmdOffset = 0.0; - igndbg << "[JointController "<dataPtr->joints_[j].pid.Init(p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, cmdOffset); + this->dataPtr->joints_[j].pid.Init( + p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, + cmdOffset); } else if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); @@ -411,9 +420,36 @@ bool IgnitionSystem::initSim( registerSensors(hardware_info); - return true; + this->node_ = rclcpp::Node::make_shared(hardware_info.name, + rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)); + + auto spin = [this]() + { + rclcpp::spin(node_); + }; + + spin_thread_ = std::thread(spin); + + cb_ = this->node_->add_on_set_parameters_callback(std::bind(&IgnitionSystem::myCallback, this, std::placeholders::_1)); + + + return true; } + rcl_interfaces::msg::SetParametersResult IgnitionSystem::myCallback(const std::vector ¶meters) { + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + for (const auto ¶m: parameters) + { + RCLCPP_INFO(this->node_->get_logger(), "%s", param.get_name().c_str()); + RCLCPP_INFO(this->node_->get_logger(), "%s", param.get_type_name().c_str()); + RCLCPP_INFO(this->node_->get_logger(), "%s", param.value_to_string().c_str()); + } + return result; + } + void IgnitionSystem::registerSensors( const hardware_interface::HardwareInfo & hardware_info) { @@ -522,6 +558,7 @@ CallbackReturn IgnitionSystem::on_activate(const rclcpp_lifecycle::State & previ CallbackReturn IgnitionSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) { + if (spin_thread_.joinable()) spin_thread_.join(); return CallbackReturn::SUCCESS; return hardware_interface::SystemInterface::on_deactivate(previous_state); } @@ -623,10 +660,10 @@ hardware_interface::return_type IgnitionSystem::write( const rclcpp::Duration & period) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - // assuming every joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + // assuming every joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( @@ -644,27 +681,37 @@ hardware_interface::return_type IgnitionSystem::write( } } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { - // calculate error with clamped position command - double error = this->dataPtr->joints_[i].joint_position - std::clamp(this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - - // error can be maximal 10 percent of the range - error = copysign(1.0, error) * std::clamp(std::abs(error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())*0.1); - - // calculate target force/torque - double target_force = this->dataPtr->joints_[i].pid.Update(error, std::chrono::duration(period.to_chrono())); - - auto forceCmd = - this->dataPtr->ecm->Component(this->dataPtr->joints_[i].sim_joint); + // calculate error with clamped position command + double error = this->dataPtr->joints_[i].joint_position - std::clamp( + this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); + + // error can be maximal 10 percent of the range + error = + copysign( + 1.0, + error) * + std::clamp( + std::abs(error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 1.0); + + // calculate target force/torque + double target_force = this->dataPtr->joints_[i].pid.Update( + error, std::chrono::duration( + period.to_chrono())); + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); - if (forceCmd == nullptr) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) @@ -699,41 +746,50 @@ hardware_interface::return_type IgnitionSystem::write( } // set values of all mimic joints with respect to mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + /*for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { - // assuming every mimic joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + // assuming every mimic joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); if (mimic_interface == "position") { // Get the joint position - double position_mimicked_joint = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position; + double position_mimicked_joint = + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position; - double position_mimic_joint = this->dataPtr->joints_[mimic_joint.joint_index].joint_position; + double position_mimic_joint = + this->dataPtr->joints_[mimic_joint.joint_index].joint_position; - // calculate error with clamped position command - double position_error = - position_mimic_joint - std::clamp(position_mimicked_joint * mimic_joint.multiplier, jointAxis->Data().Lower(), jointAxis->Data().Upper()); + // calculate error with clamped position command + double position_error = + position_mimic_joint - std::clamp( + position_mimicked_joint * mimic_joint.multiplier, + jointAxis->Data().Lower(), jointAxis->Data().Upper()); - // error can be maximal 10 percent of the range - position_error = copysign(1.0, position_error) * std::clamp(std::abs(position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())*0.1); + // error can be maximal 10 percent of the range + position_error = copysign(1.0, position_error) * std::clamp( + std::abs( + position_error), 0.0, std::abs( + jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); - // calculate target force/torque - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update(position_error, std::chrono::duration(period.to_chrono())); + // calculate target force/torque + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update( + position_error, + std::chrono::duration(period.to_chrono())); - auto forceCmd = - this->dataPtr->ecm->Component(this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - if (forceCmd == nullptr) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } }/* if (mimic_interface == "velocity") { // get the velocity of mimicked joint @@ -775,9 +831,9 @@ hardware_interface::return_type IgnitionSystem::write( {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); } - }*/ + } } - } + }*/ return hardware_interface::return_type::OK; }