diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 4bbe6eb673..2fb8bc3993 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -59,6 +59,7 @@ The controllers are using `common hardware interface definitions`_, and may use Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Motion Primitive Controller <../motion_primitives_controllers/userdoc.rst> + Gravity Compensation Controller <../gravity_compensation_controller/doc/userdoc.rst> Broadcasters ********************** diff --git a/gravity_compensation_pd_controller/CMakeLists.txt b/gravity_compensation_pd_controller/CMakeLists.txt new file mode 100644 index 0000000000..bbb9548498 --- /dev/null +++ b/gravity_compensation_pd_controller/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 3.16) +project(gravity_compensation_pd_controller) + +if(CMAKE_COMPILER_IS_GNUCXX + OR CMAKE_CXX_COMPILER_ID + MATCHES + "Clang" +) + add_compile_options( + -Wall + -Wextra + -Wpedantic + ) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + Eigen3 + generate_parameter_library + hardware_interface + inverse_dynamics_solver + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + urdf +) + +find_package(ament_cmake REQUIRED) + +foreach(dependency IN + ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + find_package(${dependency} REQUIRED) +endforeach() + +generate_parameter_library(gravity_compensation_pd_controller_parameters + src/gravity_compensation_pd_controller_parameters.yaml +) + +add_library(gravity_compensation_pd_controller SHARED + src/gravity_compensation_pd_controller.cpp +) + +target_compile_features( + gravity_compensation_pd_controller + PUBLIC cxx_std_17 +) + +target_include_directories( + gravity_compensation_pd_controller + PUBLIC $ + $ +) + +target_link_libraries( + gravity_compensation_pd_controller + PUBLIC gravity_compensation_pd_controller_parameters +) + +target_link_libraries( + gravity_compensation_pd_controller + PUBLIC controller_interface::controller_interface + hardware_interface::hardware_interface + hardware_interface::mock_components + inverse_dynamics_solver::inverse_dynamics_solver_lib + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + realtime_tools::thread_priority + urdf::urdf +) + +# Causes the visibility macros to use dllexport rather than dllimport, which is appropriate when +# building the dll but not consuming it. +target_compile_definitions( + gravity_compensation_pd_controller + PRIVATE "GRAVITY_COMPENSATION_PD_CONTROLLER_BUILDING_DLL" +) + +pluginlib_export_plugin_description_file( + controller_interface gravity_compensation_pd_controller.xml +) + +install(DIRECTORY include + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS gravity_compensation_pd_controller gravity_compensation_pd_controller_parameters + EXPORT export_gravity_compensation_pd_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_gravity_compensation_pd_controller + test/test_gravity_compensation_pd_controller.cpp + ) + target_link_libraries(test_gravity_compensation_pd_controller + gravity_compensation_pd_controller + ros2_control_test_assets::ros2_control_test_assets + ) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_gravity_compensation_pd_controller test/test_load_gravity_compensation_pd_controller.cpp) + target_link_libraries(test_load_gravity_compensation_pd_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) +endif() + +ament_export_targets(export_gravity_compensation_pd_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/gravity_compensation_pd_controller/doc/gravity_compensation_pd_controller_parameters.md b/gravity_compensation_pd_controller/doc/gravity_compensation_pd_controller_parameters.md new file mode 100644 index 0000000000..d4150c0123 --- /dev/null +++ b/gravity_compensation_pd_controller/doc/gravity_compensation_pd_controller_parameters.md @@ -0,0 +1,118 @@ +# Gravity Compensation Pd Controller Parameters + +Default Config +```yaml +gravity_compensation_pd_controller: + ros__parameters: + command_interfaces_names_override: + effort: '{}' + compensate_gravity: true + d_gains: '{}' + dynamics_solver: + dynamics_solver_plugin: '' + root: '' + tip: '' + joint_space_reference_controller: '' + joints: '{}' + p_gains: '{}' + state_interfaces_names_override: + position: '{}' + velocity: '{}' + +``` + +## joints + +Specifies which joints will be used by the controller. + +* Type: `string_array` +* Default Value: {} + +## joint_space_reference_controller + +Name of the joint space reference controller. + +* Type: `string` +* Default Value: "" +* Read only: True + +## state_interfaces_names_override.position + +If this parameter is set, it will override the default names for the position interface of the state interfaces. + +* Type: `string_array` +* Default Value: {} +* Read only: True + +## state_interfaces_names_override.velocity + +If this parameter is set, it will override the default names for the velocity interface of the state interfaces. + +* Type: `string_array` +* Default Value: {} +* Read only: True + +## command_interfaces_names_override.effort + +If this parameter is set, it will override the default names for the effort interface of the command interfaces. + +* Type: `string_array` +* Default Value: {} +* Read only: True + +## p_gains + +* Type: `double_array` +* Default Value: {} + +## d_gains + +* Type: `double_array` +* Default Value: {} + +## compensate_gravity + +If true, the controller will compensate for gravity. + +* Type: `bool` +* Default Value: true + +## dynamics_solver.dynamics_solver_plugin + +Name of the dynamics solver to use. + +* Type: `string` +* Read only: True + +*Constraints:* + - parameter is not empty + +*Additional Constraints:* + + + +## dynamics_solver.root + +Specifies the base link of the robot description used by the inverse dynamics solver. + +* Type: `string` +* Read only: True + +*Constraints:* + - parameter is not empty + +*Additional Constraints:* + + + +## dynamics_solver.tip + +Specifies the end effector link of the robot description used by the inverse dynamics solver. + +* Type: `string` +* Read only: True + +*Constraints:* + - parameter is not empty + +*Additional Constraints:* diff --git a/gravity_compensation_pd_controller/doc/media/gravity_compensation_pd_controller_scheme.png b/gravity_compensation_pd_controller/doc/media/gravity_compensation_pd_controller_scheme.png new file mode 100644 index 0000000000..c6c9a5f6b3 Binary files /dev/null and b/gravity_compensation_pd_controller/doc/media/gravity_compensation_pd_controller_scheme.png differ diff --git a/gravity_compensation_pd_controller/doc/userdoc.rst b/gravity_compensation_pd_controller/doc/userdoc.rst new file mode 100644 index 0000000000..7a7a4a77fb --- /dev/null +++ b/gravity_compensation_pd_controller/doc/userdoc.rst @@ -0,0 +1,62 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gravity_compensation_pd_controller/doc/userdoc.rst + +.. _gravity_compensation_pd_controller_userdoc: + +Gravity Compensation PD Controller +================================== + +This controller provides a PD controller with gravity compensation that converts joint position references to joint effort commands for a robotic manipulator. +It is a chainable controller; therefore, it requires another controller to provide joint position references via chaining. + +The controller uses the robot's dynamic model to compute the control action. +In particular, it relies on the `InverseDynamicsSolver `_ interface to estimate the gravity torque vector. + +Control Law +----------- + +The implemented control law follows the scheme shown in the figure, based on the formulation presented in *B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, "Robotics: Modelling, Planning and Control"*. + +.. image:: ../doc/media/gravity_compensation_pd_controller_scheme.png + :alt: Gravity Compensation PD Controller Scheme + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- ``/position`` [double] + +Commands +,,,,,,,, +- ``/effort`` [double] + +States +,,,,,, +- ``/position`` [double] +- ``/velocity`` [double] + + +Behavior in Edge Cases +---------------------- + +The controller is designed to handle the following edge cases: + +- **Computed torque exceeding joint limits**. If the computed torque exceeds the joint limits, the controller will saturate the output to remain within the range specified in the robot description. + +- **Invalid joint position reference**. If the joint position reference is ``NaN``, the controller will fall back to the last valid reference to compute the control action. Initially, the first valid reference is set to the robot's starting joint positions. + +- **Invalid joint position state**. If the controller detects a ``NaN`` value in the joint position state reported by the robot, it will return an error to the controller manager. In this case, the controller cannot compute a valid control action and will output ``NaN`` values. It is assumed that the hardware interface is capable of handling ``NaN`` values appropriately. + + +Parameters +---------- + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: ../src/gravity_compensation_pd_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/config/test_gravity_compensation_pd_controller.yaml + :language: yaml diff --git a/gravity_compensation_pd_controller/gravity_compensation_pd_controller.xml b/gravity_compensation_pd_controller/gravity_compensation_pd_controller.xml new file mode 100644 index 0000000000..bdbda33e8d --- /dev/null +++ b/gravity_compensation_pd_controller/gravity_compensation_pd_controller.xml @@ -0,0 +1,9 @@ + + + + A gravity compensation PD controller developed by the Automatic Control Group of the University of Salerno, Italy. + + + diff --git a/gravity_compensation_pd_controller/include/gravity_compensation_pd_controller/gravity_compensation_pd_controller.hpp b/gravity_compensation_pd_controller/include/gravity_compensation_pd_controller/gravity_compensation_pd_controller.hpp new file mode 100644 index 0000000000..a706ac3fdf --- /dev/null +++ b/gravity_compensation_pd_controller/include/gravity_compensation_pd_controller/gravity_compensation_pd_controller.hpp @@ -0,0 +1,191 @@ +// Copyright (c) 2025, University of Salerno, Automatic Control Group +// +// 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. +// +// Authors: Davide Risi + +#ifndef GRAVITY_COMPENSATION_PD_CONTROLLER__GRAVITY_COMPENSATION_PD_CONTROLLER_HPP_ +#define GRAVITY_COMPENSATION_PD_CONTROLLER__GRAVITY_COMPENSATION_PD_CONTROLLER_HPP_ + +#include "gravity_compensation_pd_controller/gravity_compensation_pd_controller_parameters.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gravity_compensation_pd_controller +{ + +struct RobotJointState +{ + std::vector positions; + std::vector velocities; +}; + +/** + * @class GravityCompensationPDController + * @brief A class representing a PD controller with gravity compensation. + * + * This class implements the \c controller_interface::ChainableControllerInterface interface. + * All of the public methods override the corresponding methods of the \c + * controller_interface::ChainableControllerInterface class. Please refer to the documentation of + * the base class for more details. + */ +class GravityCompensationPDController : public controller_interface::ChainableControllerInterface +{ +public: + GravityCompensationPDController(); + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + // The following methods are overridden from the base class. Refer to the base class documentation + // for details. + std::vector on_export_reference_interfaces() override; + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + bool on_set_chained_mode(bool chained_mode) override; + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + /** + * @brief Computes the joint effort command using a PD control law with optional gravity + * compensation. + * + * This method calculates the joint effort command according to the control law described in the + * README file. If \c compensate_gravity parameter is disabled, the control law reduces to a + * standard PD controller. The resulting effort command is stored in the \c joint_command_ + * attribute. + */ + void compute_control_law_(); + + /** + * @brief Reads the state interfaces and updates the internal joint state. + * + * @return True if successful, false otherwise. + */ + [[nodiscard]] bool read_state_interfaces_(); + + /** + * @brief Writes the command interfaces with the computed joint effort command. + * + * @return True if successful, false otherwise. + */ + [[nodiscard]] bool write_command_interfaces_(); + + /** + * @brief Reads the reference interfaces and updates the internal joint reference. + * + * @return True if successful, false otherwise. + */ + [[nodiscard]] bool read_reference_interfaces_(); + + /** + * @brief Reads the joint effort limits from the URDF file populating the torque_limits_ member. + * + * @return True if successful, false otherwise. + */ + bool read_joint_effort_limits_from_urdf(); + + /** + * @brief Plugin loader for the inverse dynamics solver. + */ + pluginlib::ClassLoader dynamics_solver_loader_; + + /** + * @brief Shared pointer to the inverse dynamics solver used for gravity compensation. + */ + std::shared_ptr dynamics_solver_; + + /** + * @brief Variables to store the joint command, reference, and last reference. + */ + std::vector joint_command_, joint_reference_, last_joint_reference_; + + /** + * @brief Shared pointer to the parameter listener responsible for handling the controller's + * parameters. + */ + std::shared_ptr parameter_handler_; + + /** + * @brief Vector to store the torque limits for each joint. + */ + Eigen::VectorXd torque_limits_; + + /** + * @brief Eigen vector to store the position error between the reference and current joint + * positions. + */ + Eigen::VectorXd position_error_; + + /** + * @brief Eigen vector to store the computed joint effort command. + */ + Eigen::VectorXd eigen_effort_command_; + + /** + * @brief Diagonal matrix for proportional gains (Kp) used in the PD control law. + */ + Eigen::DiagonalMatrix Kp_; + + /** + * @brief Diagonal matrix for derivative gains (Kd) used in the PD control law. + */ + Eigen::DiagonalMatrix Kd_; + + /** + * @brief Constant to store the duration of the throttle interval as an integral value in + * milliseconds. + */ + static constexpr int DURATION_MS_{1000}; + + /** + * @brief Number of joints to control. + */ + std::size_t num_joints_{0}; + + /** + * @brief Internal variable to store the current joint state of the robot. + */ + RobotJointState robot_joint_state_; +}; + +} // namespace gravity_compensation_pd_controller + +#endif // GRAVITY_COMPENSATION_PD_CONTROLLER__GRAVITY_COMPENSATION_PD_CONTROLLER_HPP_ diff --git a/gravity_compensation_pd_controller/package.xml b/gravity_compensation_pd_controller/package.xml new file mode 100644 index 0000000000..df25a5453c --- /dev/null +++ b/gravity_compensation_pd_controller/package.xml @@ -0,0 +1,32 @@ + + + + gravity_compensation_pd_controller + 0.1.0 + A ROS2 package implementing a PD controller with gravity compensation + Davide Risi + Enrico Ferrentino + Apache License 2.0 + + ament_cmake + + controller_interface + eigen + generate_parameter_library + hardware_interface + inverse_dynamics_solver + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + kdl_inverse_dynamics_solver + + + ament_cmake + + diff --git a/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller.cpp b/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller.cpp new file mode 100644 index 0000000000..3cd78ebcad --- /dev/null +++ b/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller.cpp @@ -0,0 +1,418 @@ +// Copyright (c) 2025, University of Salerno, Automatic Control Group +// +// 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. +// +// Authors: Davide Risi + +#include "gravity_compensation_pd_controller/gravity_compensation_pd_controller.hpp" + +#include + +// URDF +#include + +namespace gravity_compensation_pd_controller +{ +GravityCompensationPDController::GravityCompensationPDController() +: controller_interface::ChainableControllerInterface(), + dynamics_solver_loader_( + "inverse_dynamics_solver", "inverse_dynamics_solver::InverseDynamicsSolver"), + dynamics_solver_(nullptr) +{ +} + +controller_interface::CallbackReturn GravityCompensationPDController::on_init() +{ + try + { + parameter_handler_ = + std::make_shared(get_node()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Number of joints to control is fixed after initialization + num_joints_ = parameter_handler_->get_params().joints.size(); + + // Retrieve torque limits from URDF + if (!read_joint_effort_limits_from_urdf()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint effort limits from URDF"); + return controller_interface::CallbackReturn::ERROR; + } + + // Allocate dynamic memory + robot_joint_state_.positions.assign(num_joints_, 0.0); + robot_joint_state_.velocities.assign(num_joints_, 0.0); + joint_command_.assign(num_joints_, 0.0); + joint_reference_.assign(num_joints_, 0.0); + last_joint_reference_ = joint_reference_; + + // Allocate Eigen vectors' dynamic memory for real-time safeness + position_error_.resize(num_joints_); + eigen_effort_command_.resize(num_joints_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GravityCompensationPDController::command_interface_configuration() const +{ + const auto params = parameter_handler_->get_params(); + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params.joints.size()); + + if (params.command_interfaces_names_override.effort.size() == num_joints_) + { + command_interfaces_config.names.insert( + command_interfaces_config.names.end(), + params.command_interfaces_names_override.effort.begin(), + params.command_interfaces_names_override.effort.end()); + } + else + { + for (const auto & joint_name : params.joints) + { + command_interfaces_config.names.push_back(joint_name + "/effort"); + } + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +GravityCompensationPDController::state_interface_configuration() const +{ + const auto params = parameter_handler_->get_params(); + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(params.joints.size() * 2); + + if (params.state_interfaces_names_override.position.size() == num_joints_) + { + state_interfaces_config.names.insert( + state_interfaces_config.names.end(), params.state_interfaces_names_override.position.begin(), + params.state_interfaces_names_override.position.end()); + } + else + { + for (const auto & joint_name : params.joints) + { + state_interfaces_config.names.push_back(joint_name + "/position"); + } + } + + if (params.state_interfaces_names_override.velocity.size() == num_joints_) + { + state_interfaces_config.names.insert( + state_interfaces_config.names.end(), params.state_interfaces_names_override.velocity.begin(), + params.state_interfaces_names_override.velocity.end()); + } + else + { + for (const auto & joint_name : params.joints) + { + state_interfaces_config.names.push_back(joint_name + "/velocity"); + } + } + return state_interfaces_config; +} + +std::vector +GravityCompensationPDController::on_export_reference_interfaces() +{ + const auto params = parameter_handler_->get_params(); + ChainableControllerInterface::reference_interfaces_.assign( + num_joints_, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(num_joints_); + + for (std::size_t i = 0; i < params.joints.size(); ++i) + { + reference_interfaces.emplace_back( + hardware_interface::CommandInterface( + std::string(get_node()->get_name()) + "/" + params.joints[i], "position", + &ChainableControllerInterface::reference_interfaces_[i])); + } + return reference_interfaces; +} + +controller_interface::CallbackReturn GravityCompensationPDController::on_error( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set the command to NaN values, to notify the hardware that the controller is unable to provide + // valid commands + std::fill(joint_command_.begin(), joint_command_.end(), std::numeric_limits::quiet_NaN()); + if (!write_command_interfaces_()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to write NaN values to command interfaces while in error state."); + return controller_interface::CallbackReturn::ERROR; + } + + RCLCPP_ERROR( + get_node()->get_logger(), + "Controller is in error state. Writing NaN values to command interfaces. Restart the " + "controller to recover."); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GravityCompensationPDController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + gravity_compensation_pd_controller::Params params = parameter_handler_->get_params(); + + if (dynamics_solver_ == nullptr) + { + try + { + dynamics_solver_ = + dynamics_solver_loader_.createSharedInstance(params.dynamics_solver.dynamics_solver_plugin); + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception while loading the dynamics solver plugin '%s': '%s'", + params.dynamics_solver.dynamics_solver_plugin.c_str(), ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Set the robot description as node parameters + get_node()->declare_parameter("robot_description", get_robot_description()); + + // Initialize the dynamics solver + try + { + dynamics_solver_->initialize(get_node()->get_node_parameters_interface(), "dynamics_solver"); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Exception while initializing the dynamics solver plugin '%s': '%s'", + params.dynamics_solver.dynamics_solver_plugin.c_str(), e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + if (params.p_gains.size() != num_joints_ || params.d_gains.size() != num_joints_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Gains size does not match joints size"); + return controller_interface::CallbackReturn::ERROR; + } + + Kp_.diagonal() = Eigen::Map(params.p_gains.data(), params.p_gains.size()); + Kd_.diagonal() = Eigen::Map(params.d_gains.data(), params.d_gains.size()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GravityCompensationPDController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Read the current state of the robot from the hardware + if (!read_state_interfaces_()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); + return controller_interface::CallbackReturn::ERROR; + } + + // Use current joint state as first valid reference + joint_reference_ = robot_joint_state_.positions; + last_joint_reference_ = joint_reference_; + std::fill(joint_command_.begin(), joint_command_.end(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +GravityCompensationPDController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +controller_interface::return_type GravityCompensationPDController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // Read the current state of the robot from the hardware + if (!read_state_interfaces_()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint states from the hardware.\n"); + return controller_interface::return_type::ERROR; + } + + // Read the reference from the reference interfaces + if (!read_reference_interfaces_()) + { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), DURATION_MS_, + "Received NaN values in joint reference positions. Using last valid reference instead."); + // If NaN values are detected in the joint reference positions, use the last valid reference + joint_reference_ = last_joint_reference_; + } + + compute_control_law_(); + + // Write the command to the hardware + if (!write_command_interfaces_()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to write joint command to the hardware.\n"); + return controller_interface::return_type::ERROR; + } + + // Update the last command and reference + last_joint_reference_ = joint_reference_; + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn GravityCompensationPDController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool GravityCompensationPDController::on_set_chained_mode(bool /* chained_mode */) { return true; } + +void GravityCompensationPDController::compute_control_law_() +{ + Eigen::VectorXd current_positions = Eigen::Map( + robot_joint_state_.positions.data(), robot_joint_state_.positions.size()); + Eigen::VectorXd current_velocities = Eigen::Map( + robot_joint_state_.velocities.data(), robot_joint_state_.velocities.size()); + + for (std::size_t i = 0; i < num_joints_; i++) + { + position_error_(i) = joint_reference_[i] - current_positions(i); + } + + // Compute the effort command using PD control law + if (parameter_handler_->get_params().compensate_gravity) + { + eigen_effort_command_ = dynamics_solver_->getGravityVector(current_positions); + } + + eigen_effort_command_ += Kp_ * position_error_ - Kd_ * current_velocities; + + // Apply torque limits using Eigen + eigen_effort_command_ = eigen_effort_command_.cwiseMax(-torque_limits_).cwiseMin(torque_limits_); + std::copy( + eigen_effort_command_.data(), eigen_effort_command_.data() + num_joints_, + joint_command_.begin()); +} + +bool GravityCompensationPDController::read_state_interfaces_() +{ + bool all_read{true}; + for (std::size_t i = 0; i < num_joints_; i++) + { + std::optional state_position_op = + ControllerInterfaceBase::state_interfaces_[i].get_optional(); + std::optional state_velocity_op = + ControllerInterfaceBase::state_interfaces_[i + num_joints_].get_optional(); + + if (!state_position_op.has_value() || !state_velocity_op.has_value()) + { + all_read = false; + } + robot_joint_state_.positions[i] = state_position_op.value(); + robot_joint_state_.velocities[i] = state_velocity_op.value(); + } + return all_read; +} + +bool GravityCompensationPDController::write_command_interfaces_() +{ + bool all_written{true}; + for (std::size_t i = 0; i < num_joints_; i++) + { + all_written &= ControllerInterfaceBase::command_interfaces_[i].set_value(joint_command_[i]); + } + return all_written; +} + +bool GravityCompensationPDController::read_reference_interfaces_() +{ + bool all_read{true}; + for (std::size_t i = 0; i < num_joints_; i++) + { + if (std::isnan(ChainableControllerInterface::reference_interfaces_[i])) + { + all_read = false; + } + joint_reference_[i] = ChainableControllerInterface::reference_interfaces_[i]; + } + return all_read; +} + +bool GravityCompensationPDController::read_joint_effort_limits_from_urdf() +{ + torque_limits_.resize(num_joints_); + const std::string & robot_description = get_robot_description(); + const std::vector & joint_names = parameter_handler_->get_params().joints; + + if (robot_description.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Robot description is empty."); + return false; + } + + urdf::Model robot_model; + if (!robot_model.initString(robot_description)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF."); + return false; + } + + std::size_t num_joints{joint_names.size()}; + std::vector joints_without_limits; + + for (std::size_t i = 0; i < num_joints; ++i) + { + const std::string & joint_name{joint_names[i]}; + urdf::JointConstSharedPtr joint{robot_model.getJoint(joint_name)}; + + if (!joint) return false; + + if (joint->limits) + { + torque_limits_[i] = joint->limits->effort; + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Joint '%s' does not have effort limits specified in the URDF. Setting to infinity.", + joint_name.c_str()); + torque_limits_[i] = std::numeric_limits::max(); + } + } + + return true; +} + +} // namespace gravity_compensation_pd_controller + +PLUGINLIB_EXPORT_CLASS( + gravity_compensation_pd_controller::GravityCompensationPDController, + controller_interface::ChainableControllerInterface) diff --git a/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller_parameters.yaml b/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller_parameters.yaml new file mode 100644 index 0000000000..832c2814f7 --- /dev/null +++ b/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller_parameters.yaml @@ -0,0 +1,74 @@ +gravity_compensation_pd_controller: + joints: { + type: string_array, + description: "Specifies which joints will be used by the controller.", + default_value: [], + } + joint_space_reference_controller: { + type: string, + description: "Name of the joint space reference controller.", + default_value: "", + read_only: true, + } + state_interfaces_names_override: + position: { + type: string_array, + description: "If this parameter is set, it will override the default names for the position interface of the state interfaces.", + default_value: [], + read_only: true + } + velocity: { + type: string_array, + description: "If this parameter is set, it will override the default names for the velocity interface of the state interfaces.", + default_value: [], + read_only: true + } + command_interfaces_names_override: + effort: { + type: string_array, + description: "If this parameter is set, it will override the default names for the effort interface of the command interfaces.", + default_value: [], + read_only: true + } + + p_gains: { + type: double_array, + default_value: [], + } + d_gains: { + type: double_array, + default_value: [], + } + compensate_gravity: { + type: bool, + description: "If true, the controller will compensate for gravity.", + default_value: true, + } + dynamics_solver: + dynamics_solver_plugin: + { + type: string, + description: "Name of the dynamics solver to use.", + read_only: true, + validation: { + not_empty<>: null + } + } + root: + { + type: string, + description: "Specifies the base link of the robot description used by the inverse dynamics solver.", + read_only: true, + validation: { + not_empty<>: null + } + } + tip: + { + type: string, + description: "Specifies the end effector link of the robot description used by the inverse dynamics solver.", + read_only: true, + validation: { + not_empty<>: null + } + } diff --git a/gravity_compensation_pd_controller/test/config/test_gravity_compensation_pd_controller.yaml b/gravity_compensation_pd_controller/test/config/test_gravity_compensation_pd_controller.yaml new file mode 100644 index 0000000000..7ab999667e --- /dev/null +++ b/gravity_compensation_pd_controller/test/config/test_gravity_compensation_pd_controller.yaml @@ -0,0 +1,16 @@ +test_gravity_compensation_pd_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + + dynamics_solver: + dynamics_solver_plugin: kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL + tip: link3 + root: base_link + + p_gains: [200.0, 300.0, 400.0] + d_gains: [10.0, 10.0, 10.0] + + compensate_gravity: true diff --git a/gravity_compensation_pd_controller/test/test_asset_robot_description.hpp b/gravity_compensation_pd_controller/test/test_asset_robot_description.hpp new file mode 100644 index 0000000000..238f7bf5d6 --- /dev/null +++ b/gravity_compensation_pd_controller/test/test_asset_robot_description.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2025, University of Salerno, Automatic Control Group +// +// 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. +// +// Authors: Davide Risi + +#ifndef TEST_ASSET_ROBOT_DESCRIPTION_HPP_ +#define TEST_ASSET_ROBOT_DESCRIPTION_HPP_ + +#include +#include "ros2_control_test_assets/descriptions.hpp" + +namespace ros2_control_test_assets +{ + +const auto system_hardware_resources = + R"( + + + mock_components/GenericSystem + false + 0.0 + + + + + 0.0 + + + 0.0 + + + + + + 0.0 + + + 0.0 + + + + + + 0.0 + + + 0.0 + + + +)"; + +const auto valid_robot_urdf = + std::string(urdf_head) + std::string(system_hardware_resources) + std::string(urdf_tail); + +} // namespace ros2_control_test_assets + +#endif // TEST_ASSET_ROBOT_DESCRIPTION_HPP_ diff --git a/gravity_compensation_pd_controller/test/test_gravity_compensation_pd_controller.cpp b/gravity_compensation_pd_controller/test/test_gravity_compensation_pd_controller.cpp new file mode 100644 index 0000000000..431a07d3bf --- /dev/null +++ b/gravity_compensation_pd_controller/test/test_gravity_compensation_pd_controller.cpp @@ -0,0 +1,304 @@ +// Copyright (c) 2025, University of Salerno, Automatic Control Group +// +// 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. +// +// Authors: Davide Risi + +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/utilities.hpp" +#include "test_gravity_compensation_pd_controller.hpp" + +#include "test_asset_robot_description.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; + +// ****** Definitions of the test cases for GravityCompensationPDController public API ****** + +void GravityCompensationPDControllerTest::SetUp() +{ + controller_ = std::make_unique(); + std::vector default_parameters = { + rclcpp::Parameter("joints", joint_names_), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter( + "dynamics_solver.dynamics_solver_plugin", + std::string("kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL")), + rclcpp::Parameter("dynamics_solver.tip", std::string("link3")), + rclcpp::Parameter("dynamics_solver.root", std::string("base_link"))}; + SetUpController(default_parameters); +} + +void GravityCompensationPDControllerTest::TearDown() +{ + controller_.reset(nullptr); + command_interfaces_.clear(); + state_interfaces_.clear(); +} + +void GravityCompensationPDControllerTestBase::assign_interfaces_() +{ + std::vector command_ifs; + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) + { + auto cmd_interface = std::make_unique( + joint_names_[i], "effort", &joint_command_values_[i]); + command_ifs.emplace_back(*cmd_interface); + command_interfaces_.push_back(std::move(cmd_interface)); + } + + std::vector state_ifs; + + // Create position state interfaces + for (auto i = 0u; i < joint_position_values_.size(); ++i) + { + auto state_interface = std::make_unique( + joint_names_[i], "position", &joint_position_values_[i]); + state_ifs.emplace_back(*state_interface); + state_interfaces_.push_back(std::move(state_interface)); + } + + // Create velocity state interfaces + for (auto i = 0u; i < joint_velocity_values_.size(); ++i) + { + auto state_interface = std::make_unique( + joint_names_[i], "velocity", &joint_velocity_values_[i]); + state_ifs.emplace_back(*state_interface); + state_interfaces_.push_back(std::move(state_interface)); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +void GravityCompensationPDControllerTestBase::SetUpController( + const std::vector & parameters) +{ + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides(parameters); + + const auto result = controller_->init( + "test_gravity_compensation_pd_controller", ros2_control_test_assets::valid_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + + assign_interfaces_(); + executor_.add_node(controller_->get_node()->get_node_base_interface()); +} + +TEST_F(GravityCompensationPDControllerTest, ConfigureAndActivateParamsSuccess) +{ + ASSERT_EQ(controller_->on_init(), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(GravityCompensationPDControllerTest, NoCommandCheckTest) +{ + ASSERT_EQ(controller_->on_init(), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // This method is protected, so we need the friend class + controller_->on_export_reference_interfaces(); + + // Update successful, no command received yet + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Check joint commands with gravity compensation only and no PD action + EXPECT_NEAR(joint_command_values_[0], 0.1, 1e-9); + EXPECT_NEAR(joint_command_values_[1], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[2], 0.0, 1e-9); +} + +TEST_F(GravityCompensationPDControllerTest, StopJointsOnDeactivateTest) +{ + ASSERT_EQ(controller_->on_init(), CallbackReturn::SUCCESS); + + // configure successful + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check joint commands are still the default ones + EXPECT_NEAR(joint_command_values_[0], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[1], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[2], 0.0, 1e-9); + + // stop the controller + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check joint commands are now zero + EXPECT_NEAR(joint_command_values_[0], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[1], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[2], 0.0, 1e-9); +} + +TEST_F(GravityCompensationPDControllerTest, CommandNanOnErrorTest) +{ + ASSERT_EQ(controller_->on_init(), CallbackReturn::SUCCESS); + + // configure successful + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check joint commands are still the default ones + EXPECT_NEAR(joint_command_values_[0], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[1], 0.0, 1e-9); + EXPECT_NEAR(joint_command_values_[2], 0.0, 1e-9); + + // stop the controller + ASSERT_EQ(controller_->on_error(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // check joint commands are now NaN + EXPECT_TRUE(std::isnan(joint_command_values_[0])); + EXPECT_TRUE(std::isnan(joint_command_values_[1])); + EXPECT_TRUE(std::isnan(joint_command_values_[2])); +} + +// ****** Definitions of the test cases for invalid parameters ****** + +void GravityCompensationPDControllerInvalidParameterTest::SetUp() +{ + controller_ = std::make_unique(); + // Parameters are set in each test case + const std::vector parameters = WithParamInterface::GetParam(); + SetUpController(parameters); +} + +void GravityCompensationPDControllerInvalidParameterTest::TearDown() +{ + controller_.reset(nullptr); + command_interfaces_.clear(); + state_interfaces_.clear(); +} + +TEST_P(GravityCompensationPDControllerInvalidParameterTest, ConfigureFail) +{ + // configure should fail + ASSERT_EQ(controller_->on_init(), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +INSTANTIATE_TEST_SUITE_P( + InvalidConfigParameters, GravityCompensationPDControllerInvalidParameterTest, + testing::Values( + // Empty joint names + std::vector{ + rclcpp::Parameter("joints", std::vector{}), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter( + "dynamics_solver.dynamics_solver_plugin", + std::string("kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL")), + rclcpp::Parameter("dynamics_solver.tip", std::string("link3")), + rclcpp::Parameter("dynamics_solver.root", std::string("base_link"))}, + + // Mismatched p_gains size + std::vector{ + rclcpp::Parameter("joints", std::vector{"joint1", "joint2", "joint3"}), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter( + "dynamics_solver.dynamics_solver_plugin", + std::string("kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL")), + rclcpp::Parameter("dynamics_solver.tip", std::string("link3")), + rclcpp::Parameter("dynamics_solver.root", std::string("base_link"))}, + + // Mismatched d_gains size + std::vector{ + rclcpp::Parameter("joints", std::vector{"joint1", "joint2", "joint3"}), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter( + "dynamics_solver.dynamics_solver_plugin", + std::string("kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL")), + rclcpp::Parameter("dynamics_solver.tip", std::string("link3")), + rclcpp::Parameter("dynamics_solver.root", std::string("base_link"))})); + +// ****** Definitions of the test cases for missing parameters ****** + +void GravityCompensationPDControllerMissingParameterTest::SetUp() +{ + controller_ = std::make_unique(); + // Parameters are set in each test case + const std::vector parameters = WithParamInterface::GetParam(); + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides(parameters); + + controller_->init( + "test_gravity_compensation_pd_controller", ros2_control_test_assets::valid_robot_urdf, 0, "", + node_options); + + executor_.add_node(controller_->get_node()->get_node_base_interface()); +} + +TEST_P(GravityCompensationPDControllerMissingParameterTest, InitFail) +{ + // init should fail + ASSERT_EQ(controller_->on_init(), CallbackReturn::ERROR); +} + +INSTANTIATE_TEST_SUITE_P( + InvalidInitParameters, GravityCompensationPDControllerMissingParameterTest, + testing::Values( + // Missing dynamics solver plugin + std::vector{ + rclcpp::Parameter("joints", std::vector{"joint1", "joint2", "joint3"}), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter("dynamics_solver.tip", std::string("link3")), + rclcpp::Parameter("dynamics_solver.root", std::string("base_link"))}, + // Missing dynamics solver tip + std::vector{ + rclcpp::Parameter("joints", std::vector{"joint1", "joint2", "joint3"}), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter( + "dynamics_solver.dynamics_solver_plugin", + std::string("kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL")), + rclcpp::Parameter("dynamics_solver.root", std::string("base_link"))}, + // Missing dynamics solver root + std::vector{ + rclcpp::Parameter("joints", std::vector{"joint1", "joint2", "joint3"}), + rclcpp::Parameter("p_gains", std::vector{100.0, 100.0, 100.0}), + rclcpp::Parameter("d_gains", std::vector{5.0, 5.0, 5.0}), + rclcpp::Parameter("compensate_gravity", true), + rclcpp::Parameter( + "dynamics_solver.dynamics_solver_plugin", + std::string("kdl_inverse_dynamics_solver/InverseDynamicsSolverKDL")), + rclcpp::Parameter("dynamics_solver.tip", std::string("link3"))})); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return result; +} diff --git a/gravity_compensation_pd_controller/test/test_gravity_compensation_pd_controller.hpp b/gravity_compensation_pd_controller/test/test_gravity_compensation_pd_controller.hpp new file mode 100644 index 0000000000..fdd909d404 --- /dev/null +++ b/gravity_compensation_pd_controller/test/test_gravity_compensation_pd_controller.hpp @@ -0,0 +1,84 @@ +// Copyright (c) 2025, University of Salerno, Automatic Control Group +// +// 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. +// +// Authors: Davide Risi + +#ifndef TEST_GRAVITY_COMPENSATION_PD_CONTROLLER_HPP_ +#define TEST_GRAVITY_COMPENSATION_PD_CONTROLLER_HPP_ + +#include + +#include +#include +#include + +#include "gravity_compensation_pd_controller/gravity_compensation_pd_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +// Create a friend class to access private and protected members for testing +class FriendGravityCompensationPDController +: public gravity_compensation_pd_controller::GravityCompensationPDController +{ + FRIEND_TEST(GravityCompensationPDControllerTest, NoCommandCheckTest); +}; + +class GravityCompensationPDControllerTestBase +{ +public: + void SetUpController(const std::vector & parameters = {}); + +protected: + void assign_interfaces_(); + +protected: + std::unique_ptr controller_; + const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + std::vector joint_command_values_ = {0.0, 0.0, 0.0}; + std::vector joint_position_values_ = {0.0, 0.0, 0.0}; + std::vector joint_velocity_values_ = {0.0, 0.0, 0.0}; + + // Store the interfaces to keep them alive + std::vector> command_interfaces_; + std::vector> state_interfaces_; + + rclcpp::executors::SingleThreadedExecutor executor_; +}; + +class GravityCompensationPDControllerTest : public GravityCompensationPDControllerTestBase, + public testing::Test +{ +public: + void SetUp() override; + void TearDown() override; +}; + +class GravityCompensationPDControllerInvalidParameterTest +: public testing::TestWithParam>, + public GravityCompensationPDControllerTestBase +{ +public: + void SetUp() override; + void TearDown() override; +}; + +class GravityCompensationPDControllerMissingParameterTest +: public GravityCompensationPDControllerInvalidParameterTest +{ +public: + void SetUp() override; +}; + +#endif // TEST_GRAVITY_COMPENSATION_PD_CONTROLLER_HPP_ diff --git a/gravity_compensation_pd_controller/test/test_load_gravity_compensation_pd_controller.cpp b/gravity_compensation_pd_controller/test/test_load_gravity_compensation_pd_controller.cpp new file mode 100644 index 0000000000..ee372b8da3 --- /dev/null +++ b/gravity_compensation_pd_controller/test/test_load_gravity_compensation_pd_controller.cpp @@ -0,0 +1,53 @@ +// Copyright (c) 2025, University of Salerno, Automatic Control Group +// +// 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. +// +// Authors: Davide Risi + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" + +#include "test_asset_robot_description.hpp" + +TEST(TestLoadGravityCompensationPDController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::valid_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_gravity_compensation_pd_controller.yaml"; + + cm.set_parameter({"test_gravity_compensation_pd_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_gravity_compensation_pd_controller.type", + "gravity_compensation_pd_controller/GravityCompensationPDController"}); + + ASSERT_NE(cm.load_controller("test_gravity_compensation_pd_controller"), nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}