From 2d983a95eb271e9367e39da80a01e39615bebbf5 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 7 Jun 2021 21:46:30 -0700 Subject: [PATCH] wip, effort_group_position_controller Signed-off-by: Karsten Knese --- effort_controllers/CMakeLists.txt | 5 + .../effort_controllers_plugins.xml | 5 + .../joint_group_position_controller.hpp | 74 ++++++++ effort_controllers/package.xml | 2 + .../src/joint_group_position_controller.cpp | 160 ++++++++++++++++++ 5 files changed, 246 insertions(+) create mode 100644 effort_controllers/include/effort_controllers/joint_group_position_controller.hpp create mode 100644 effort_controllers/src/joint_group_position_controller.cpp diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index ac95d8567c..155bf21f09 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -12,6 +12,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(control_toolbox REQUIRED) find_package(forward_command_controller REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -19,9 +21,12 @@ find_package(rclcpp REQUIRED) add_library(effort_controllers SHARED src/joint_group_effort_controller.cpp + src/joint_group_position_controller.cpp ) target_include_directories(effort_controllers PRIVATE include) ament_target_dependencies(effort_controllers + angles + control_toolbox forward_command_controller pluginlib rclcpp diff --git a/effort_controllers/effort_controllers_plugins.xml b/effort_controllers/effort_controllers_plugins.xml index 3f7d5853ae..f1652c745e 100644 --- a/effort_controllers/effort_controllers_plugins.xml +++ b/effort_controllers/effort_controllers_plugins.xml @@ -4,4 +4,9 @@ The joint effort controller commands a group of joints through the effort interface + + + The joint position controller commands a group of joints through the effort interface + + diff --git a/effort_controllers/include/effort_controllers/joint_group_position_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_position_controller.hpp new file mode 100644 index 0000000000..26925a659d --- /dev/null +++ b/effort_controllers/include/effort_controllers/joint_group_position_controller.hpp @@ -0,0 +1,74 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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. + +#ifndef EFFORT_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ +#define EFFORT_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ + +#include + +#include "control_toolbox/pid.hpp" +#include "forward_command_controller/forward_command_controller.hpp" +#include "effort_controllers/visibility_control.h" + +namespace effort_controllers +{ + +/** + * \brief Forward command controller for a set of effort controlled joints (linear or angular). + * + * This class forwards the commanded efforts down to a set of joints. + * + * \param joints Names of the joints to control. + * + * Subscribes to: + * - \b command (std_msgs::msg::Float64MultiArray) : The effort commands to apply. + */ +class JointGroupPositionController : public forward_command_controller::ForwardCommandController +{ +public: + EFFORT_CONTROLLERS_PUBLIC + JointGroupPositionController(); + + EFFORT_CONTROLLERS_PUBLIC + controller_interface::return_type + init(const std::string & controller_name) override; + + EFFORT_CONTROLLERS_PUBLIC + controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + EFFORT_CONTROLLERS_PUBLIC + CallbackReturn + on_configure(const rclcpp_lifecycle::State & previous_state) override; + + EFFORT_CONTROLLERS_PUBLIC + CallbackReturn + on_activate(const rclcpp_lifecycle::State & previous_state) override; + + EFFORT_CONTROLLERS_PUBLIC + CallbackReturn + on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + EFFORT_CONTROLLERS_PUBLIC + controller_interface::return_type + update() override; + +private: + std::vector pids_; + std::chrono::time_point t0; +}; + +} // namespace effort_controllers + +#endif // EFFORT_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index deeedb26ab..74b3e53c44 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -10,6 +10,8 @@ ament_cmake + angles + control_toolbox forward_command_controller rclcpp diff --git a/effort_controllers/src/joint_group_position_controller.cpp b/effort_controllers/src/joint_group_position_controller.cpp new file mode 100644 index 0000000000..dcebd1171f --- /dev/null +++ b/effort_controllers/src/joint_group_position_controller.cpp @@ -0,0 +1,160 @@ +// Copyright 2020 PAL Robotics S.L. +// +// 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 +#include + +#include "angles/angles.h" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "effort_controllers/joint_group_position_controller.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/parameter.hpp" + +namespace effort_controllers +{ +using CallbackReturn = JointGroupPositionController::CallbackReturn; + +JointGroupPositionController::JointGroupPositionController() +: forward_command_controller::ForwardCommandController() +{ + logger_name_ = "joint effort controller"; + interface_name_ = hardware_interface::HW_IF_EFFORT; +} + +controller_interface::return_type +JointGroupPositionController::init( + const std::string & controller_name) +{ + auto ret = ForwardCommandController::init(controller_name); + if (ret != controller_interface::return_type::OK) { + return ret; + } + + try { + // undeclare interface parameter used in the general forward_command_controller + get_node()->undeclare_parameter("interface_name"); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +controller_interface::InterfaceConfiguration +JointGroupPositionController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & joint : joint_names_) { + state_interfaces_config.names.push_back(joint + "/position"); + } + + return state_interfaces_config; +} + +CallbackReturn JointGroupPositionController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = ForwardCommandController::on_configure(previous_state); + + fprintf(stderr, "got %zu joints\n", joint_names_.size()); + pids_.resize(joint_names_.size()); + std::string gains_prefix = "gains"; + for (auto k = 0u; k < joint_names_.size(); ++k) { + auto p = get_node()->get_parameter(gains_prefix + "." + joint_names_[k] + ".p").as_double(); + auto i = get_node()->get_parameter(gains_prefix + "." + joint_names_[k] + ".i").as_double(); + auto d = get_node()->get_parameter(gains_prefix + "." + joint_names_[k] + ".d").as_double(); + pids_[k].initPid(p, i, d, 0.0, 0.0); + fprintf(stderr, "got gains for %s as (%f, %f, %f)\n", joint_names_[k].c_str(), p, i, d); + } + + return ret; +} + +CallbackReturn JointGroupPositionController::on_activate( + const rclcpp_lifecycle::State & /* previous_state */) +{ + if (command_interfaces_.size() != state_interfaces_.size()) { + fprintf(stderr, "state interfaces don't match with command interfaces\n"); + return CallbackReturn::ERROR; + } + t0 = std::chrono::system_clock::now(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn JointGroupPositionController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = ForwardCommandController::on_deactivate(previous_state); + + // stop all joints + for (auto & command_interface : command_interfaces_) { + command_interface.set_value(0.0); + } + + return ret; +} + +controller_interface::return_type JointGroupPositionController::update() +{ + auto period = std::chrono::duration_cast( + std::chrono::system_clock::now() - t0).count(); + t0 = std::chrono::system_clock::now(); + + auto joint_position_commands = *rt_command_ptr_.readFromRT(); + // no command received yet + if (!joint_position_commands) { + return controller_interface::return_type::OK; + } + + if (joint_position_commands->data.size() != command_interfaces_.size()) { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), + *node_->get_clock(), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + joint_position_commands->data.size(), command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + + for(auto i = 0u; i < joint_names_.size(); ++i) + { + double command_position = joint_position_commands->data[i]; + double current_position = state_interfaces_[i].get_value(); + auto error = angles::shortest_angular_distance(current_position, command_position); + + // Set the PID error and compute the PID command with nonuniform + // time step size. + auto commanded_effort = pids_[i].computeCommand(error, period); + command_interfaces_[i].set_value(commanded_effort); + + // TODO(karsten1987): + /* + * enforce joint limits + * calculate error terms depending on joint type + */ + } + + t0 = std::chrono::system_clock::now(); + return controller_interface::return_type::OK; +} + +} // namespace effort_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + effort_controllers::JointGroupPositionController, controller_interface::ControllerInterface)