From 0206ee68b6f5e46bbc9f7a31f776dd8e0e483322 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 21 Mar 2022 16:24:41 +0100 Subject: [PATCH 001/111] Make forward controller chainable. Enabling chanable mode for forward command controllers. Fix all bugs in chained-controllers mode. Remove debug output from forwarding controller. Updated example ffwd chainable controller for new structure. --- forward_command_controller/CMakeLists.txt | 27 ++++ .../forward_command_plugin.xml | 54 ++++++- .../chainable_forward_controller.hpp | 85 +++++++++++ .../forward_command_controller.hpp | 70 ++++++++- .../forward_controller.hpp | 67 +++++++++ .../forward_controllers_base.hpp | 36 ++--- ...i_interface_forward_command_controller.hpp | 72 ++++++++- .../src/chainable_forward_controller.cpp | 140 ++++++++++++++++++ .../src/forward_command_controller.cpp | 64 ++------ .../src/forward_controller.cpp | 74 +++++++++ .../src/forward_controllers_base.cpp | 78 ++++------ ...i_interface_forward_command_controller.cpp | 56 ++----- .../test/test_forward_command_controller.cpp | 1 + .../test/test_forward_command_controller.hpp | 1 + ...oad_chained_forward_command_controller.cpp | 42 ++++++ ...i_interface_forward_command_controller.cpp | 41 +++++ .../test_load_forward_command_controller.cpp | 3 + ...i_interface_forward_command_controller.hpp | 1 + .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 30 +++- 20 files changed, 769 insertions(+), 174 deletions(-) create mode 100644 forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp create mode 100644 forward_command_controller/include/forward_command_controller/forward_controller.hpp create mode 100644 forward_command_controller/src/chainable_forward_controller.cpp create mode 100644 forward_command_controller/src/forward_controller.cpp create mode 100644 forward_command_controller/test/test_load_chained_forward_command_controller.cpp create mode 100644 forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 2490b5c856..30fe885b5d 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(std_msgs REQUIRED) add_library(forward_command_controller SHARED + src/chainable_forward_controller.cpp + src/forward_controller.cpp src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp @@ -101,6 +103,31 @@ if(BUILD_TESTING) target_link_libraries(test_multi_interface_forward_command_controller forward_command_controller ) + + ament_add_gmock( + test_load_chained_forward_command_controller + test/test_load_chained_forward_command_controller.cpp + ) + target_include_directories(test_load_chained_forward_command_controller PRIVATE include) + ament_target_dependencies( + test_load_chained_forward_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_load_chained_multi_interface_forward_command_controller + test/test_load_chained_multi_interface_forward_command_controller.cpp + ) + target_include_directories(test_load_chained_multi_interface_forward_command_controller PRIVATE include) + ament_target_dependencies( + test_load_chained_multi_interface_forward_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + endif() ament_export_dependencies( diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 48fa29936d..58026d62b5 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,11 +1,61 @@ + + - + The forward command controller commands a group of joints in a given interface + type="forward_command_controller::MultiInterfaceForwardCommandController" + base_class_type="controller_interface::ControllerInterface"> + + MultiInterfaceForwardController ros2_control controller. + + + + + + The forward command controller commands a group of joints in a given interface + + + MultiInterfaceForwardController ros2_control controller. diff --git a/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp b/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp new file mode 100644 index 0000000000..e62b39237b --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp @@ -0,0 +1,85 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ +#define FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "forward_command_controller/forward_controllers_base.hpp" +#include "forward_command_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace forward_command_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; + +/** + * \brief Forward command controller for a set of joints and interfaces. + * + * This class forwards the command signal down to a set of joints or interfaces. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. + */ +class ChainableForwardController : public ForwardControllersBase, + public controller_interface::ChainableControllerInterface +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableForwardController(); + + FORWARD_COMMAND_CONTROLLER_PUBLIC + ~ChainableForwardController() = default; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + controller_interface::return_type update_reference_from_subscribers() override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + std::vector reference_interface_names_; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index f50af328d8..c8f8dcca23 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -18,8 +18,11 @@ #include #include +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace forward_command_controller { @@ -34,20 +37,79 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class ForwardCommandController : public ForwardControllersBase +template < + typename T, + typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr, + typename std::enable_if< + std::is_convertible::value, T>::type * = + nullptr> +class BaseForwardCommandController : public T { public: FORWARD_COMMAND_CONTROLLER_PUBLIC - ForwardCommandController(); + BaseForwardCommandController() : T() {} protected: - void declare_parameters() override; - controller_interface::CallbackReturn read_parameters() override; + void declare_parameters() override + { + controller_interface::ControllerInterface::auto_declare>( + "joints", std::vector()); + controller_interface::ControllerInterface::auto_declare("interface_name", ""); + }; + + controller_interface::CallbackReturn read_parameters() override + { + joint_names_ = T::get_node()->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + // Specialized, child controllers set interfaces before calling configure function. + if (interface_name_.empty()) + { + interface_name_ = T::get_node()->get_parameter("interface_name").as_string(); + } + + if (interface_name_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : joint_names_) + { + T::command_interface_names_.push_back(joint + "/" + interface_name_); + } + + return controller_interface::CallbackReturn::SUCCESS; + }; std::vector joint_names_; std::string interface_name_; }; +class ForwardCommandController : public BaseForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardCommandController() : BaseForwardCommandController() {} +}; + +class ChainableForwardCommandController +: public BaseForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableForwardCommandController() : BaseForwardCommandController() + { + } +}; + } // namespace forward_command_controller #endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_controller.hpp new file mode 100644 index 0000000000..5bf2d1f831 --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/forward_controller.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ +#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ + +#include "controller_interface/controller_interface.hpp" +#include "forward_command_controller/forward_controllers_base.hpp" +#include "forward_command_controller/visibility_control.h" + +namespace forward_command_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; + +/** + * \brief Forward command controller for a set of joints and interfaces. + */ +class ForwardController : public ForwardControllersBase, + public controller_interface::ControllerInterface +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardController(); + + FORWARD_COMMAND_CONTROLLER_PUBLIC + ~ForwardController() = default; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 3e153d7e2e..ad6e1a5d23 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -32,14 +32,14 @@ namespace forward_command_controller using CmdType = std_msgs::msg::Float64MultiArray; /** - * \brief Forward command controller for a set of joints and interfaces. + * \brief Forward command controller base class for shared implementation. * * This class forwards the command signal down to a set of joints or interfaces. * * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class ForwardControllersBase : public controller_interface::ControllerInterface +class ForwardControllersBase { public: FORWARD_COMMAND_CONTROLLER_PUBLIC @@ -49,29 +49,28 @@ class ForwardControllersBase : public controller_interface::ControllerInterface ~ForwardControllersBase() = default; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration get_command_interface_configuration() const; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::InterfaceConfiguration get_state_interface_configuration() const; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn execute_init( + const std::shared_ptr & node); FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn execute_configure( + const rclcpp_lifecycle::State & previous_state, + std::vector & command_interfaces); FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn execute_activate( + const rclcpp_lifecycle::State & previous_state, + std::vector & command_interfaces); FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::CallbackReturn execute_deactivate( + const rclcpp_lifecycle::State & previous_state); protected: /** @@ -81,7 +80,7 @@ class ForwardControllersBase : public controller_interface::ControllerInterface virtual void declare_parameters() = 0; /** - * Derived controllers have to read parameters in this method and set `command_interface_types_` + * Derived controllers have to read parameters in this method and set `command_interface_names_` * variable. The variable is then used to propagate the command interface configuration to * controller manager. The method is called from `on_configure`-method of this class. * @@ -95,10 +94,13 @@ class ForwardControllersBase : public controller_interface::ControllerInterface std::vector joint_names_; std::string interface_name_; - std::vector command_interface_types_; + std::vector command_interface_names_; realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; + +private: + std::shared_ptr node_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 8acec36c5c..30cabfa6d6 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -18,8 +18,11 @@ #include #include +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace forward_command_controller { @@ -34,21 +37,80 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class MultiInterfaceForwardCommandController -: public forward_command_controller::ForwardControllersBase +template < + typename T, + typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr, + typename std::enable_if< + std::is_convertible::value, T>::type * = + nullptr> +class BaseMultiInterfaceForwardCommandController : public T { public: FORWARD_COMMAND_CONTROLLER_PUBLIC - MultiInterfaceForwardCommandController(); + BaseMultiInterfaceForwardCommandController() : T() {} protected: - void declare_parameters() override; - controller_interface::CallbackReturn read_parameters() override; + void declare_parameters() override + { + controller_interface::ControllerInterface::auto_declare("joint", joint_name_); + controller_interface::ControllerInterface::auto_declare>( + "interface_names", interface_names_); + }; + + controller_interface::CallbackReturn read_parameters() override + { + joint_name_ = T::get_node()->get_parameter("joint").as_string(); + interface_names_ = T::get_node()->get_parameter("interface_names").as_string_array(); + + if (joint_name_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'joint' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (interface_names_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & interface : interface_names_) + + { + T::command_interface_names_.push_back(joint_name_ + "/" + interface); + } + + return controller_interface::CallbackReturn::SUCCESS; + }; std::string joint_name_; std::vector interface_names_; }; +class MultiInterfaceForwardCommandController +: public BaseMultiInterfaceForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + MultiInterfaceForwardCommandController() + : BaseMultiInterfaceForwardCommandController() + { + } +}; + +class ChainableMultiInterfaceForwardCommandController +: public BaseMultiInterfaceForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableMultiInterfaceForwardCommandController() + : BaseMultiInterfaceForwardCommandController() + { + } +}; + } // namespace forward_command_controller #endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp new file mode 100644 index 0000000000..796040516c --- /dev/null +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -0,0 +1,140 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 "forward_command_controller/chainable_forward_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace forward_command_controller +{ +ChainableForwardController::ChainableForwardController() +: ForwardControllersBase(), controller_interface::ChainableControllerInterface() +{ +} + +CallbackReturn ChainableForwardController::on_init() { return execute_init(get_node()); } + +CallbackReturn ChainableForwardController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = execute_configure(previous_state, command_interfaces_); + RCLCPP_ERROR(get_node()->get_logger(), "Configure is successful, now tryting configure ref_itfs"); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // The names should be in the same order as for command interfaces for easier matching + reference_interface_names_ = command_interface_names_; + // for any case make reference interfaces size of command interfaces + reference_interfaces_.resize( + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + for (const auto & value : reference_interfaces_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +ChainableForwardController::command_interface_configuration() const +{ + return get_command_interface_configuration(); +} + +controller_interface::InterfaceConfiguration +ChainableForwardController::state_interface_configuration() const +{ + return get_state_interface_configuration(); +} + +std::vector +ChainableForwardController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + + for (size_t i = 0; i < reference_interface_names_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +bool ChainableForwardController::on_set_chained_mode(bool chained_mode) +{ + // we can set chained mode in any situation + (void)chained_mode; + return true; +} + +CallbackReturn ChainableForwardController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = execute_activate(previous_state, command_interfaces_); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn ChainableForwardController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + return execute_deactivate(previous_state); +} + +controller_interface::return_type ChainableForwardController::update_reference_from_subscribers() +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + // message is valid + if (!(!joint_commands || !(*joint_commands))) + { + reference_interfaces_ = (*joint_commands)->data; + } +} + +controller_interface::return_type ChainableForwardController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isnan(reference_interfaces_[i])) + { + command_interfaces_[i].set_value(reference_interfaces_[i]); + } + } + + return controller_interface::return_type::OK; +} + +} // namespace forward_command_controller diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 46e90cd1c4..9bd27e0215 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,58 +14,20 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include -#include -#include -#include -#include - -#include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" - -namespace forward_command_controller -{ -ForwardCommandController::ForwardCommandController() : ForwardControllersBase() {} - -void ForwardCommandController::declare_parameters() -{ - get_node()->declare_parameter>("joints", std::vector()); - get_node()->declare_parameter("interface_name", ""); -} - -controller_interface::CallbackReturn ForwardCommandController::read_parameters() -{ - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) - { - interface_name_ = get_node()->get_parameter("interface_name").as_string(); - } - - if (interface_name_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & joint : joint_names_) - { - command_interface_types_.push_back(joint + "/" + interface_name_); - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -} // namespace forward_command_controller - +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "pluginlib/class_list_macros.hpp" +// PLUGINLIB_EXPORT_CLASS( +// forward_command_controller::ForwardCommandController< +// forward_command_controller::ForwardController>, +// controller_interface::ControllerInterface) +// PLUGINLIB_EXPORT_CLASS( +// forward_command_controller::ForwardCommandController< +// forward_command_controller::ChainableForwardController>, +// controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ChainableForwardCommandController, + controller_interface::ControllerInterface) diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp new file mode 100644 index 0000000000..0574ce7b5c --- /dev/null +++ b/forward_command_controller/src/forward_controller.cpp @@ -0,0 +1,74 @@ +// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) +// +// 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 "forward_command_controller/forward_controller.hpp" + +#include "forward_command_controller/visibility_control.h" + +namespace forward_command_controller +{ +ForwardController::ForwardController() +: ForwardControllersBase(), controller_interface::ControllerInterface() +{ +} + +controller_interface::InterfaceConfiguration ForwardController::command_interface_configuration() + const +{ + return get_command_interface_configuration(); +} + +controller_interface::InterfaceConfiguration ForwardController::state_interface_configuration() + const +{ + return get_state_interface_configuration(); +} + +CallbackReturn ForwardController::on_init() { return execute_init(get_node()); } + +CallbackReturn ForwardController::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + return execute_configure(previous_state, command_interfaces_); +} + +CallbackReturn ForwardController::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + return execute_activate(previous_state, command_interfaces_); +} + +CallbackReturn ForwardController::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + return execute_deactivate(previous_state); +} + +controller_interface::return_type ForwardController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!joint_commands || !(*joint_commands)) + { + return controller_interface::return_type::OK; + } + + for (auto index = 0ul; index < command_interfaces_.size(); ++index) + { + command_interfaces_[index].set_value((*joint_commands)->data[index]); + } + + return controller_interface::return_type::OK; +} + +} // namespace forward_command_controller diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index e4ea46fcc5..bea74aeb6f 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -28,14 +28,15 @@ namespace forward_command_controller { ForwardControllersBase::ForwardControllersBase() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - joints_command_subscriber_(nullptr) +: rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) { } -controller_interface::CallbackReturn ForwardControllersBase::on_init() +controller_interface::CallbackReturn ForwardControllersBase::execute_init( + const std::shared_ptr & node) { + node_ = node; + try { declare_parameters(); @@ -49,42 +50,53 @@ controller_interface::CallbackReturn ForwardControllersBase::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardControllersBase::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn ForwardControllersBase::execute_configure( + const rclcpp_lifecycle::State & /*previous_state*/, + std::vector & command_interfaces) { auto ret = this->read_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_node()->get_logger(), "Error when reading parameters."); return ret; } - joints_command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + joints_command_subscriber_ = node_->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { + // check if message is correct size, if not ignore + if (msg->data.size() == command_interface_names_.size()) + { + rt_command_ptr_.writeFromNonRT(msg); + } + }); + + // pre-reserve command interfaces + command_interfaces.reserve(command_interface_names_.size()); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration -ForwardControllersBase::command_interface_configuration() const +ForwardControllersBase::get_command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names = command_interface_types_; + command_interfaces_config.names = command_interface_names_; return command_interfaces_config; } -controller_interface::InterfaceConfiguration ForwardControllersBase::state_interface_configuration() - const +controller_interface::InterfaceConfiguration +ForwardControllersBase::get_state_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn ForwardControllersBase::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn ForwardControllersBase::execute_activate( + const rclcpp_lifecycle::State & /*previous_state*/, + std::vector & command_interfaces) { // check if we have all resources defined in the "points" parameter // also verify that we *only* have the resources defined in the "points" parameter @@ -93,12 +105,12 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( ordered_interfaces; if ( !controller_interface::get_ordered_interfaces( - command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || - command_interface_types_.size() != ordered_interfaces.size()) + command_interfaces, command_interface_names_, std::string(""), ordered_interfaces) || + command_interface_names_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu command interfaces, got %zu", - command_interface_types_.size(), ordered_interfaces.size()); + command_interface_names_.size(), ordered_interfaces.size()); return controller_interface::CallbackReturn::ERROR; } @@ -109,7 +121,7 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( +controller_interface::CallbackReturn ForwardControllersBase::execute_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer @@ -117,32 +129,4 @@ controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForwardControllersBase::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - auto joint_commands = rt_command_ptr_.readFromRT(); - - // no command received yet - if (!joint_commands || !(*joint_commands)) - { - return controller_interface::return_type::OK; - } - - if ((*joint_commands)->data.size() != command_interfaces_.size()) - { - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *(get_node()->get_clock()), 1000, - "command size (%zu) does not match number of interfaces (%zu)", - (*joint_commands)->data.size(), command_interfaces_.size()); - return controller_interface::return_type::ERROR; - } - - for (auto index = 0ul; index < command_interfaces_.size(); ++index) - { - command_interfaces_[index].set_value((*joint_commands)->data[index]); - } - - return controller_interface::return_type::OK; -} - } // namespace forward_command_controller diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 6cc22cf549..4828049857 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -14,51 +14,23 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" -#include -#include - -namespace forward_command_controller -{ -MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() -: ForwardControllersBase() -{ -} - -void MultiInterfaceForwardCommandController::declare_parameters() -{ - get_node()->declare_parameter("joint", joint_name_); - get_node()->declare_parameter>("interface_names", interface_names_); -} - -controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() -{ - joint_name_ = get_node()->get_parameter("joint").as_string(); - interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); - - if (joint_name_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - if (interface_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & interface : interface_names_) - { - command_interface_types_.push_back(joint_name_ + "/" + interface); - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -} // namespace forward_command_controller +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "pluginlib/class_list_macros.hpp" +// PLUGINLIB_EXPORT_CLASS( +// forward_command_controller::MultiInterfaceForwardCommandController< +// forward_command_controller::ForwardController>, +// controller_interface::ControllerInterface) +// PLUGINLIB_EXPORT_CLASS( +// forward_command_controller::MultiInterfaceForwardCommandController< +// forward_command_controller::ChainableForwardController>, +// controller_interface::ControllerInterface) + PLUGINLIB_EXPORT_CLASS( forward_command_controller::MultiInterfaceForwardCommandController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ChainableMultiInterfaceForwardCommandController, + controller_interface::ControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..0240dbb4e0 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -23,6 +23,7 @@ #include "test_forward_command_controller.hpp" #include "forward_command_controller/forward_command_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..d9acbdf6c4 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -22,6 +22,7 @@ #include "gmock/gmock.h" #include "forward_command_controller/forward_command_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/forward_command_controller/test/test_load_chained_forward_command_controller.cpp b/forward_command_controller/test/test_load_chained_forward_command_controller.cpp new file mode 100644 index 0000000000..9c93fc8d7e --- /dev/null +++ b/forward_command_controller/test/test_load_chained_forward_command_controller.cpp @@ -0,0 +1,42 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 "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 "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadForwardCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/ChainedForwardCommandController")); + + rclcpp::shutdown(); +} diff --git a/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp new file mode 100644 index 0000000000..f73ba042de --- /dev/null +++ b/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik, Inc. +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 "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 "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMultiInterfaceForwardController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/ChainedMultiInterfaceForwardCommandController")); +} diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index 464b57b69d..03d735fb1c 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -34,6 +34,9 @@ TEST(TestLoadForwardCommandController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + cm.load_controller( + "test_forward_command_controller", "forward_command_controller/ForwardCommandController"); + ASSERT_NO_THROW(cm.load_controller( "test_forward_command_controller", "forward_command_controller/ForwardCommandController")); diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..8846b2bcdc 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -23,6 +23,7 @@ #include "gmock/gmock.h" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 29789b30d3..a1bfdf4469 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa protected: std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index eecbecbb04..f171b018a8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -56,6 +56,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { // with the lifecycle node being initialized, we can declare parameters auto_declare>("joints", joint_names_); + auto_declare>("command_joints", command_joint_names_); auto_declare>("command_interfaces", command_interface_types_); auto_declare>("state_interfaces", state_interface_types_); auto_declare("state_publish_rate", 50.0); @@ -81,8 +82,8 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) + conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : command_interface_types_) { @@ -458,6 +459,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // update parameters joint_names_ = get_node()->get_parameter("joints").as_string_array(); + // TODO(destogl): why is this here? Add comment or move if (!reset()) { return CallbackReturn::FAILURE; @@ -465,9 +467,25 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (joint_names_.empty()) { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { @@ -775,12 +793,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); - return CallbackReturn::ERROR; + command_joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; } } for (const auto & interface : state_interface_types_) @@ -794,7 +812,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } } From 43f39a090f284c0295ae20453fdc2b9211f5f599 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 23 Mar 2022 18:23:11 +0100 Subject: [PATCH 002/111] Enabling chanable mode for forward command controllers. --- .../forward_controllers_base.hpp | 1 - .../src/chainable_forward_controller.cpp | 15 +++++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index ad6e1a5d23..c04ed7b985 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -68,7 +68,6 @@ class ForwardControllersBase const rclcpp_lifecycle::State & previous_state, std::vector & command_interfaces); - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::CallbackReturn execute_deactivate( const rclcpp_lifecycle::State & previous_state); diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 796040516c..3f126244d9 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -32,9 +32,12 @@ ChainableForwardController::ChainableForwardController() { } -CallbackReturn ChainableForwardController::on_init() { return execute_init(get_node()); } +controller_interface::CallbackReturn ChainableForwardController::on_init() +{ + return execute_init(get_node()); +} -CallbackReturn ChainableForwardController::on_configure( +controller_interface::CallbackReturn ChainableForwardController::on_configure( const rclcpp_lifecycle::State & previous_state) { auto ret = execute_configure(previous_state, command_interfaces_); @@ -48,7 +51,7 @@ CallbackReturn ChainableForwardController::on_configure( reference_interface_names_ = command_interface_names_; // for any case make reference interfaces size of command interfaces reference_interfaces_.resize( - reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + command_interfaces_.size(), std::numeric_limits::quiet_NaN()); for (const auto & value : reference_interfaces_) { @@ -91,7 +94,7 @@ bool ChainableForwardController::on_set_chained_mode(bool chained_mode) return true; } -CallbackReturn ChainableForwardController::on_activate( +controller_interface::CallbackReturn ChainableForwardController::on_activate( const rclcpp_lifecycle::State & previous_state) { auto ret = execute_activate(previous_state, command_interfaces_); @@ -104,10 +107,10 @@ CallbackReturn ChainableForwardController::on_activate( reference_interfaces_.begin(), reference_interfaces_.end(), std::numeric_limits::quiet_NaN()); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn ChainableForwardController::on_deactivate( +controller_interface::CallbackReturn ChainableForwardController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { return execute_deactivate(previous_state); From 29c4393a2b5c6d2d7c32be228ba1611afdf42ab7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Mar 2022 17:16:28 +0100 Subject: [PATCH 003/111] Fix all bugs in chained-controllers mode. --- .../src/chainable_forward_controller.cpp | 11 +++++------ .../src/forward_controllers_base.cpp | 2 +- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 3f126244d9..7f59cd50c3 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -51,12 +51,7 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( reference_interface_names_ = command_interface_names_; // for any case make reference interfaces size of command interfaces reference_interfaces_.resize( - command_interfaces_.size(), std::numeric_limits::quiet_NaN()); - - for (const auto & value : reference_interfaces_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); - } + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); return CallbackReturn::SUCCESS; } @@ -133,6 +128,10 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { + RCLCPP_ERROR( + get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", + reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); + command_interfaces_[i].set_value(reference_interfaces_[i]); } } diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index bea74aeb6f..fcb7bb2ab8 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -57,7 +57,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_configure( auto ret = this->read_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_node()->get_logger(), "Error when reading parameters."); + RCLCPP_ERROR(node_->get_logger(), "Error when reading parameters."); return ret; } From 6f2f956818866a77e8413a8b8b47f7bdfea1acf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 29 Mar 2022 19:24:27 +0200 Subject: [PATCH 004/111] Remove debug output from forwarding controller. --- .../src/chainable_forward_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 7f59cd50c3..83f7ceacae 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -128,10 +128,6 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { - RCLCPP_ERROR( - get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", - reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); - command_interfaces_[i].set_value(reference_interfaces_[i]); } } From 704ba5dea205de5a4214fece0bc0073441dd6bcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 21 Mar 2022 16:24:41 +0100 Subject: [PATCH 005/111] Make forward controller chainable. --- .../forward_command_plugin.xml | 32 ------------------- .../chainable_forward_controller.hpp | 4 +++ ...i_interface_forward_command_controller.hpp | 1 - .../src/chainable_forward_controller.cpp | 1 - .../src/forward_command_controller.cpp | 9 +----- ...i_interface_forward_command_controller.cpp | 9 ------ .../test/test_forward_command_controller.hpp | 3 +- ...i_interface_forward_command_controller.cpp | 1 + ...i_interface_forward_command_controller.hpp | 3 +- 9 files changed, 10 insertions(+), 53 deletions(-) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 58026d62b5..37ddedc035 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,35 +1,3 @@ - - ::quiet_NaN()); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 9bd27e0215..82319bbc65 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -16,16 +16,9 @@ #include "forward_command_controller/chainable_forward_controller.hpp" #include "forward_command_controller/forward_controller.hpp" + #include "pluginlib/class_list_macros.hpp" -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::ForwardCommandController< -// forward_command_controller::ForwardController>, -// controller_interface::ControllerInterface) -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::ForwardCommandController< -// forward_command_controller::ChainableForwardController>, -// controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 4828049857..a1020fcb17 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -19,15 +19,6 @@ #include "pluginlib/class_list_macros.hpp" -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::MultiInterfaceForwardCommandController< -// forward_command_controller::ForwardController>, -// controller_interface::ControllerInterface) -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::MultiInterfaceForwardCommandController< -// forward_command_controller::ChainableForwardController>, -// controller_interface::ControllerInterface) - PLUGINLIB_EXPORT_CLASS( forward_command_controller::MultiInterfaceForwardCommandController, controller_interface::ControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index d9acbdf6c4..66a5df629b 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -30,7 +30,8 @@ using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; // subclassing and friending so we can access member variables -class FriendForwardCommandController : public forward_command_controller::ForwardCommandController +class FriendForwardCommandController : public forward_command_controller::ForwardCommandController< + forward_command_controller::ForwardController> { FRIEND_TEST(ForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(ForwardCommandControllerTest, InterfaceParameterNotSet); diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 0cada04859..bb4ded549e 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -24,6 +24,7 @@ #include "test_multi_interface_forward_command_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 8846b2bcdc..74cf5a7550 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -35,7 +35,8 @@ using hardware_interface::HW_IF_VELOCITY; // subclassing and friending so we can access member variables class FriendMultiInterfaceForwardCommandController -: public forward_command_controller::MultiInterfaceForwardCommandController +: public forward_command_controller::MultiInterfaceForwardCommandController< + forward_command_controller::ForwardController> { FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet); From 6759bb583973a394b3b30c1dda1e1bc40e726548 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 23 Mar 2022 18:23:11 +0100 Subject: [PATCH 006/111] Enabling chanable mode for forward command controllers. --- .../forward_command_plugin.xml | 32 +++++++++++++++++++ .../chainable_forward_controller.hpp | 4 --- ...i_interface_forward_command_controller.hpp | 1 + .../src/chainable_forward_controller.cpp | 3 +- .../test/test_forward_command_controller.hpp | 3 +- ...i_interface_forward_command_controller.hpp | 3 +- 6 files changed, 37 insertions(+), 9 deletions(-) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 37ddedc035..3697404a34 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,3 +1,35 @@ + + ::quiet_NaN()); + command_interfaces_.size(), std::numeric_limits::quiet_NaN()); return CallbackReturn::SUCCESS; } diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 66a5df629b..d9acbdf6c4 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -30,8 +30,7 @@ using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; // subclassing and friending so we can access member variables -class FriendForwardCommandController : public forward_command_controller::ForwardCommandController< - forward_command_controller::ForwardController> +class FriendForwardCommandController : public forward_command_controller::ForwardCommandController { FRIEND_TEST(ForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(ForwardCommandControllerTest, InterfaceParameterNotSet); diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 74cf5a7550..8846b2bcdc 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -35,8 +35,7 @@ using hardware_interface::HW_IF_VELOCITY; // subclassing and friending so we can access member variables class FriendMultiInterfaceForwardCommandController -: public forward_command_controller::MultiInterfaceForwardCommandController< - forward_command_controller::ForwardController> +: public forward_command_controller::MultiInterfaceForwardCommandController { FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet); From 7afdefc95a4ae1c1f44d91cb78b593bd183928b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Mar 2022 17:16:28 +0100 Subject: [PATCH 007/111] Fix all bugs in chained-controllers mode. --- .../src/chainable_forward_controller.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index bef1ceffda..09c62ad5b3 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -51,7 +51,12 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( reference_interface_names_ = command_interface_names_; // for any case make reference interfaces size of command interfaces reference_interfaces_.resize( - command_interfaces_.size(), std::numeric_limits::quiet_NaN()); + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + for (const auto & value : reference_interfaces_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); + } return CallbackReturn::SUCCESS; } @@ -128,6 +133,10 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { + RCLCPP_ERROR( + get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", + reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); + command_interfaces_[i].set_value(reference_interfaces_[i]); } } From 7c03f79e76dfa1a8e2cda945e03e64ea16792c07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 29 Mar 2022 19:24:27 +0200 Subject: [PATCH 008/111] Remove debug output from forwarding controller. --- .../src/chainable_forward_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 09c62ad5b3..e1ffa0553b 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -133,10 +133,6 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { - RCLCPP_ERROR( - get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", - reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); - command_interfaces_[i].set_value(reference_interfaces_[i]); } } From f8556eabd73a91e823442e71f23cd764b15b40ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Apr 2022 21:53:24 +0200 Subject: [PATCH 009/111] Some compile fixes --- .../src/forward_controller.cpp | 14 ++++++++++---- .../src/forward_controllers_base.cpp | 6 +++--- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp index 0574ce7b5c..790738a9be 100644 --- a/forward_command_controller/src/forward_controller.cpp +++ b/forward_command_controller/src/forward_controller.cpp @@ -35,19 +35,25 @@ controller_interface::InterfaceConfiguration ForwardController::state_interface_ return get_state_interface_configuration(); } -CallbackReturn ForwardController::on_init() { return execute_init(get_node()); } +controller_interface::CallbackReturn ForwardController::on_init() +{ + return execute_init(get_node()); +} -CallbackReturn ForwardController::on_configure(const rclcpp_lifecycle::State & previous_state) +controller_interface::CallbackReturn ForwardController::on_configure( + const rclcpp_lifecycle::State & previous_state) { return execute_configure(previous_state, command_interfaces_); } -CallbackReturn ForwardController::on_activate(const rclcpp_lifecycle::State & previous_state) +controller_interface::CallbackReturn ForwardController::on_activate( + const rclcpp_lifecycle::State & previous_state) { return execute_activate(previous_state, command_interfaces_); } -CallbackReturn ForwardController::on_deactivate(const rclcpp_lifecycle::State & previous_state) +controller_interface::CallbackReturn ForwardController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) { return execute_deactivate(previous_state); } diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index fcb7bb2ab8..3aa76d4423 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -73,7 +73,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_configure( // pre-reserve command interfaces command_interfaces.reserve(command_interface_names_.size()); - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + RCLCPP_INFO(node_->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -109,7 +109,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_activate( command_interface_names_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + node_->get_logger(), "Expected %zu command interfaces, got %zu", command_interface_names_.size(), ordered_interfaces.size()); return controller_interface::CallbackReturn::ERROR; } @@ -117,7 +117,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_activate( // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); - RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + RCLCPP_INFO(node_->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; } From d77b4b6b1c9723c8d955855e547fefa95d2fbcde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Apr 2022 22:20:24 +0200 Subject: [PATCH 010/111] Fix some small issues. --- .../src/chainable_forward_controller.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index e1ffa0553b..25a247dd2e 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -41,7 +41,6 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( const rclcpp_lifecycle::State & previous_state) { auto ret = execute_configure(previous_state, command_interfaces_); - RCLCPP_ERROR(get_node()->get_logger(), "Configure is successful, now tryting configure ref_itfs"); if (ret != CallbackReturn::SUCCESS) { return ret; @@ -53,11 +52,6 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( reference_interfaces_.resize( reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); - for (const auto & value : reference_interfaces_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); - } - return CallbackReturn::SUCCESS; } @@ -124,6 +118,8 @@ controller_interface::return_type ChainableForwardController::update_reference_f { reference_interfaces_ = (*joint_commands)->data; } + + return controller_interface::return_type::OK; } controller_interface::return_type ChainableForwardController::update_and_write_commands( From 778ed130ee8ab951509c18cce57ae1e41bdbb45d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 16 May 2022 20:56:42 +0200 Subject: [PATCH 011/111] Update to use class. --- forward_command_controller/forward_command_plugin.xml | 4 ++-- .../forward_command_controller.hpp | 6 +++--- .../multi_interface_forward_command_controller.hpp | 6 +++--- .../src/forward_command_controller.cpp | 2 +- .../src/multi_interface_forward_command_controller.cpp | 2 +- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 3697404a34..47d661d603 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -48,14 +48,14 @@ + base_class_type="controller_interface::ChainableControllerInterface"> The forward command controller commands a group of joints in a given interface + base_class_type="controller_interface::ChainableControllerInterface"> MultiInterfaceForwardController ros2_control controller. diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index c8f8dcca23..d799be9245 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -43,7 +43,7 @@ template < std::is_convertible::value, T>::type * = nullptr, typename std::enable_if< - std::is_convertible::value, T>::type * = + std::is_convertible::value, T>::type * = nullptr> class BaseForwardCommandController : public T { @@ -54,9 +54,9 @@ class BaseForwardCommandController : public T protected: void declare_parameters() override { - controller_interface::ControllerInterface::auto_declare>( + controller_interface::ControllerInterfaceBase::auto_declare>( "joints", std::vector()); - controller_interface::ControllerInterface::auto_declare("interface_name", ""); + controller_interface::ControllerInterfaceBase::auto_declare("interface_name", ""); }; controller_interface::CallbackReturn read_parameters() override diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 30cabfa6d6..273832e727 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -43,7 +43,7 @@ template < std::is_convertible::value, T>::type * = nullptr, typename std::enable_if< - std::is_convertible::value, T>::type * = + std::is_convertible::value, T>::type * = nullptr> class BaseMultiInterfaceForwardCommandController : public T { @@ -54,8 +54,8 @@ class BaseMultiInterfaceForwardCommandController : public T protected: void declare_parameters() override { - controller_interface::ControllerInterface::auto_declare("joint", joint_name_); - controller_interface::ControllerInterface::auto_declare>( + controller_interface::ControllerInterfaceBase::auto_declare("joint", joint_name_); + controller_interface::ControllerInterfaceBase::auto_declare>( "interface_names", interface_names_); }; diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 82319bbc65..55be87dfae 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -23,4 +23,4 @@ PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ChainableForwardCommandController, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index a1020fcb17..e6749bda9e 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -24,4 +24,4 @@ PLUGINLIB_EXPORT_CLASS( controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ChainableMultiInterfaceForwardCommandController, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) From 39b640e2df877ff1f09f01263ec7040c4b1a06b5 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 19 Jun 2022 17:31:55 -0600 Subject: [PATCH 012/111] added admittance controller --- admittance_controller/CMakeLists.txt | 143 +++++ admittance_controller/README.md | 12 + .../admittance_controller.xml | 9 + .../admittance_controller.hpp | 170 ++++++ .../admittance_controller/admittance_rule.hpp | 363 ++++++++++++ .../admittance_rule_impl.hpp | 362 ++++++++++++ .../parameter_handler.hpp | 388 +++++++++++++ .../visibility_control.h | 49 ++ admittance_controller/package.xml | 44 ++ .../src/admittance_controller.cpp | 523 ++++++++++++++++++ .../src/parameter_handler.cpp | 216 ++++++++ .../test/test_admittance_controller.cpp | 446 +++++++++++++++ .../test/test_admittance_controller.hpp | 449 +++++++++++++++ .../test/test_load_admittance_controller.cpp | 41 ++ 14 files changed, 3215 insertions(+) create mode 100644 admittance_controller/CMakeLists.txt create mode 100644 admittance_controller/README.md create mode 100644 admittance_controller/admittance_controller.xml create mode 100644 admittance_controller/include/admittance_controller/admittance_controller.hpp create mode 100644 admittance_controller/include/admittance_controller/admittance_rule.hpp create mode 100644 admittance_controller/include/admittance_controller/admittance_rule_impl.hpp create mode 100644 admittance_controller/include/admittance_controller/parameter_handler.hpp create mode 100644 admittance_controller/include/admittance_controller/visibility_control.h create mode 100644 admittance_controller/package.xml create mode 100644 admittance_controller/src/admittance_controller.cpp create mode 100644 admittance_controller/src/parameter_handler.cpp create mode 100644 admittance_controller/test/test_admittance_controller.cpp create mode 100644 admittance_controller/test/test_admittance_controller.hpp create mode 100644 admittance_controller/test/test_load_admittance_controller.cpp diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt new file mode 100644 index 0000000000..089d9a6dcb --- /dev/null +++ b/admittance_controller/CMakeLists.txt @@ -0,0 +1,143 @@ +cmake_minimum_required(VERSION 3.5) +project(admittance_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(control_msgs REQUIRED) +find_package(control_toolbox REQUIRED) +find_package(controller_interface REQUIRED) +find_package(kinematics_interface REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(joint_trajectory_controller REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(angles REQUIRED) +find_package(rcutils REQUIRED) + + + +# The admittance controller +add_library(admittance_controller SHARED + src/admittance_controller.cpp + src/parameter_handler.cpp +) +target_include_directories( + admittance_controller + PRIVATE + include +) + +ament_target_dependencies( + admittance_controller + backward_ros + control_msgs + control_toolbox + controller_interface + kinematics_interface + ${Eigen_LIBRARIES} + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + angles +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) + + +install( + TARGETS admittance_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +set(BUILD_TESTING 0) +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) + target_include_directories(test_load_admittance_controller PRIVATE include) + ament_target_dependencies( + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) + target_include_directories(test_admittance_controller PRIVATE include) + target_link_libraries(test_admittance_controller admittance_controller) + ament_target_dependencies( + test_admittance_controller + control_msgs + controller_interface + hardware_interface + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) + +ament_export_libraries( + admittance_controller +) + + +ament_export_dependencies( + backward_ros + control_msgs + control_toolbox + controller_interface + kinematics_interface + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs tf2_ros +) + +ament_package() diff --git a/admittance_controller/README.md b/admittance_controller/README.md new file mode 100644 index 0000000000..e182700c9f --- /dev/null +++ b/admittance_controller/README.md @@ -0,0 +1,12 @@ +admittance_controller +========================================== + +Implemenation of admittance controllers for different input and output interface. + +Pluginlib-Library: admittance_controller +Plugin: admittance_controller/AdmittanceController (controller_interface::ControllerInterface) + +Requires an external differential IK plugin. One possibility is at: + +https://github.com/PickNikRobotics/moveit_differential_ik_plugin +# admittance_controller diff --git a/admittance_controller/admittance_controller.xml b/admittance_controller/admittance_controller.xml new file mode 100644 index 0000000000..3e4d5cb682 --- /dev/null +++ b/admittance_controller/admittance_controller.xml @@ -0,0 +1,9 @@ + + + + AdmittanceController ros2_control controller. + + + diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp new file mode 100644 index 0000000000..82fecca5a8 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -0,0 +1,170 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "admittance_controller/admittance_rule.hpp" +#include "admittance_controller/visibility_control.h" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/force_torque_sensor.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" + + +// TODO(destogl): this is only temporary to work with servo. It should be either trajectory_msgs/msg/JointTrajectoryPoint or std_msgs/msg/Float64MultiArray +#include "trajectory_msgs/msg/joint_trajectory.hpp" + + +using namespace std::chrono_literals; + +namespace admittance_controller +{ + using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + struct RTBuffers{ + realtime_tools::RealtimeBuffer> input_wrench_command_; + realtime_tools::RealtimeBuffer> input_joint_command_; + std::unique_ptr> state_publisher_; + }; + + struct ParameterStruct{ + std::vector joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + std::vector chainable_command_interface_types_; + std::string ft_sensor_name_; + bool use_joint_commands_as_input_; + std::string joint_limiter_type_; + bool allow_partial_joints_goal_; + bool allow_integration_in_goal_trajectories_; + double action_monitor_rate_; + bool open_loop_control_; + }; + + +class AdmittanceController : public controller_interface::ChainableControllerInterface +{ +public: + ADMITTANCE_CONTROLLER_PUBLIC + AdmittanceController(); + + ADMITTANCE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ADMITTANCE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + +protected: + std::vector on_export_reference_interfaces() override; + controller_interface::return_type update_reference_from_subscribers() override; + bool on_set_chained_mode(bool chained_mode) override; + + int num_joints_{}; + std::vector> joint_position_command_interface_; + std::vector> joint_velocity_command_interface_; + std::vector> joint_acceleration_command_interface_; + std::vector> joint_effort_command_interface_; + std::vector> joint_position_state_interface_; + std::vector> joint_velocity_state_interface_; + std::vector> joint_acceleration_state_interface_; + std::vector> joint_position_chainable_interface_; + std::vector> joint_velocity_chainable_interface_; + std::vector> joint_acceleration_chainable_interface_; + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + // joint limiter TODO + std::unique_ptr force_torque_sensor_; + // controller parameters filled by ROS + ParameterStruct params; + // ROS subscribers + rclcpp::Subscription::SharedPtr input_wrench_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; + rclcpp::Publisher::SharedPtr s_publisher_ = nullptr; + // ROS messages + std::shared_ptr traj_command_msg; + std::shared_ptr wrench_msg; + std::shared_ptr joint_command_msg; + // real-time buffer + RTBuffers rtBuffers; + + // controller running state + bool controller_is_active_{}; + const std::set allowed_state_interface_types_ = { + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, + }; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + trajectory_msgs::msg::JointTrajectoryPoint last_state_reference_; + trajectory_msgs::msg::JointTrajectoryPoint state_offset_; + trajectory_msgs::msg::JointTrajectoryPoint prev_trajectory_point_; + // control loop data + trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, + state_error_; + trajectory_msgs::msg::JointTrajectory pre_admittance_point; + std::vector open_loop_buffer; + + // helper methods + void wrench_stamped_callback(const std::shared_ptr msg); + void joint_command_callback(const std::shared_ptr msg); + void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state); + void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + void read_state_reference_from_chainable_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp new file mode 100644 index 0000000000..1b2bc1a06d --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -0,0 +1,363 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ + +#include + +#include +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "admittance_controller/parameter_handler.hpp" +#include "controller_interface/controller_interface.hpp" +#include "control_toolbox/filters.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" + + +// kinematics plugins +#include "kinematics_interface/kinematics_interface_base.hpp" +#include "pluginlib/class_loader.hpp" + +namespace { // Utility namespace + +// Numerical accuracy checks. Used as deadbands. + static constexpr double WRENCH_EPSILON = 1e-10; + static constexpr double POSE_ERROR_EPSILON = 1e-12; + static constexpr double POSE_EPSILON = 1e-15; + + +} // utility namespace + +namespace admittance_controller { + + class AdmittanceParameters : public control_toolbox::ParameterHandler { + public: + AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 24, 6) { + add_string_parameter("IK.base", false); + add_string_parameter("IK.group_name", false); + add_string_parameter("IK.plugin_name", false); + add_string_parameter("control_frame", true); + add_string_parameter("sensor_frame", false); + add_string_parameter("end_effector_name", false); + + add_bool_parameter("open_loop_control", true); + add_bool_parameter("enable_parameter_update_without_reactivation", false); + + add_bool_parameter("admittance.selected_axes.x", true); + add_bool_parameter("admittance.selected_axes.y", true); + add_bool_parameter("admittance.selected_axes.z", true); + add_bool_parameter("admittance.selected_axes.rx", true); + add_bool_parameter("admittance.selected_axes.ry", true); + add_bool_parameter("admittance.selected_axes.rz", true); + + add_double_parameter("admittance.mass.x", true); + add_double_parameter("admittance.mass.y", true); + add_double_parameter("admittance.mass.z", true); + add_double_parameter("admittance.mass.rx", true); + add_double_parameter("admittance.mass.ry", true); + add_double_parameter("admittance.mass.rz", true); + add_double_parameter("admittance.stiffness.x", true); + add_double_parameter("admittance.stiffness.y", true); + add_double_parameter("admittance.stiffness.z", true); + add_double_parameter("admittance.stiffness.rx", true); + add_double_parameter("admittance.stiffness.ry", true); + add_double_parameter("admittance.stiffness.rz", true); + add_double_parameter("admittance.damping.x", true); + add_double_parameter("admittance.damping.y", true); + add_double_parameter("admittance.damping.z", true); + add_double_parameter("admittance.damping.rx", true); + add_double_parameter("admittance.damping.ry", true); + add_double_parameter("admittance.damping.rz", true); + add_double_parameter("admittance.damping_ratio.x", true); + add_double_parameter("admittance.damping_ratio.y", true); + add_double_parameter("admittance.damping_ratio.z", true); + add_double_parameter("admittance.damping_ratio.rx", true); + add_double_parameter("admittance.damping_ratio.ry", true); + add_double_parameter("admittance.damping_ratio.rz", true); + } + + bool check_if_parameters_are_valid() override { + bool ret = true; + + // Check if any string parameter is empty + ret = !empty_parameter_in_list(string_parameters_); + + int index = 0; + int offset_index_bool = 2; + // check if parameters are all properly set for selected axes + for (size_t i = 0; i < 6; ++i) { + if (bool_parameters_[offset_index_bool + i].second) { + // check mass parameters + index = i; + if (std::isnan(double_parameters_[index].second)) { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), + "Parameter '%s' has to be set", double_parameters_[index].first.name.c_str()); + ret = false; + } + // Check stiffness parameters + index = i + 6; + if (std::isnan(double_parameters_[index].second)) { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), + "Parameter '%s' has to be set", double_parameters_[index].first.name.c_str()); + ret = false; + } + // Check damping or damping_ratio parameters + index = i + 12; + if (std::isnan(double_parameters_[index].second) && + std::isnan(double_parameters_[index + 6].second)) { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), + "Either parameter '%s' of '%s' has to be set", + double_parameters_[index].first.name.c_str(), + double_parameters_[index + 6].first.name.c_str() + ); + ret = false; + } + } + } + + return ret; + } + + /** + * Conversion to damping when damping_ratio (zeta) parameter is used. + * Using formula: D = damping_ratio * 2 * sqrt( M * S ) + */ + void convert_damping_ratio_to_damping() { + for (auto i = 0ul; i < damping_ratio_.size(); ++i) { + if (!std::isnan(damping_ratio_[i])) { + damping_[i] = damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); + } else { + RCUTILS_LOG_DEBUG_NAMED( + logger_name_.c_str(), + "Damping ratio for axis %zu not used because it is NaN.", i); + } + } + } + + void update_storage() override { + ik_base_frame_ = string_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "IK Base frame: %s", ik_base_frame_.c_str()); + ik_group_name_ = string_parameters_[1].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "IK group name frame: %s", ik_group_name_.c_str()); + ik_plugin_name_ = string_parameters_[2].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "IK plugin name: %s", ik_plugin_name_.c_str()); + control_frame_ = string_parameters_[3].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Control frame: %s", control_frame_.c_str()); + sensor_frame_ = string_parameters_[4].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Sensor frame: %s", sensor_frame_.c_str()); + end_effector_name_ = string_parameters_[5].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Sensor frame: %s", sensor_frame_.c_str()); + + open_loop_control_ = bool_parameters_[0].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Using open loop: %s", (open_loop_control_ ? "True" : "False")); + enable_parameter_update_without_reactivation_ = bool_parameters_[1].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Using update without reactivation: %s", (enable_parameter_update_without_reactivation_ ? "True" : "False")); + + + int offset_index_bool = 2; // 2 because there are already two parameters used above + for (size_t i = 0; i < 6; ++i) { + selected_axes_[i] = bool_parameters_[i + offset_index_bool].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Axis %zu is %sselected", i, (selected_axes_[i] ? "" : "not ")); + + mass_[i] = double_parameters_[i].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Mass for the axis %zu is %e", i, mass_[i]); + stiffness_[i] = double_parameters_[i + 6].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Stiffness for the axis %zu is %e", i, stiffness_[i]); + damping_[i] = double_parameters_[i + 12].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Damping for the axis %zu is %e", i, damping_[i]); + damping_ratio_[i] = double_parameters_[i + 18].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Damping_ratio for the axis %zu is %e", i, damping_ratio_[i]); + } + + convert_damping_ratio_to_damping(); + } + + // IK parameters + std::string ik_base_frame_; + std::string ik_group_name_; + std::string ik_plugin_name_; + std::string end_effector_name_; + // Depends on the scenario: usually base_link, tool or end-effector + std::string control_frame_; + // Admittance calculations (displacement etc) are done in this frame. + // Frame where wrench measurements are taken + std::string sensor_frame_; + + bool open_loop_control_; + bool enable_parameter_update_without_reactivation_; + + std::array damping_; + std::array damping_ratio_; + std::array mass_; + std::array selected_axes_; + std::array stiffness_; + }; + + + class AdmittanceRule { + public: + AdmittanceRule() = default; + + controller_interface::return_type configure(std::shared_ptr node, int num_joint); + + controller_interface::return_type reset(); + +// controller_interface::return_type update( +// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, +// const geometry_msgs::msg::Wrench & measured_wrench, +// const geometry_msgs::msg::PoseStamped & reference_pose, +// const rclcpp::Duration & period, +// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states +// ); +// +// controller_interface::return_type update( +// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, +// const geometry_msgs::msg::Wrench & measured_wrench, +// const std::array & reference_joint_deltas, +// const rclcpp::Duration & period, +// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states +// ); + + /** + * Calculate 'desired joint states' based on the 'measured force' and 'reference joint state'. + * + * \param[in] current_joint_state + * \param[in] measured_wrench + * \param[in] reference_joint_state + * \param[in] period + * \param[out] desired_joint_state + */ + controller_interface::return_type update( + const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, + const geometry_msgs::msg::Wrench &measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, + const rclcpp::Duration &period, + trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states); + +// controller_interface::return_type update( +// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, +// const geometry_msgs::msg::Wrench & measured_wrench, +// const geometry_msgs::msg::PoseStamped & reference_pose, +// const geometry_msgs::msg::WrenchStamped & reference_force, +// const rclcpp::Duration & period, +// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states +// ); + + controller_interface::return_type get_controller_state( + control_msgs::msg::AdmittanceControllerState &state_message + ); + + + public: + // TODO(destogl): Add parameter for this + bool feedforward_commanded_input_ = true; + + + // Dynamic admittance parameters + AdmittanceParameters parameters_; + + // Filter parameter for exponential smoothing + const double alpha = 0.1; // TODO make a ros param + const double EPSILON = 0.001; + + + protected: + /** + * All values are in he controller frame + */ + void calculate_admittance_rule( + const Eigen::Vector &wrench, + const rclcpp::Duration &period + ); + + void process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench + ); + + Eigen::Matrix4d invert_transform(const Eigen::Matrix4d &T); + Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d& R) const; + Eigen::Matrix3d normalize_rotation(Eigen::Matrix3d R); + + // Differential IK algorithm (loads a plugin) + std::shared_ptr> ik_loader_; + std::unique_ptr ik_; + + // + int num_joints_; + + Eigen::Vector admittance_acceleration_; + std::vector admittance_acceleration_vec; + Eigen::Vector admittance_velocity_; + std::vector admittance_velocity_vec; + Eigen::Matrix admittance_position_; + std::vector admittance_position_vec; + + Eigen::Matrix cur_ee_transform; + std::vector cur_ee_transform_vec; + Eigen::Matrix desired_ee_transform; + std::vector desired_ee_transform_vec; + Eigen::Matrix cur_sensor_transform; + std::vector cur_sensor_transform_vec; + Eigen::Matrix cur_control_transform; + std::vector cur_control_transform_vec; + + Eigen::Vector wrench; + + Eigen::Vector desired_ee_vel; + std::vector desired_ee_vel_vec; + + std::vector joint_vel; + std::vector joint_acc; + std::vector joint_pos; + + trajectory_msgs::msg::JointTrajectoryPoint admittance_rule_calculated_values_; + + private: + + }; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp new file mode 100644 index 0000000000..21c2bfbaf8 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -0,0 +1,362 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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: Denis Stogl, Andy Zelenak + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ + +#include "admittance_controller/admittance_rule.hpp" + +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2_eigen/tf2_eigen.hpp" + +namespace admittance_controller { + + controller_interface::return_type + AdmittanceRule::configure(std::shared_ptr node, int num_joints) { + // initialize memory (non-realtime function) + + num_joints_ = num_joints; + + // initialize all value to zero + reset(); + + // Load the differential IK plugin + if (!parameters_.ik_plugin_name_.empty()) { + try { + // TODO(destogl): add "ik_interface" into separate package and then rename the package in + // the next line from "admittance_controller" to "ik_base_plugin" + ik_loader_ = std::make_shared>( + "kdl_plugin", "kinematics_interface::KinematicsBaseClass"); + ik_ = std::unique_ptr( + ik_loader_->createUnmanagedInstance(parameters_.ik_plugin_name_)); + if (!ik_->initialize(node, parameters_.end_effector_name_)) { + return controller_interface::return_type::ERROR; + } + } + catch (pluginlib::PluginlibException &ex) { + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", + parameters_.ik_plugin_name_.c_str(), ex.what()); + return controller_interface::return_type::ERROR; + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), + "A differential IK plugin name was not specified in the config file."); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; + } + + controller_interface::return_type AdmittanceRule::reset() { + // reset all values back to zero + + // admittance state vectors + admittance_position_vec.resize(16, 0.0); + admittance_velocity_vec.resize(6, 0.0); + admittance_acceleration_vec.resize(6, 0.0); + + // admittance state vectors in joint space + joint_pos.resize(num_joints_, 0.0); + joint_vel.resize(num_joints_, 0.0); + joint_acc.resize(num_joints_, 0.0); + + // link transforms in base link frame + cur_ee_transform_vec.resize(16, 0.0); + desired_ee_transform_vec.resize(16, 0.0); + cur_sensor_transform_vec.resize(16, 0.0); + cur_control_transform_vec.resize(16, 0.0); + + cur_ee_transform.setZero(); + desired_ee_transform.setZero(); + cur_sensor_transform.setZero(); + cur_control_transform.setZero(); + admittance_position_.setIdentity(); + admittance_velocity_.setZero(); + admittance_acceleration_.setZero(); + wrench.setZero(); + desired_ee_vel.setZero(); + + return controller_interface::return_type::OK; + } + +// Update from reference joint states + controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, + const geometry_msgs::msg::Wrench &measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, + const rclcpp::Duration &period, + trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { + + desired_joint_state = reference_joint_state; + bool no_failure = true; + + no_failure &= ik_->calculate_link_transform(desired_joint_state.positions, parameters_.end_effector_name_, desired_ee_transform_vec); + memcpy(desired_ee_transform.data(),desired_ee_transform_vec.data(), 16*sizeof(double)); + + no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.end_effector_name_, cur_ee_transform_vec); + memcpy(cur_ee_transform.data(),cur_ee_transform_vec.data(), 16*sizeof(double)); + + no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.sensor_frame_, cur_sensor_transform_vec); + memcpy(cur_sensor_transform.data(),cur_sensor_transform_vec.data(), 16*sizeof(double)); + + no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.control_frame_, cur_control_transform_vec); + memcpy(cur_control_transform.data(),cur_control_transform_vec.data(), 16*sizeof(double)); + + auto cur_control_transform_inv = invert_transform(cur_control_transform); + auto cur_control_rot_inv = cur_control_transform_inv(Eigen::seq(0,2), Eigen::seq(0,2)); + auto cur_control_rot = cur_control_transform(Eigen::seq(0,2), Eigen::seq(0,2)); + auto cur_sensor_rot = cur_sensor_transform(Eigen::seq(0,2), Eigen::seq(0,2)); + + desired_ee_vel(Eigen::seq(0,2)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(0,2)); + desired_ee_vel(Eigen::seq(3,5)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(3,5)); + + // apply filter and update wrench vector + process_wrench_measurements(measured_wrench); + // transform into control frame + Eigen::Vector wrench_control; + wrench_control(Eigen::seq(0, 2)) = cur_control_rot_inv * cur_sensor_rot * wrench(Eigen::seq(0, 2)); + wrench_control(Eigen::seq(3, 5)) = cur_control_rot_inv * cur_sensor_rot * wrench(Eigen::seq(3, 5)); + + // Compute admittance control law: F = M*a + D*v + S*(x - x_d) + calculate_admittance_rule(wrench_control, period); + + // transform admittance values back to base frame + // velocity + Eigen::Vector3d admittance_velocity_cf = cur_control_rot*admittance_velocity_(Eigen::seq(0,2)); + memcpy(admittance_velocity_vec.data(),admittance_velocity_cf.data(), 3*sizeof(double)); + Eigen::Vector3d admittance_angular_vel_cf = cur_control_rot*admittance_velocity_(Eigen::seq(3,5)); + memcpy(admittance_velocity_vec.data()+3, admittance_angular_vel_cf.data(), 3*sizeof(double)); + // velocity acceleration + Eigen::Vector3d admittance_accel_cf = cur_control_rot * admittance_acceleration_(Eigen::seq(0, 2)); + memcpy(admittance_acceleration_vec.data(),admittance_accel_cf.data(), 3*sizeof(double)); + Eigen::Vector3d admittance_angular_accel_cf = cur_control_rot * admittance_acceleration_(Eigen::seq(3, 5)); + memcpy(admittance_acceleration_vec.data()+3, admittance_angular_vel_cf.data(), 3*sizeof(double)); + + // calculate robot joint velocity from admittance velocity + no_failure &= ik_->convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_vec, joint_vel); + // calculate robot joint acceleration from admittance velocity + no_failure &= ik_->convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_vec, joint_acc); + + if (!no_failure){ + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; + } + + // update joint desired joint state + for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { + joint_pos[i] += joint_vel[i] * (1.0 / 1000.0);//- .2 * joint_pos[i] * (1.0 / 1000.0); + desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos[i]; + } + for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { + desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel[i]; + } + for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { + desired_joint_state.accelerations[i] = joint_acc[i]; + } + + return controller_interface::return_type::OK; + } + + void AdmittanceRule::calculate_admittance_rule( + const Eigen::Vector &wrench, + const rclcpp::Duration &period + ) { + // Compute admittance control law: F = M*a + D*v + S*(x - x_d) + std::array pose_error = {}; + for (size_t axis = 0; axis < 3; ++axis) { //TODO 6 + if (parameters_.selected_axes_[axis]) { + pose_error[axis] = -admittance_position_(axis, 3); + admittance_acceleration_[axis] = (1.0 / parameters_.mass_[axis]) * (wrench[axis] + + (parameters_.damping_[axis] * + (desired_ee_vel[axis] - + admittance_velocity_[axis])) + + (parameters_.stiffness_[axis] * + pose_error[axis])); + admittance_velocity_[axis] += admittance_acceleration_[axis] * (1.0 / 1000);//period.nanoseconds() TODO fix time isssue + admittance_position_(axis, 3) += admittance_velocity_[axis] * (1.0 / 1000); + } + } + + auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); + Eigen::Vector3d V = get_rotation_axis(R); + double theta = acos((1.0/2.0)*(R.trace()-1)); + // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped + auto tmp = V[0]*(R(1,2)-R(2,1))+V[1]*(R(2,0)-R(0,2)) + +V[2]*(R(0,1)-R(1,0)); + double sign = (tmp >= 0) ? 1.0 : -1.0; + + for (size_t axis = 3; axis < 6; ++axis) { + if (parameters_.selected_axes_[axis]) { + pose_error[axis] = sign*theta*V(axis-3); + admittance_acceleration_[axis] = (1.0 / parameters_.mass_[axis]) * (wrench[axis] + + (parameters_.damping_[axis] * + (desired_ee_vel[axis] - + admittance_velocity_[axis])) + + (parameters_.stiffness_[axis] * + pose_error[axis])); + admittance_velocity_[axis] += admittance_acceleration_[axis] * (1.0 / 1000);//period.nanoseconds() TODO fix time issue + } + } + + Eigen::Matrix3d skew_symmetric; + skew_symmetric << 0, -admittance_velocity_[2+3], admittance_velocity_[1+3], + admittance_velocity_[2+3], 0, -admittance_velocity_[0+3], + -admittance_velocity_[1+3], admittance_velocity_[0+3], 0; + + Eigen::Matrix3d R_dot = skew_symmetric*R; + R += R_dot*(1.0 / 1000); + R = normalize_rotation(R); + admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)) = R; + } + + Eigen::Vector3d AdmittanceRule::get_rotation_axis(const Eigen::Matrix3d& R) const{ + + Eigen::Vector3d V; + bool solved = false; + double v1 = 1; + double v2, v3; + double R_buffer[9]; + + // rotate matrix rows and columns to hit every edge case + for (int i = 0; i < 3; i++){ + + for(int r = 0; r < 3; r++){ + for(int c = 0; c < 3; c++){ + R_buffer[((c*3 - 3*i) % 9) + ((r - i) % 3)] = R(r,c); + } + } + auto R11 = R_buffer[0]; + auto R12 = R_buffer[1]; + auto R32 = R_buffer[7]; + auto R13 = R_buffer[2]; + auto R33 = R_buffer[8]; + // degenerate: one axis rotation + if (abs(R12+R13) < EPSILON && R11 > 0){ + v2 = 0; + v3 = 0; + V[i % 3] = v1; + V[(i+1) % 3] = v2; + V[(i+2) % 3] = v3; + solved = true; + break; + } + // degenerate: two axis rotation + if (abs(R12 + R13) < EPSILON && R11 < 0){ + v1 = 0; + v2 = 1; + v3 = R32/(1-R33); + V[i % 3] = v1; + V[(i+1) % 3] = v2; + V[(i+2) % 3] = v3; + solved = true; + break; + } + } + + // general case: three axis rotation + if (!solved){ + v3 = (-R(0,1) -((R(1,1)-1)*(1-R(0,0)))/R(1,0)); + // if v3 is zero, special case + if (abs(v3) > EPSILON) { + v3 = v3 / (R(2,1) - ((R(1,1) - 1) * (R(2,0))) / (R(1,0))); + } + v2 = (1-R(0,0)-R(2,0)*v3)/R(1,0); + + V[0] = v1; + V[1] = v2; + V[2] = v3; + } + + V.normalize(); + return V; + } + + Eigen::Matrix3d AdmittanceRule::normalize_rotation(Eigen::Matrix3d R){ + // enforce orthonormal constraint + + Eigen::Vector3d R_0 = R(Eigen::seq(0,2), Eigen::seq(0,0)); + R_0.normalize(); + Eigen::Vector3d R_1 = R(Eigen::seq(0,2), Eigen::seq(1,1)); + R_1.normalize(); + Eigen::Vector3d R_2 = R(Eigen::seq(0,2), Eigen::seq(2,2)); + R_2.normalize(); + + double drift = R_0.dot(R_1); + R_1 += R_0*(-drift); + R_1.normalize(); + + R_2 = R_0.cross(R_1); + R_2.normalize(); + + Eigen::Matrix3d R_out; + R_out.col(0) << R_0; + R_out.col(1) << R_1; + R_out.col(2) << R_2; + + return R_out; + } + + Eigen::Matrix4d AdmittanceRule::invert_transform(const Eigen::Matrix4d & T){ + Eigen::Matrix4d T_inv = T; + Eigen::Matrix3d R = T_inv(Eigen::seq(0,2), Eigen::seq(0,2)); + T_inv(Eigen::seq(0,2), Eigen::seq(0,2)) = R.transpose(); + T_inv(Eigen::seq(0,2), Eigen::seq(3,3)) = -R.transpose()*T_inv(Eigen::seq(0,2), Eigen::seq(3,3)); + return T_inv; + + } + + controller_interface::return_type AdmittanceRule::get_controller_state( + control_msgs::msg::AdmittanceControllerState &state_message) { + // TODO update this + // state_message.input_wrench_control_frame = reference_wrench_control_frame_; +// state_message.input_pose_control_frame = reference_pose_ik_base_frame_; +// state_message.measured_wrench = measured_wrench_; +// state_message.measured_wrench_filtered = measured_wrench_filtered_; +// state_message.measured_wrench_control_frame = measured_wrench_ik_base_frame_; +// +// state_message.admittance_rule_calculated_values = admittance_rule_calculated_values_; +// +// state_message.current_pose = current_pose_ik_base_frame_; +// state_message.desired_pose = admittance_pose_ik_base_frame_; +// // TODO(destogl): Enable this field for debugging. +//// state_message.relative_admittance = sum_of_admittance_displacements_; +// state_message.relative_desired_pose = relative_admittance_pose_ik_base_frame_; + + return controller_interface::return_type::OK; + } + + void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench + ) { + wrench[0] = filters::exponentialSmoothing( + measured_wrench.force.x, wrench[0], alpha); + wrench[1] = filters::exponentialSmoothing( + measured_wrench.force.y, wrench[1], alpha); + wrench[2] = filters::exponentialSmoothing( + measured_wrench.force.z, wrench[2], alpha); + + wrench[3] = filters::exponentialSmoothing( + measured_wrench.torque.x, wrench[3], alpha); + wrench[4]= filters::exponentialSmoothing( + measured_wrench.torque.y, wrench[4], alpha); + wrench[5] = filters::exponentialSmoothing( + measured_wrench.torque.z, wrench[5], alpha); + } + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ diff --git a/admittance_controller/include/admittance_controller/parameter_handler.hpp b/admittance_controller/include/admittance_controller/parameter_handler.hpp new file mode 100644 index 0000000000..7f7037547c --- /dev/null +++ b/admittance_controller/include/admittance_controller/parameter_handler.hpp @@ -0,0 +1,388 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. +// +/// \author: Denis Stogl + +#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ +#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rcutils/logging_macros.h" + +namespace control_toolbox +{ +using rclcpp::ParameterType; + +namespace impl +{ +inline std::string normalize_params_prefix(std::string prefix) +{ + if (!prefix.empty()) + { + if ('.' != prefix.back()) + { + prefix += '.'; + } + } + return prefix; +} +} // namespace impl + +struct Parameter +{ + Parameter() = default; + + explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) + : name(name), type(type), configurable(configurable) + { + } + + std::string name; + uint8_t type; + bool configurable; +}; + +class ParameterHandler +{ +public: + ParameterHandler( + const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, + int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, + int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, + int nr_string_array_params = 0); + + virtual ~ParameterHandler() = default; + + void initialize(std::shared_ptr node) + { + initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); + } + + void initialize( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, + const std::string & logger_name = "::ControllerParameters") + { + params_interface_ = node_params; + logger_name_ = logger_name + "." + "parameters"; + } + + void declare_parameters(); + + /** + * Gets all defined parameters from parameter server after parameters are declared. + * Optionally, check parameters' validity and update storage. + * + * \param[in] check_validity check validity after getting parameters (default: **true**). + * \param[in] update update parameters in storage after getting them (default: **true**). + * \return true if all parameters are read successfully, false if a parameter is not provided or + * parameter configuration is wrong. + */ + bool get_parameters(bool check_validity = true, bool update = true); + + /** + * Update all storage variables from the parameter maps. + * Parameters will be only updated if they are previously read from parameter server or dynamic + * reconfigure callback occurred. + * + * \param[in] check_validity check validity before updating parameters (default: **true**). + * \return is update was successful, i.e., parameters are valid. If \p check_validity is set + * **false**, the result is always **true**. + */ + bool update(bool check_validity = true); + + rcl_interfaces::msg::SetParametersResult set_parameter_callback( + const std::vector & parameters); + +protected: + /** + * Override this method to implement custom parameters check. + * Default implementation does not checks anything, always returns true. + * + * \return **true** if parameters are valid, **false** otherwise. + */ + virtual bool check_if_parameters_are_valid() { return true; } + + /** + * Mapping between parameter storage and implementation members. + * The order of parameter in storage is the order of adding parameters to the storage. + * E.g., to access the value of 5-th string parameter added to storage use: + * `fifth_string_param_ = string_parameters_[4].second; + * where `fifth_string_param_` is the name of the member of a child class. + */ + virtual void update_storage() = 0; + +protected: + void add_bool_parameter( + const std::string & name, const bool & configurable = false, const bool & default_value = false) + { + bool_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), + default_value}); + } + + void add_integer_parameter( + const std::string & name, const bool & configurable = false, + const int & default_value = std::numeric_limits::quiet_NaN()) + { + integer_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), + default_value}); + } + + void add_double_parameter( + const std::string & name, const bool & configurable = false, + const double & default_value = std::numeric_limits::quiet_NaN()) + { + double_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), + default_value}); + } + + void add_string_parameter( + const std::string & name, const bool & configurable = false, + const std::string & default_value = "") + { + string_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), + default_value}); + } + + void add_byte_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + byte_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), + default_value}); + } + + void add_bool_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + bool_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), + default_value}); + } + + void add_integer_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + integer_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), + default_value}); + } + void add_double_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + double_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), + default_value}); + } + + void add_string_array_parameter( + const std::string & name, const bool & configurable = false, + const std::vector & default_value = {}) + { + string_array_parameters_.push_back( + {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), + default_value}); + } + + template + bool empty_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (parameter.second.empty()) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + return empty_numeric_parameter_in_list(parameters); + } + + bool empty_parameter_in_list(const std::vector> & parameters) + { + return empty_numeric_parameter_in_list(parameters); + } + + template + bool find_and_assign_parameter_value( + std::vector> & parameter_list, + const rclcpp::Parameter & input_parameter) + { + auto is_in_list = [&](const auto & parameter) { + return parameter.first.name == input_parameter.get_name(); + }; + + auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); + + if (it != parameter_list.end()) + { + if (!it->first.configurable) + { + throw std::runtime_error( + "Parameter " + input_parameter.get_name() + " is not configurable."); + } + else + { + it->second = input_parameter.get_value(); + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), + input_parameter.value_to_string().c_str()); + return true; + } + } + else + { + return false; + } + } + + // Storage members + std::vector> bool_parameters_; + std::vector> integer_parameters_; + std::vector> double_parameters_; + std::vector> string_parameters_; + std::vector>> byte_array_parameters_; + std::vector>> bool_array_parameters_; + std::vector>> integer_array_parameters_; + std::vector>> double_array_parameters_; + std::vector>> string_array_parameters_; + + // Functional members + bool declared_; + bool up_to_date_; + std::string params_prefix_; + + // Input/Output members to ROS 2 + std::string logger_name_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; + +private: + template + void declare_parameters_from_list( + rclcpp::Node::SharedPtr node, const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + node->declare_parameter(parameter.first.name, parameter.second); + } + } + + template + void declare_parameters_from_list(const std::vector> & parameters) + { + for (const auto & parameter : parameters) + { + // declare parameter only if it does not already exists + if (!params_interface_->has_parameter(parameter.first.name)) + { + rclcpp::ParameterValue default_parameter_value(parameter.second); + rcl_interfaces::msg::ParameterDescriptor descr; + descr.name = parameter.first.name; + descr.type = parameter.first.type; + descr.read_only = false; + + params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); + } + } + } + + template + bool empty_numeric_parameter_in_list(const std::vector> & parameters) + { + bool ret = false; + for (const auto & parameter : parameters) + { + if (std::isnan(parameter.second)) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); + ret = true; + } + } + return ret; + } + + template + bool get_parameters_from_list( + const rclcpp::Node::SharedPtr node, std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = node->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } + + template + bool get_parameters_from_list(std::vector> & parameters) + { + bool ret = true; + for (auto & parameter : parameters) + { + try + { + rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? + ret = params_interface_->get_parameter(parameter.first.name, input_parameter); + parameter.second = input_parameter.get_value(); + } + catch (rclcpp::ParameterTypeException & e) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); + ret = false; + break; + } + } + return ret; + } +}; + +} // namespace control_toolbox + +#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/admittance_controller/include/admittance_controller/visibility_control.h b/admittance_controller/include/admittance_controller/visibility_control.h new file mode 100644 index 0000000000..24f17a5c2c --- /dev/null +++ b/admittance_controller/include/admittance_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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 ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define ADMITTANCE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define ADMITTANCE_CONTROLLER_EXPORT __declspec(dllexport) +#define ADMITTANCE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef ADMITTANCE_CONTROLLER_BUILDING_DLL +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_EXPORT +#else +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_IMPORT +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#else +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define ADMITTANCE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml new file mode 100644 index 0000000000..85adb21b1a --- /dev/null +++ b/admittance_controller/package.xml @@ -0,0 +1,44 @@ + + + + admittance_controller + 0.0.0 + Implementation of admittance controllers for different input and output interface. + Denis Štogl + Andy Zelenak + Apache License 2.0 + + ament_cmake + + backward_ros + control_msgs + control_toolbox + controller_interface + kinematics_interface + filters + geometry_msgs + hardware_interface + joint_limits + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + trajectory_msgs + + + + ament_cmake_gmock + control_msgs + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp new file mode 100644 index 0000000000..55a1a549dc --- /dev/null +++ b/admittance_controller/src/admittance_controller.cpp @@ -0,0 +1,523 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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: Denis Stogl, Andy Zelenak, Paul Gesel + +#include "admittance_controller/admittance_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include "admittance_controller/admittance_rule_impl.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "rcutils/logging_macros.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops + +namespace admittance_controller { + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + + AdmittanceController::AdmittanceController() = default; + + void AdmittanceController::wrench_stamped_callback(const std::shared_ptr msg) { + if (controller_is_active_) { + rtBuffers.input_wrench_command_.writeFromNonRT(msg); + } + } + + void + AdmittanceController::joint_command_callback(const std::shared_ptr msg) { + if (controller_is_active_) { + rtBuffers.input_joint_command_.writeFromNonRT(msg); + } + } + + CallbackReturn AdmittanceController::on_init() { +// // set chained mode +// set_chained_mode(true); + // initialize RTbuffers + rtBuffers.input_joint_command_.writeFromNonRT(std::shared_ptr()); + rtBuffers.input_wrench_command_.writeFromNonRT(std::shared_ptr()); + // initialize controller parameters + admittance_ = std::make_unique(); + admittance_->parameters_.initialize(get_node()); + // TODO need error handling + params = { + get_node()->get_parameter("joints").as_string_array(), + get_node()->get_parameter("command_interfaces").as_string_array(), + get_node()->get_parameter("state_interfaces").as_string_array(), + get_node()->get_parameter("chainable_command_interfaces").as_string_array(), + get_node()->get_parameter("ft_sensor_name").as_string(), + get_node()->get_parameter("use_joint_commands_as_input").as_bool(), + get_node()->get_parameter("joint_limiter_type").as_string(), + get_node()->get_parameter("allow_partial_joints_goal").as_bool(), + get_node()->get_parameter("allow_integration_in_goal_trajectories").as_bool(), + get_node()->get_parameter("action_monitor_rate").as_double(), + get_node()->get_parameter("open_loop_control").as_bool(), + }; + + for (const auto &tmp: params.state_interface_types_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto &tmp: params.command_interface_types_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + for (const auto &tmp: params.chainable_command_interface_types_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } + + try { + admittance_->parameters_.declare_parameters(); + } catch (const std::exception &e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + num_joints_ = params.joint_names_.size(); + + + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() const { + // Create an InterfaceConfiguration for the controller manager. The controller manager will then try to + // claim the joint + interface combinations for state interfaces from the resource manager. Finally, + // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. + // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported + + std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); + + for (const auto &interface: params.state_interface_types_) { + for (const auto &joint: params.joint_names_) { + state_interfaces_config_names.push_back(joint + "/" + interface); + } + } + auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); + state_interfaces_config_names.insert(state_interfaces_config_names.end(), ft_interfaces.begin(), + ft_interfaces.end()); + + return {controller_interface::interface_configuration_type::INDIVIDUAL, + state_interfaces_config_names}; + } + + controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() const { + // Create an InterfaceConfiguration for the controller manager. The controller manager will then try to + // claim the joint + interface combinations for command interfaces from the resource manager. Finally, + // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase + // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported + + std::vector command_interfaces_config_names;//(joint_names_.size() * command_interface_types_.size()); + + for (const auto &interface: params.command_interface_types_) { + for (const auto &joint: params.joint_names_) { + command_interfaces_config_names.push_back(joint + "/" + interface); + } + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, + command_interfaces_config_names}; + } + + std::vector AdmittanceController::on_export_reference_interfaces() { + + std::vector chainable_command_interfaces; + auto num_chainable_interfaces = params.chainable_command_interface_types_.size() * params.joint_names_.size(); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + chainable_command_interfaces.reserve(num_chainable_interfaces); + + auto index = 0ul; + for (const auto &interface: params.chainable_command_interface_types_) { + for (const auto &joint: params.joint_names_) { + chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(std::string(get_node()->get_name()), + joint + "/" + interface, + reference_interfaces_.data() + + index)); + } + } + + joint_position_chainable_interface_.clear(); + joint_velocity_chainable_interface_.clear(); + std::unordered_map> *> chainable_interface_map = { + {hardware_interface::HW_IF_POSITION, &joint_position_chainable_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_chainable_interface_} + }; + + for (auto i = 0ul; i < params.chainable_command_interface_types_.size(); i++) { + for (auto j = 0ul; j < params.joint_names_.size(); j++) { + chainable_interface_map[params.chainable_command_interface_types_[i]]->emplace_back( + chainable_command_interfaces[i * num_joints_ + j]); + } + } + + return chainable_command_interfaces; + } + + controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { + // update input reference from ros subscriber message + return controller_interface::return_type::OK; //TODO fix this + + joint_command_msg = *rtBuffers.input_joint_command_.readFromRT(); + for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { + joint_position_chainable_interface_[i].get().set_value(joint_command_msg->positions[i]); + } + for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { + joint_velocity_chainable_interface_[i].get().set_value(joint_command_msg->velocities[i]); + } + for (auto i = 0ul; i < joint_command_msg->accelerations.size(); i++) { + joint_acceleration_chainable_interface_[i].get().set_value(joint_command_msg->accelerations[i]); + } + + return controller_interface::return_type::OK; + } + + bool AdmittanceController::on_set_chained_mode(bool chained_mode) { + // this method set chained mode to value of chained_mode if true is returned + return true; + } + + CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { + // load and set all ROS parameters + if (!admittance_->parameters_.get_parameters()) { + RCLCPP_ERROR(get_node()->get_logger(), "Error happened during reading parameters"); + return CallbackReturn::ERROR; + } + + // Print output so users can be sure the interface setup is correct + auto get_interface_list = [](const std::vector &interface_types) { + std::stringstream ss_command_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) { + if (index != 0) { + ss_command_interfaces << " "; + } + ss_command_interfaces << interface_types[index]; + } + return ss_command_interfaces.str(); + }; + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(params.command_interface_types_).c_str(), + get_interface_list(params.state_interface_types_).c_str()); + + RCLCPP_INFO(get_node()->get_logger(), "Action status changes will be monitored at %.2f Hz.", + params.action_monitor_rate_); + if (params.use_joint_commands_as_input_) { + RCLCPP_INFO(get_node()->get_logger(), "Using Joint input mode."); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Admittance controller does not support non-joint input modes."); + return CallbackReturn::ERROR; + } + + // setup subscribers and publishers + input_wrench_command_subscriber_ = get_node()->create_subscription( + "~/ft_data", rclcpp::SystemDefaultsQoS(), + std::bind(&AdmittanceController::wrench_stamped_callback, this, std::placeholders::_1)); + input_joint_command_subscriber_ = get_node()->create_subscription( + "~/pose_commands", rclcpp::SystemDefaultsQoS(), + std::bind(&AdmittanceController::joint_command_callback, this, std::placeholders::_1)); + s_publisher_ = get_node()->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + rtBuffers.state_publisher_ = std::make_unique>(s_publisher_); + + // Initialize state message + rtBuffers.state_publisher_->lock(); + rtBuffers.state_publisher_->msg_.joint_names = params.joint_names_; + rtBuffers.state_publisher_->msg_.actual_joint_state.positions.resize(num_joints_, 0.0); + rtBuffers.state_publisher_->msg_.desired_joint_state.positions.resize(num_joints_, 0.0); + rtBuffers.state_publisher_->msg_.error_joint_state.positions.resize(num_joints_, 0.0); + rtBuffers.state_publisher_->unlock(); + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(params.ft_sensor_name_)); + + // configure admittance rule + admittance_->configure(get_node(), num_joints_); + // HACK: This is workaround because it seems that updating parameters only in `on_activate` does + // not work properly: why? + admittance_->parameters_.update(); + + return LifecycleNodeInterface::on_configure(previous_state); + } + + + CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { + // on_activate is called when the lifecycle activates. Realtime constraints are required. + controller_is_active_ = true; + joint_position_command_interface_.clear(); + joint_velocity_command_interface_.clear(); + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + // assign state interfaces + std::unordered_map> *> command_interface_map = { + {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} + }; + + for (auto i = 0ul; i < params.command_interface_types_.size(); i++) { + for (auto j = 0ul; j < params.joint_names_.size(); j++) { + command_interface_map[params.command_interface_types_[i]]->emplace_back( + command_interfaces_[i * num_joints_ + j]); + } + } + + std::unordered_map> *> state_interface_map = { + {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} + }; + + for (auto i = 0ul; i < params.state_interface_types_.size(); i++) { + for (auto j = 0ul; j < params.joint_names_.size(); j++) { + state_interface_map[params.state_interface_types_[i]]->emplace_back(state_interfaces_[i * num_joints_ + j]); + } + } + + // Initialize interface of the FTS semantic semantic component + force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + // Initialize Admittance Rule from current states + admittance_->reset(); + + // if in open loop mode, the position state interface should be ignored + if (params.open_loop_control_) { +// state_offset_.positions.resize(joint_position_state_interface_.size(), 0.0); +// for (int i=0; i < num_joints_; i++){ +// state_offset_.positions[i] = joint_position_state_interface_[i].get().get_value(); +// } +// open_loop_buffer.resize(joint_position_state_interface_.size(), 0.0); + + open_loop_buffer.resize(joint_position_state_interface_.size(), 0.0); + for (int i = 0; i < num_joints_; i++) { + open_loop_buffer[i] = joint_position_state_interface_[i].get().get_value(); + } + } + + // Handle state after restart or initial startup + read_state_from_hardware(last_state_reference_); + // if last_state_reference_ is empty, we have no information about the state, assume zero + if (last_state_reference_.positions.empty()) last_state_reference_.positions.resize(num_joints_, 0.0); + read_state_from_command_interfaces(last_commanded_state_); + // if last_commanded_state_ is empty, then our safest option is to set it to the last reference + if (last_commanded_state_.positions.empty()) last_commanded_state_.positions = last_state_reference_.positions; + + // reset dynamic fields in case non-zero + if (last_state_reference_.velocities.empty()) last_state_reference_.velocities.assign(num_joints_, 0.0); + if (last_state_reference_.accelerations.empty()) last_state_reference_.accelerations.assign(num_joints_, 0.0); + if (last_commanded_state_.velocities.empty()) last_commanded_state_.velocities.assign(num_joints_, 0.0); + if (last_commanded_state_.accelerations.empty()) last_commanded_state_.accelerations.assign(num_joints_, 0.0); + // initialize state reference + state_reference_ = last_state_reference_; + + // if there are no state position interfaces, then force use of open loop control + if (joint_position_state_interface_.empty() && !params.open_loop_control_) { + params.open_loop_control_ = true; + RCLCPP_INFO(get_node()->get_logger(), + "control loop control set to true because no position state interface was provided."); + } + + return CallbackReturn::SUCCESS;; + } + + controller_interface::return_type + AdmittanceController::update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) { + // Realtime constraints are required in this function + + // update input reference from chainable interfaces + read_state_reference_from_chainable_interfaces(state_reference_); + + // sense: get all controller inputs + geometry_msgs::msg::Wrench ft_values; + force_torque_sensor_->get_values_as_message(ft_values); + read_state_from_hardware(state_current_); + + // if values are not available, assume that the current state is the last commanded + if (state_current_.positions.empty()) { state_current_.positions = last_commanded_state_.positions; } + if (state_current_.velocities.empty()) { state_current_.velocities = last_commanded_state_.velocities; } + if (state_current_.accelerations.empty()) { state_current_.accelerations = last_commanded_state_.accelerations; } + + + // command: determine desired state from trajectory or pose goal + // and apply admittance controller + + admittance_->update(state_current_, ft_values, state_reference_, period, state_desired_); + + // write calculated values to joint interfaces + // at goal time (end of trajectory), check goal reference error and send fail to + // action server out of tolerance + for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { + joint_position_command_interface_[i].get().set_value(state_desired_.positions[i]); + if(params.open_loop_control_){ + open_loop_buffer[i] = state_desired_.positions[i]; + } + last_commanded_state_.positions[i] = state_desired_.positions[i]; + } + for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { + double dt = 1.0 / 60; // hack! + joint_velocity_command_interface_[i].get().set_value(state_desired_.velocities[i]); + last_commanded_state_.velocities[i] = state_desired_.velocities[i]; + last_commanded_state_.positions[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! + if(params.open_loop_control_){ + open_loop_buffer[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! + } + } + for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { + joint_acceleration_command_interface_[i].get().set_value(state_desired_.accelerations[i]); + last_commanded_state_.accelerations[i] = state_desired_.accelerations[i]; + } + + // save state reference before applying admittance rule TODO why is this here? + //pre_admittance_point.points[0] = state_reference_; TODO fix this + // Publish controller state + rtBuffers.state_publisher_->lock(); + rtBuffers.state_publisher_->msg_.input_joint_command = pre_admittance_point; + rtBuffers.state_publisher_->msg_.desired_joint_state = state_desired_; + rtBuffers.state_publisher_->msg_.actual_joint_state = state_current_; + rtBuffers.state_publisher_->msg_.error_joint_state = state_error_; + admittance_->get_controller_state(rtBuffers.state_publisher_->msg_); + rtBuffers.state_publisher_->unlockAndPublish(); + + return controller_interface::return_type::OK; + } + + CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { + controller_is_active_ = false; + force_torque_sensor_->release_interfaces(); + + return LifecycleNodeInterface::on_deactivate(previous_state); + } + + CallbackReturn AdmittanceController::on_cleanup(const rclcpp_lifecycle::State &previous_state) { + + return CallbackReturn::SUCCESS; + } + + CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { + admittance_->reset(); + return CallbackReturn::SUCCESS; + } + + void AdmittanceController::read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint &state_current) { + // Fill fields of state argument from hardware state interfaces. If the hardware does not exist, + // the values are nan, that corresponding state field will be set to empty. If running in open loop + // control, all states fields will be empty + + state_current.positions.resize(joint_position_state_interface_.size()); + state_current.velocities.resize(joint_velocity_state_interface_.size()); + state_current.accelerations.resize(joint_acceleration_state_interface_.size()); + + // fill state message with values from hardware state interfaces + for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { + if (params.open_loop_control_) { + state_current.positions[i] = open_loop_buffer[i]; + } else { + state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); + } + if (std::isnan(state_current.positions[i])) { + state_current.positions.clear(); + break; + } + } + for (auto i = 0ul; i < joint_velocity_state_interface_.size(); i++) { + state_current.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); + if (std::isnan(state_current.velocities[i])) { + state_current.velocities.clear(); + break; + } + } + for (auto i = 0ul; i < joint_acceleration_state_interface_.size(); i++) { + state_current.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); + if (std::isnan(state_current.accelerations[i])) { + state_current.accelerations.clear(); + break; + } + } + } + + void AdmittanceController::read_state_from_command_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint &commanded_state) { + // Fill fields of state argument from hardware command interfaces. If the interface does not exist or + // the values are nan, that corresponding state field will be set to empty + + commanded_state.positions.resize(joint_position_command_interface_.size(), 0.0); + commanded_state.velocities.resize(joint_velocity_command_interface_.size(), 0.0); + commanded_state.accelerations.resize(joint_acceleration_command_interface_.size(), 0.0); + + // fill state message with values from hardware command interfaces + for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { + commanded_state.positions[i] = joint_position_command_interface_[i].get().get_value(); + if (std::isnan(commanded_state.positions[i])) { + commanded_state.positions.clear(); + break; + } + } + for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { + commanded_state.velocities[i] = joint_velocity_command_interface_[i].get().get_value(); + if (std::isnan(commanded_state.velocities[i])) { + commanded_state.velocities.clear(); + break; + } + } + for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { + commanded_state.accelerations[i] = joint_acceleration_command_interface_[i].get().get_value(); + if (std::isnan(commanded_state.accelerations[i])) { + commanded_state.accelerations.clear(); + break; + } + } + } + + void AdmittanceController::read_state_reference_from_chainable_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { + // Fill fields of state argument from hardware command interfaces. If the interface does not exist or + // the values are nan, that corresponding state field will be set to empty + + state_reference.positions.resize(joint_position_chainable_interface_.size(), 0.0); + state_reference.velocities.resize(joint_velocity_chainable_interface_.size(), 0.0); + state_reference.accelerations.resize(joint_acceleration_chainable_interface_.size(), 0.0); + + for (auto i = 0ul; i < joint_position_chainable_interface_.size(); i++) { + state_reference.positions[i] = joint_position_chainable_interface_[i].get().get_value(); + if (std::isnan(state_reference.positions[i])) { + state_reference.positions = last_state_reference_.positions; + break; + } + } + for (auto i = 0ul; i < joint_velocity_chainable_interface_.size(); i++) { + state_reference.velocities[i] = joint_velocity_chainable_interface_[i].get().get_value(); + if (std::isnan(state_reference.velocities[i])) { + state_reference.velocities = last_state_reference_.velocities; + break; + } + } + for (auto i = 0ul; i < joint_acceleration_chainable_interface_.size(); i++) { + state_reference.accelerations[i] = joint_acceleration_chainable_interface_[i].get().get_value(); + if (std::isnan(state_reference.accelerations[i])) { + state_reference.accelerations = last_state_reference_.accelerations; + break; + } + } + + } + +} // namespace admittance_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(admittance_controller::AdmittanceController, + controller_interface::ChainableControllerInterface) diff --git a/admittance_controller/src/parameter_handler.cpp b/admittance_controller/src/parameter_handler.cpp new file mode 100644 index 0000000000..e2eedf1276 --- /dev/null +++ b/admittance_controller/src/parameter_handler.cpp @@ -0,0 +1,216 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. +// +/// \author: Denis Stogl + +#include "admittance_controller/parameter_handler.hpp" + +#include +#include + +#include "rcutils/logging_macros.h" + +namespace control_toolbox +{ +ParameterHandler::ParameterHandler( + const std::string & params_prefix, int nr_bool_params, int nr_integer_params, + int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, + int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) +: declared_(false), up_to_date_(false), params_prefix_("") +{ + params_prefix_ = impl::normalize_params_prefix(params_prefix); + + bool_parameters_.reserve(nr_bool_params); + integer_parameters_.reserve(nr_integer_params); + double_parameters_.reserve(nr_double_params); + string_parameters_.reserve(nr_string_params); + byte_array_parameters_.reserve(nr_byte_array_params); + bool_array_parameters_.reserve(nr_bool_array_params); + integer_array_parameters_.reserve(nr_integer_array_params); + double_array_parameters_.reserve(nr_double_array_params); + string_array_parameters_.reserve(nr_string_array_params); +} + +void ParameterHandler::declare_parameters() +{ + if (!declared_) + { + declare_parameters_from_list(bool_parameters_); + declare_parameters_from_list(integer_parameters_); + declare_parameters_from_list(double_parameters_); + declare_parameters_from_list(string_parameters_); + declare_parameters_from_list(byte_array_parameters_); + declare_parameters_from_list(bool_array_parameters_); + declare_parameters_from_list(integer_array_parameters_); + declare_parameters_from_list(double_array_parameters_); + declare_parameters_from_list(string_array_parameters_); + + declared_ = true; + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), + "Parameters already declared. Declaration should be done only once. " + "Nothing bad will happen, but please correct your code-logic."); + } +} + +bool ParameterHandler::get_parameters(bool check_validity, bool update) +{ + // TODO(destogl): Should we "auto-declare" parameters here? + if (!declared_) + { + RCUTILS_LOG_ERROR_NAMED( + logger_name_.c_str(), "Can not get parameters. Please declare them first."); + return false; + } + + bool ret = false; + + // If parameters are updated using dynamic reconfigure callback then there is no need to read + // them again. To ignore multiple manual reads + ret = + get_parameters_from_list(bool_parameters_) && get_parameters_from_list(integer_parameters_) && + get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && + get_parameters_from_list(byte_array_parameters_) && + get_parameters_from_list(bool_array_parameters_) && + get_parameters_from_list(integer_array_parameters_) && + get_parameters_from_list(double_array_parameters_) && + get_parameters_from_list(string_array_parameters_); + + if (ret && check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + // If it is all good until now the parameters are not up to date anymore + if (ret) + { + up_to_date_ = false; + } + + if (ret && update) + { + ret = this->update(false); + } + + return ret; +} + +bool ParameterHandler::update(bool check_validity) +{ + bool ret = true; + + // Let's make this efficient and execute code only if parameters are updated + if (!up_to_date_) + { + if (check_validity) + { + ret = this->check_if_parameters_are_valid(); + } + + if (ret) + { + this->update_storage(); + } + else + { + RCUTILS_LOG_WARN_NAMED( + logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); + } + // reset variable to update parameters only when this is needed + up_to_date_ = true; + } + + return ret; +} + +rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. + for (const auto & input_parameter : parameters) + { + bool found = false; + + try + { + found = find_and_assign_parameter_value(bool_parameters_, input_parameter); + if (!found) + { + found = find_and_assign_parameter_value(integer_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(double_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(integer_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); + } + if (!found) + { + found = find_and_assign_parameter_value(string_array_parameters_, input_parameter); + } + + RCUTILS_LOG_INFO_EXPRESSION_NAMED( + found, logger_name_.c_str(), + "Dynamic parameters got changed! Maybe you have to restart controller to update the " + "parameters internally."); + + if (found) + { + up_to_date_ = false; + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + catch (const std::runtime_error & e) + { + result.successful = false; + result.reason = e.what(); + RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); + break; + } + } + + return result; +} + +} // namespace control_toolbox diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp new file mode 100644 index 0000000000..36999d764e --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -0,0 +1,446 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. +// +/// \author: Denis Stogl + +#include "test_admittance_controller.hpp" + +#include +#include +#include +#include + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_missing) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// TODO(anyone): the new gtest version afer 1.8.0 uses INSTANTIATE_TEST_SUITE_P +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedParameters, + ::testing::Values( + std::make_tuple( + std::string("joints"), + rclcpp::ParameterValue(std::vector()) + ), + std::make_tuple( + std::string("command_interfaces"), + rclcpp::ParameterValue(std::vector()) + ), + std::make_tuple( + std::string("state_interfaces"), + rclcpp::ParameterValue(std::vector()) + ), + std::make_tuple( + std::string("ft_sensor_name"), + rclcpp::ParameterValue("") + ), +// std::make_tuple( +// std::string("use_joint_commands_as_input"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("hardware_state_has_offset"), +// rclcpp::ParameterValue(false) +// ), + std::make_tuple( + std::string("IK.base"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("IK.group_name"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("control_frame"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("sensor_frame"), + rclcpp::ParameterValue("") + ), + // TODO(anyone): this tests are unstable... +// std::make_tuple( +// std::string("admittance.selected_axes.x"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.y"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.z"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.rx"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.ry"), +// rclcpp::ParameterValue(false) +// ), +// std::make_tuple( +// std::string("admittance.selected_axes.rz"), +// rclcpp::ParameterValue(false) +// ), + std::make_tuple( + std::string("admittance.mass.x"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.y"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.z"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.rx"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.ry"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.mass.rz"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.x"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.y"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.z"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.rx"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.ry"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.damping.rz"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.x"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.y"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.z"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.rx"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.ry"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ), + std::make_tuple( + std::string("admittance.stiffness.rz"), + rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + ) + ) +); + +TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->command_interface_types_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->joint_names_.size() == joint_names_.size()); + ASSERT_TRUE(std::equal(controller_->joint_names_.begin(), controller_->joint_names_.end(), + joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(!controller_->command_interface_types_.empty()); + ASSERT_TRUE(controller_->command_interface_types_.size() == command_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->command_interface_types_.begin(), controller_->command_interface_types_.end(), + command_interface_types_.begin(), command_interface_types_.end())); + + ASSERT_TRUE(!controller_->state_interface_types_.empty()); + ASSERT_TRUE(controller_->state_interface_types_.size() == state_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->state_interface_types_.begin(), controller_->state_interface_types_.end(), + state_interface_types_.begin(), state_interface_types_.end())); + + ASSERT_EQ(controller_->ft_sensor_name_, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->parameters_.ik_base_frame_, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ik_group_name_, ik_group_name_); + ASSERT_EQ(controller_->admittance_->parameters_.sensor_frame_, sensor_frame_); + + ASSERT_TRUE(!controller_->admittance_->parameters_.selected_axes_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.selected_axes_.size() == admittance_selected_axes_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.selected_axes_.begin(), controller_->admittance_->parameters_.selected_axes_.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.mass_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.mass_.size() == admittance_mass_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.mass_.begin(), controller_->admittance_->parameters_.mass_.end(), + admittance_mass_.begin(), admittance_mass_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.damping_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.damping_.size() == admittance_damping_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.damping_.begin(), controller_->admittance_->parameters_.damping_.end(), + admittance_damping_.begin(), admittance_damping_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.stiffness_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.stiffness_.size() == admittance_stiffness_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.stiffness_.begin(), controller_->admittance_->parameters_.stiffness_.end(), + admittance_stiffness_.begin(), admittance_stiffness_.end())); +} + +TEST_F(AdmittanceControllerTest, check_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + + ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + + ASSERT_EQ(controller_->state_interfaces_.size(), state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); +} + + +TEST_F(AdmittanceControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check ordered interfaces + ASSERT_TRUE(!controller_->joint_command_interface_.empty()); + for (const auto & interface : command_interface_types_) { + auto it = std::find( + controller_->allowed_interface_types_.begin(), controller_->allowed_interface_types_.end(), interface); + auto index = std::distance(controller_->allowed_interface_types_.begin(), it); + + ASSERT_TRUE(controller_->joint_command_interface_[index].size() == joint_names_.size()); + } + + ASSERT_TRUE(!controller_->joint_state_interface_.empty()); + for (const auto & interface : state_interface_types_) { + auto it = std::find( + controller_->allowed_interface_types_.begin(), controller_->allowed_interface_types_.end(), interface); + auto index = std::distance(controller_->allowed_interface_types_.begin(), it); + + ASSERT_TRUE(controller_->joint_state_interface_[index].size() == joint_names_.size()); + } +} + +TEST_F(AdmittanceControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + assign_interfaces(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, publish_status_success) +{ + // TODO: Write also a test when Cartesian commands are used. + SetUpController(true, true); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // Check that wrench command are all zero since not used + ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_wrench_command.wrench.force.x, 0.0); + ASSERT_EQ(msg.input_wrench_command.wrench.force.y, 0.0); + ASSERT_EQ(msg.input_wrench_command.wrench.force.z, 0.0); + ASSERT_EQ(msg.input_wrench_command.wrench.torque.x, 0.0); + ASSERT_EQ(msg.input_wrench_command.wrench.torque.y, 0.0); + ASSERT_EQ(msg.input_wrench_command.wrench.torque.z, 0.0); + + // Check Cartesian command message + ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.x)); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.y)); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.z)); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.x)); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.y)); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.z)); + ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.w)); + + // Check joint command message + ASSERT_TRUE(std::equal( + msg.input_joint_command.joint_names.begin(), msg.input_joint_command.joint_names.end(), + joint_names_.begin(), joint_names_.end())); + ASSERT_EQ(msg.input_joint_command.points.size(), 1u); + ASSERT_TRUE(std::equal(msg.input_joint_command.points[0].positions.begin(), msg.input_joint_command.points[0].positions.end(), joint_state_values_.begin(), joint_state_values_.end())); + + ASSERT_TRUE(std::find_if_not(msg.input_joint_command.points[0].velocities.begin(), msg.input_joint_command.points[0].velocities.end(), + [](const auto & value){ return value == 0.0;}) == msg.input_joint_command.points[0].velocities.end()); + + // Check messages filled from AdmittanceRule.cpp + ASSERT_EQ(msg.measured_wrench.header.frame_id, sensor_frame_); + ASSERT_EQ(msg.measured_wrench.wrench.force.x, fts_state_values_[0]); + ASSERT_EQ(msg.measured_wrench.wrench.force.y, fts_state_values_[1]); + ASSERT_EQ(msg.measured_wrench.wrench.force.z, fts_state_values_[2]); + ASSERT_EQ(msg.measured_wrench.wrench.torque.x, fts_state_values_[3]); + ASSERT_EQ(msg.measured_wrench.wrench.torque.y, fts_state_values_[4]); + ASSERT_EQ(msg.measured_wrench.wrench.torque.z, fts_state_values_[5]); + + ASSERT_EQ(msg.measured_wrench_control_frame.header.frame_id, control_frame_); + ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.x, 0.0); + ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.y, 0.0); + ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.z, 0.0); + ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.x, 0.0); + ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.y, 0.0); + ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.z, 0.0); + + ASSERT_EQ(msg.desired_pose.header.frame_id, control_frame_); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.x)); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.y)); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.z)); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.x)); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.y)); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.z)); + ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.w)); + + ASSERT_EQ(msg.relative_desired_pose.header.frame_id, control_frame_); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.x)); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.y)); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.z)); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.x)); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.y)); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.z)); + ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.w)); + + // Check joint related messages + ASSERT_TRUE(std::equal( + msg.joint_names.begin(), msg.joint_names.end(), joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(std::equal( + msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), + joint_state_values_.begin(), joint_state_values_.end())); + + ASSERT_TRUE(std::equal( + msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), + joint_state_values_.begin(), joint_state_values_.end())); + + ASSERT_TRUE(std::equal( + msg.desired_joint_state.positions.begin(), msg.desired_joint_state.positions.end(), + joint_state_values_.begin(), joint_state_values_.end())); + + ASSERT_TRUE(std::find_if_not(msg.error_joint_state.positions.begin(), msg.error_joint_state.positions.end(), + [](const auto & value){ return value == 0.0;}) == msg.error_joint_state.positions.end()); +} + +TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(true, true); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + // After first update state values should be written to command values + ASSERT_TRUE(std::equal(joint_state_values_.begin(), joint_state_values_.end(), joint_command_values_.begin(), joint_command_values_.end())); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + broadcast_tfs(); + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + EXPECT_NEAR(joint_command_values_[0], 0.0, COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); +} + + +// Add test, wrong interfaces diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp new file mode 100644 index 0000000000..6476956d91 --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -0,0 +1,449 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. +// +/// \author: Denis Stogl + +#ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ +#define TEST_ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "admittance_controller/admittance_controller.hpp" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "ros2_control_test_assets/6d_robot_description.hpp" +#include "semantic_components/force_torque_sensor.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "tf2_ros/transform_broadcaster.h" + +// TODO(anyone): replace the state and command message types +using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory; +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +namespace +{ +const double COMMON_THRESHOLD = 0.001; + +constexpr auto NODE_SUCCESS = +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} + +} // namespace + +// subclassing and friending so we can access member variables +class TestableAdmittanceController +: public admittance_controller::AdmittanceController +{ + FRIEND_TEST(AdmittanceControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, interface_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AdmittanceControllerTest, check_interfaces); + FRIEND_TEST(AdmittanceControllerTest, activate_success); + FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); + +public: + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + admittance_controller::AdmittanceController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) { + if (admittance_->unified_mode_) { + input_wrench_command_subscriber_wait_set_.add_subscription(input_wrench_command_subscriber_); + } + input_pose_command_subscriber_wait_set_.add_subscription(input_pose_command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds {500}) + { + bool success = input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + + if (admittance_->unified_mode_) { + success = success && input_wrench_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + } + if (success) { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; + rclcpp::WaitSet input_pose_command_subscriber_wait_set_; +}; + +class AdmittanceControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + force_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/force_commands", rclcpp::SystemDefaultsQoS()); + pose_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + joint_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/joint_commands", rclcpp::SystemDefaultsQoS()); + + test_subscription_node_ = std::make_shared("test_subscription_node"); + test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDown() + { + controller_.reset(nullptr); + } + +protected: + void SetUpController(bool set_parameters = true, bool use_joint_commands_as_input = false) + { + const auto result = controller_->init("test_admittance_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + assign_interfaces(); + + if (set_parameters) { + controller_->get_node()->set_parameter({"use_joint_commands_as_input", use_joint_commands_as_input}); + + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"command_interfaces", command_interface_types_}); + controller_->get_node()->set_parameter({"state_interfaces", state_interface_types_}); + controller_->get_node()->set_parameter({"ft_sensor_name", ft_sensor_name_}); + controller_->get_node()->set_parameter({"hardware_state_has_offset", hardware_state_has_offset_}); + + controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); + controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); + // TODO(destogl): enable when IK support is added +// controller_->get_node()->set_parameter({"IK.plugin", ik_group_name_}); + controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_}); + controller_->get_node()->set_parameter({"robot_description", robot_description_}); + controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + + controller_->get_node()->set_parameter({"control_frame", control_frame_}); + controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_}); + controller_->get_node()->set_parameter({"fixed_world_frame", fixed_world_frame_}); + controller_->get_node()->set_parameter({"sensor_frame", sensor_frame_}); + + controller_->get_node()->set_parameter({"admittance.selected_axes.x", admittance_selected_axes_[0]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.y", admittance_selected_axes_[1]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.z", admittance_selected_axes_[2]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.rx", admittance_selected_axes_[3]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.ry", admittance_selected_axes_[4]}); + controller_->get_node()->set_parameter({"admittance.selected_axes.rz", admittance_selected_axes_[5]}); + + controller_->get_node()->set_parameter({"admittance.mass.x", admittance_mass_[0]}); + controller_->get_node()->set_parameter({"admittance.mass.y", admittance_mass_[1]}); + controller_->get_node()->set_parameter({"admittance.mass.z", admittance_mass_[2]}); + controller_->get_node()->set_parameter({"admittance.mass.rx", admittance_mass_[3]}); + controller_->get_node()->set_parameter({"admittance.mass.ry", admittance_mass_[4]}); + controller_->get_node()->set_parameter({"admittance.mass.rz", admittance_mass_[5]}); + + controller_->get_node()->set_parameter({"admittance.damping.x", admittance_damping_[0]}); + controller_->get_node()->set_parameter({"admittance.damping.y", admittance_damping_[1]}); + controller_->get_node()->set_parameter({"admittance.damping.z", admittance_damping_[2]}); + controller_->get_node()->set_parameter({"admittance.damping.rx", admittance_damping_[3]}); + controller_->get_node()->set_parameter({"admittance.damping.ry", admittance_damping_[4]}); + controller_->get_node()->set_parameter({"admittance.damping.rz", admittance_damping_[5]}); + + controller_->get_node()->set_parameter({"admittance.stiffness.x", admittance_stiffness_[0]}); + controller_->get_node()->set_parameter({"admittance.stiffness.y", admittance_stiffness_[1]}); + controller_->get_node()->set_parameter({"admittance.stiffness.z", admittance_stiffness_[2]}); + controller_->get_node()->set_parameter({"admittance.stiffness.rx", admittance_stiffness_[3]}); + controller_->get_node()->set_parameter({"admittance.stiffness.ry", admittance_stiffness_[4]}); + controller_->get_node()->set_parameter({"admittance.stiffness.rz", admittance_stiffness_[5]}); + + } + } + + void assign_interfaces() + { + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) { + command_itfs_.emplace_back( + hardware_interface::CommandInterface(joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_); + fts_state_names_ = sc_fts.get_state_interface_names(); + std::vector state_ifs; + + const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); + state_itfs_.reserve(num_state_ifs); + state_ifs.reserve(num_state_ifs); + + for (auto i = 0u; i < joint_state_values_.size(); ++i) { + state_itfs_.emplace_back( + hardware_interface::StateInterface(joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector fts_itf_names = {"force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"}; + + for (auto i = 0u; i < fts_state_names_.size(); ++i) { + state_itfs_.emplace_back( + hardware_interface::StateInterface(ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void broadcast_tfs() + { + static tf2_ros::TransformBroadcaster br(test_broadcaster_node_); + geometry_msgs::msg::TransformStamped transform_stamped; + + transform_stamped.header.stamp = test_broadcaster_node_->now(); + transform_stamped.header.frame_id = fixed_world_frame_; + transform_stamped.transform.translation.x = 1.3; + transform_stamped.transform.translation.y = 0.5; + transform_stamped.transform.translation.z = 0.5; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_tip_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.header.frame_id = ik_tip_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 0; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = control_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.05; + transform_stamped.child_frame_id = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.2; + transform_stamped.child_frame_id = endeffector_frame_; + br.sendTransform(transform_stamped); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) + { + }; + auto subscription = + test_subscription_node_->create_subscription( + "/test_admittance_controller/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + // wait for message to be passed + ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands() + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) { + if (wait_count >= 5) { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + // TODO(destogl): comment in when using unified mode +// if (controller_->admittance_->unified_mode_) { +// wait_for_topic(force_command_publisher_->get_topic_name()); +// } + + wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandWrenchMsg force_msg; + force_msg.header.frame_id = sensor_frame_; + force_msg.wrench.force.x = 0.1; + force_msg.wrench.force.y = 0.2; + force_msg.wrench.force.z = 0.3; + force_msg.wrench.torque.x = 3.1; + force_msg.wrench.torque.y = 3.2; + force_msg.wrench.torque.z = 3.3; + + ControllerCommandPoseMsg pose_msg; + pose_msg.header.frame_id = endeffector_frame_; + pose_msg.pose.position.x = 0.1; + pose_msg.pose.position.y = 0.2; + pose_msg.pose.position.z = 0.3; + pose_msg.pose.orientation.x = 0; + pose_msg.pose.orientation.y = 0; + pose_msg.pose.orientation.z = 0; + pose_msg.pose.orientation.w = 1; + + // TODO(destogl): comment in when using unified mode +// if (controller_->admittance_->unified_mode_) { +// force_command_publisher_->publish(force_msg); +// } + pose_command_publisher_->publish(pose_msg); + + wait_for_topic(joint_command_publisher_->get_topic_name()); + + ControllerCommandJointMsg joint_msg; + joint_msg.joint_names = joint_names_; + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; + auto num_joints = joint_names_.size(); + trajectory_point.positions.reserve(num_joints); + trajectory_point.velocities.resize(num_joints, 0.0); + for (auto index = 0u; index < num_joints; ++index) { + trajectory_point.positions.emplace_back(joint_state_values_[index]); + } + joint_msg.points.emplace_back(trajectory_point); + + joint_command_publisher_->publish(joint_msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + const std::vector command_interface_types_ = {"position"}; + const std::vector state_interface_types_ = {"position"}; + const std::string ft_sensor_name_ = "ft_sensor_name"; + + bool hardware_state_has_offset_ = false; + + const std::string ik_base_frame_ = "base_link"; + const std::string ik_tip_frame_ = "tool0"; + const std::string ik_group_name_ = "arm"; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + + const std::string control_frame_ = "control_frame"; + const std::string endeffector_frame_ = "endeffector_frame"; + const std::string fixed_world_frame_ = "fixed_world_frame"; + const std::string sensor_frame_ = "sensor_frame"; + + std::array admittance_selected_axes_ = {true, true, true, true, true, true}; + std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array admittance_damping_ = {100.1, 100.2, 100.3, 100.4, 100.5, 100.6}; + std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; + + std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector fts_state_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr force_command_publisher_; + rclcpp::Publisher::SharedPtr pose_command_publisher_; + rclcpp::Publisher::SharedPtr joint_command_publisher_; + rclcpp::Node::SharedPtr test_subscription_node_; + rclcpp::Node::SharedPtr test_broadcaster_node_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedParameters +: public AdmittanceControllerTest, +public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() + { + AdmittanceControllerTest::SetUp(); + } + + static void TearDownTestCase() + { + AdmittanceControllerTest::TearDownTestCase(); + } + +protected: + void SetUpController(bool set_parameters = true) + { + AdmittanceControllerTest::SetUpController(set_parameters); + controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); + controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); + } + +}; + +#endif // TEST_ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp new file mode 100644 index 0000000000..acbb3365a5 --- /dev/null +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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 "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 "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAdmittanceController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm(std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller( + "test_admittance_controller", + "admittance_controller/AdmittanceController")); +} From 4f3d709fe28edf51a6ee022fbef3c65a2737420f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 19 Jun 2022 18:37:35 -0600 Subject: [PATCH 013/111] merged additional from fwd chainable controllers --- .../forward_command_controller.hpp | 126 +++++++++--------- ...i_interface_forward_command_controller.hpp | 122 ++++++++--------- .../src/forward_command_controller.cpp | 58 +------- ...i_interface_forward_command_controller.cpp | 54 +------- 4 files changed, 135 insertions(+), 225 deletions(-) diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index d799be9245..46c0a00ba6 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -37,79 +37,79 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -template < - typename T, - typename std::enable_if< - std::is_convertible::value, - T>::type * = nullptr, - typename std::enable_if< - std::is_convertible::value, T>::type * = - nullptr> -class BaseForwardCommandController : public T -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - BaseForwardCommandController() : T() {} - -protected: - void declare_parameters() override - { - controller_interface::ControllerInterfaceBase::auto_declare>( - "joints", std::vector()); - controller_interface::ControllerInterfaceBase::auto_declare("interface_name", ""); - }; - - controller_interface::CallbackReturn read_parameters() override + template < + typename T, + typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr, + typename std::enable_if< + std::is_convertible::value, T>::type * = + nullptr> + class BaseForwardCommandController : public T { - joint_names_ = T::get_node()->get_parameter("joints").as_string_array(); + public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + BaseForwardCommandController() : T() {} - if (joint_names_.empty()) + protected: + void declare_parameters() override { - RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } + controller_interface::ControllerInterfaceBase::auto_declare>( + "joints", std::vector()); + controller_interface::ControllerInterfaceBase::auto_declare("interface_name", ""); + }; - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) + controller_interface::CallbackReturn read_parameters() override { - interface_name_ = T::get_node()->get_parameter("interface_name").as_string(); - } + joint_names_ = T::get_node()->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + // Specialized, child controllers set interfaces before calling configure function. + if (interface_name_.empty()) + { + interface_name_ = T::get_node()->get_parameter("interface_name").as_string(); + } + + if (interface_name_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : joint_names_) + { + T::command_interface_names_.push_back(joint + "/" + interface_name_); + } + + return controller_interface::CallbackReturn::SUCCESS; + }; + + std::vector joint_names_; + std::string interface_name_; + }; - if (interface_name_.empty()) - { - RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } + class ForwardCommandController : public BaseForwardCommandController + { + public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardCommandController() : BaseForwardCommandController() {} + }; - for (const auto & joint : joint_names_) + class ChainableForwardCommandController + : public BaseForwardCommandController + { + public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableForwardCommandController() : BaseForwardCommandController() { - T::command_interface_names_.push_back(joint + "/" + interface_name_); } - - return controller_interface::CallbackReturn::SUCCESS; }; - std::vector joint_names_; - std::string interface_name_; -}; - -class ForwardCommandController : public BaseForwardCommandController -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ForwardCommandController() : BaseForwardCommandController() {} -}; - -class ChainableForwardCommandController -: public BaseForwardCommandController -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ChainableForwardCommandController() : BaseForwardCommandController() - { - } -}; - } // namespace forward_command_controller -#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ +#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ \ No newline at end of file diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 273832e727..ff69a0e457 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -37,80 +37,80 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -template < - typename T, - typename std::enable_if< - std::is_convertible::value, - T>::type * = nullptr, - typename std::enable_if< - std::is_convertible::value, T>::type * = - nullptr> -class BaseMultiInterfaceForwardCommandController : public T -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - BaseMultiInterfaceForwardCommandController() : T() {} - -protected: - void declare_parameters() override - { - controller_interface::ControllerInterfaceBase::auto_declare("joint", joint_name_); - controller_interface::ControllerInterfaceBase::auto_declare>( - "interface_names", interface_names_); - }; - - controller_interface::CallbackReturn read_parameters() override + template < + typename T, + typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr, + typename std::enable_if< + std::is_convertible::value, T>::type * = + nullptr> + class BaseMultiInterfaceForwardCommandController : public T { - joint_name_ = T::get_node()->get_parameter("joint").as_string(); - interface_names_ = T::get_node()->get_parameter("interface_names").as_string_array(); + public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + BaseMultiInterfaceForwardCommandController() : T() {} - if (joint_name_.empty()) + protected: + void declare_parameters() override { - RCLCPP_ERROR(T::get_node()->get_logger(), "'joint' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } + controller_interface::ControllerInterfaceBase::auto_declare("joint", joint_name_); + controller_interface::ControllerInterfaceBase::auto_declare>( + "interface_names", interface_names_); + }; - if (interface_names_.empty()) + controller_interface::CallbackReturn read_parameters() override { - RCLCPP_ERROR(T::get_node()->get_logger(), "'interfaces' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } + joint_name_ = T::get_node()->get_parameter("joint").as_string(); + interface_names_ = T::get_node()->get_parameter("interface_names").as_string_array(); - for (const auto & interface : interface_names_) + if (joint_name_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'joint' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } - { - T::command_interface_names_.push_back(joint_name_ + "/" + interface); - } + if (interface_names_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } - return controller_interface::CallbackReturn::SUCCESS; - }; + for (const auto & interface : interface_names_) - std::string joint_name_; - std::vector interface_names_; -}; + { + T::command_interface_names_.push_back(joint_name_ + "/" + interface); + } -class MultiInterfaceForwardCommandController -: public BaseMultiInterfaceForwardCommandController -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - MultiInterfaceForwardCommandController() - : BaseMultiInterfaceForwardCommandController() + return controller_interface::CallbackReturn::SUCCESS; + }; + + std::string joint_name_; + std::vector interface_names_; + }; + + class MultiInterfaceForwardCommandController + : public BaseMultiInterfaceForwardCommandController { - } -}; + public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + MultiInterfaceForwardCommandController() + : BaseMultiInterfaceForwardCommandController() + { + } + }; -class ChainableMultiInterfaceForwardCommandController -: public BaseMultiInterfaceForwardCommandController -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ChainableMultiInterfaceForwardCommandController() - : BaseMultiInterfaceForwardCommandController() + class ChainableMultiInterfaceForwardCommandController + : public BaseMultiInterfaceForwardCommandController { - } -}; + public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableMultiInterfaceForwardCommandController() + : BaseMultiInterfaceForwardCommandController() + { + } + }; } // namespace forward_command_controller -#endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ +#endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ \ No newline at end of file diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0d9592cf44..0c623229f6 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,61 +14,13 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include -#include -#include -#include -#include - -#include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" - -namespace forward_command_controller -{ -ForwardCommandController::ForwardCommandController() : ForwardControllersBase() {} - -void ForwardCommandController::declare_parameters() -{ - auto_declare("joints", std::vector()); - auto_declare("interface_name", std::string()); -} - -controller_interface::CallbackReturn ForwardCommandController::read_parameters() -{ - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) - { - interface_name_ = get_node()->get_parameter("interface_name").as_string(); - } - - if (interface_name_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & joint : joint_names_) - { - command_interface_types_.push_back(joint + "/" + interface_name_); - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -} // namespace forward_command_controller +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) + forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( - forward_command_controller::ChainableForwardCommandController, - controller_interface::ChainableControllerInterface) + forward_command_controller::ChainableForwardCommandController, + controller_interface::ChainableControllerInterface) \ No newline at end of file diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 6fd7bb74ce..c09b37e96d 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -14,56 +14,14 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" - -#include -#include - -namespace forward_command_controller -{ -MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() -: ForwardControllersBase() -{ -} - -void MultiInterfaceForwardCommandController::declare_parameters() -{ - auto_declare("joint", joint_name_); - auto_declare("interface_names", interface_names_); -} - -controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() -{ - joint_name_ = get_node()->get_parameter("joint").as_string(); - interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); - - if (joint_name_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - if (interface_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & interface : interface_names_) - { - command_interface_types_.push_back(joint_name_ + "/" + interface); - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -} // namespace forward_command_controller - +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - forward_command_controller::MultiInterfaceForwardCommandController, - controller_interface::ControllerInterface) + forward_command_controller::MultiInterfaceForwardCommandController, + controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( - forward_command_controller::ChainableMultiInterfaceForwardCommandController, - controller_interface::ChainableControllerInterface) + forward_command_controller::ChainableMultiInterfaceForwardCommandController, + controller_interface::ChainableControllerInterface) \ No newline at end of file From fcc2be2789cd4c0186a2a749902a8efdd93e18ac Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 19 Jun 2022 20:01:03 -0600 Subject: [PATCH 014/111] fixed interface memory bug --- admittance_controller/src/admittance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 55a1a549dc..3644cdce5a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -149,7 +149,7 @@ namespace admittance_controller { chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(std::string(get_node()->get_name()), joint + "/" + interface, reference_interfaces_.data() + - index)); + index++)); } } From 0aac28e0e6cb4454f05ce24cd8e9cc1739b47908 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 19 Jun 2022 20:53:59 -0600 Subject: [PATCH 015/111] changed position/velocity reference fields --- .../admittance_controller.hpp | 6 ++- .../src/admittance_controller.cpp | 46 ++++++++----------- 2 files changed, 24 insertions(+), 28 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 82fecca5a8..c65bba1968 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -120,8 +120,10 @@ class AdmittanceController : public controller_interface::ChainableControllerInt std::vector> joint_position_state_interface_; std::vector> joint_velocity_state_interface_; std::vector> joint_acceleration_state_interface_; - std::vector> joint_position_chainable_interface_; - std::vector> joint_velocity_chainable_interface_; +// std::vector> joint_position_chainable_interface_; +// std::vector> joint_velocity_chainable_interface_; + std::vector position_reference_; + std::vector velocity_reference_; std::vector> joint_acceleration_chainable_interface_; // Admittance rule and dependent variables; std::unique_ptr admittance_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 3644cdce5a..bd7c55a5b2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -143,27 +143,21 @@ namespace admittance_controller { reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); chainable_command_interfaces.reserve(num_chainable_interfaces); + position_reference_.clear(); + velocity_reference_.clear(); + std::unordered_map *> chainable_interface_map = { + {hardware_interface::HW_IF_POSITION, &position_reference_}, + {hardware_interface::HW_IF_VELOCITY, &velocity_reference_}}; + auto index = 0ul; for (const auto &interface: params.chainable_command_interface_types_) { for (const auto &joint: params.joint_names_) { + chainable_interface_map[interface]->push_back(reference_interfaces_.data() + index); chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(std::string(get_node()->get_name()), joint + "/" + interface, reference_interfaces_.data() + - index++)); - } - } - - joint_position_chainable_interface_.clear(); - joint_velocity_chainable_interface_.clear(); - std::unordered_map> *> chainable_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_chainable_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_chainable_interface_} - }; - - for (auto i = 0ul; i < params.chainable_command_interface_types_.size(); i++) { - for (auto j = 0ul; j < params.joint_names_.size(); j++) { - chainable_interface_map[params.chainable_command_interface_types_[i]]->emplace_back( - chainable_command_interfaces[i * num_joints_ + j]); + index)); + index++; } } @@ -176,14 +170,14 @@ namespace admittance_controller { joint_command_msg = *rtBuffers.input_joint_command_.readFromRT(); for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { - joint_position_chainable_interface_[i].get().set_value(joint_command_msg->positions[i]); + *position_reference_[i] = joint_command_msg->positions[i]; } for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { - joint_velocity_chainable_interface_[i].get().set_value(joint_command_msg->velocities[i]); - } - for (auto i = 0ul; i < joint_command_msg->accelerations.size(); i++) { - joint_acceleration_chainable_interface_[i].get().set_value(joint_command_msg->accelerations[i]); + *velocity_reference_[i] = joint_command_msg->velocities[i]; } +// for (auto i = 0ul; i < joint_command_msg->accelerations.size(); i++) { +// *velocity_acceleration_[i] = joint_command_msg->accelerations[i]; +// } return controller_interface::return_type::OK; } @@ -487,19 +481,19 @@ namespace admittance_controller { // Fill fields of state argument from hardware command interfaces. If the interface does not exist or // the values are nan, that corresponding state field will be set to empty - state_reference.positions.resize(joint_position_chainable_interface_.size(), 0.0); - state_reference.velocities.resize(joint_velocity_chainable_interface_.size(), 0.0); + state_reference.positions.resize(position_reference_.size(), 0.0); + state_reference.velocities.resize(velocity_reference_.size(), 0.0); state_reference.accelerations.resize(joint_acceleration_chainable_interface_.size(), 0.0); - for (auto i = 0ul; i < joint_position_chainable_interface_.size(); i++) { - state_reference.positions[i] = joint_position_chainable_interface_[i].get().get_value(); + for (auto i = 0ul; i < position_reference_.size(); i++) { + state_reference.positions[i] = *position_reference_[i]; if (std::isnan(state_reference.positions[i])) { state_reference.positions = last_state_reference_.positions; break; } } - for (auto i = 0ul; i < joint_velocity_chainable_interface_.size(); i++) { - state_reference.velocities[i] = joint_velocity_chainable_interface_[i].get().get_value(); + for (auto i = 0ul; i < velocity_reference_.size(); i++) { + state_reference.velocities[i] = *velocity_reference_[i]; if (std::isnan(state_reference.velocities[i])) { state_reference.velocities = last_state_reference_.velocities; break; From 7033048a559a981c3d377ac81932d8d9c1ee5ed5 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 20 Jun 2022 09:34:21 -0600 Subject: [PATCH 016/111] added gravity compensation --- .../admittance_controller/admittance_rule.hpp | 34 ++++++++++++++++--- .../admittance_rule_impl.hpp | 29 ++++++++++++---- 2 files changed, 53 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 1b2bc1a06d..9a7f431242 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -45,13 +45,14 @@ namespace admittance_controller { class AdmittanceParameters : public control_toolbox::ParameterHandler { public: - AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 24, 6) { + AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 28, 7) { add_string_parameter("IK.base", false); add_string_parameter("IK.group_name", false); add_string_parameter("IK.plugin_name", false); add_string_parameter("control_frame", true); add_string_parameter("sensor_frame", false); add_string_parameter("end_effector_name", false); + add_string_parameter("CoG.frame", false); add_bool_parameter("open_loop_control", true); add_bool_parameter("enable_parameter_update_without_reactivation", false); @@ -87,6 +88,11 @@ namespace admittance_controller { add_double_parameter("admittance.damping_ratio.rx", true); add_double_parameter("admittance.damping_ratio.ry", true); add_double_parameter("admittance.damping_ratio.rz", true); + + add_double_parameter("CoG.x", false); + add_double_parameter("CoG.y", false); + add_double_parameter("CoG.z", false); + add_double_parameter("CoG.force", false); } bool check_if_parameters_are_valid() override { @@ -174,7 +180,11 @@ namespace admittance_controller { end_effector_name_ = string_parameters_[5].second; RCUTILS_LOG_INFO_NAMED( logger_name_.c_str(), - "Sensor frame: %s", sensor_frame_.c_str()); + "end effector name: %s", sensor_frame_.c_str()); + cog_frame_ = string_parameters_[6].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "COG frame: %s", cog_frame_.c_str()); open_loop_control_ = bool_parameters_[0].second; RCUTILS_LOG_INFO_NAMED( @@ -210,8 +220,18 @@ namespace admittance_controller { logger_name_.c_str(), "Damping_ratio for the axis %zu is %e", i, damping_ratio_[i]); } - convert_damping_ratio_to_damping(); + + for (int i = 0; i < 3; i++){ + cog_[i] = double_parameters_[24 + i].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Damping_ratio for the axis %zu is %e", i, cog_[i]); + } + force_ = double_parameters_[27].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "CoG force %e", force_); } // IK parameters @@ -224,15 +244,18 @@ namespace admittance_controller { // Admittance calculations (displacement etc) are done in this frame. // Frame where wrench measurements are taken std::string sensor_frame_; + std::string cog_frame_; bool open_loop_control_; bool enable_parameter_update_without_reactivation_; std::array damping_; std::array damping_ratio_; + std::array cog_; std::array mass_; std::array selected_axes_; std::array stiffness_; + double force_; }; @@ -342,10 +365,13 @@ namespace admittance_controller { std::vector cur_sensor_transform_vec; Eigen::Matrix cur_control_transform; std::vector cur_control_transform_vec; + Eigen::Matrix cog_transform; + std::vector cog_transform_vec; Eigen::Vector wrench; - Eigen::Vector desired_ee_vel; + Eigen::Vector cog_; + Eigen::Vector ee_weight; std::vector desired_ee_vel_vec; std::vector joint_vel; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 21c2bfbaf8..beabbd8e78 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -79,17 +79,23 @@ namespace admittance_controller { desired_ee_transform_vec.resize(16, 0.0); cur_sensor_transform_vec.resize(16, 0.0); cur_control_transform_vec.resize(16, 0.0); + cog_transform_vec.resize(16, 0.0); cur_ee_transform.setZero(); desired_ee_transform.setZero(); cur_sensor_transform.setZero(); cur_control_transform.setZero(); + cog_transform.setZero(); admittance_position_.setIdentity(); admittance_velocity_.setZero(); admittance_acceleration_.setZero(); wrench.setZero(); desired_ee_vel.setZero(); + memcpy(cog_.data(), parameters_.cog_.data(), 3*sizeof(double)); + ee_weight.setZero(); + ee_weight[2] = -parameters_.force_; + return controller_interface::return_type::OK; } @@ -116,10 +122,14 @@ namespace admittance_controller { no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.control_frame_, cur_control_transform_vec); memcpy(cur_control_transform.data(),cur_control_transform_vec.data(), 16*sizeof(double)); - auto cur_control_transform_inv = invert_transform(cur_control_transform); - auto cur_control_rot_inv = cur_control_transform_inv(Eigen::seq(0,2), Eigen::seq(0,2)); - auto cur_control_rot = cur_control_transform(Eigen::seq(0,2), Eigen::seq(0,2)); - auto cur_sensor_rot = cur_sensor_transform(Eigen::seq(0,2), Eigen::seq(0,2)); + no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.control_frame_, cog_transform_vec); + memcpy(cog_transform.data(),cog_transform_vec.data(), 16*sizeof(double)); + + Eigen::Matrix4d cur_control_transform_inv = invert_transform(cur_control_transform); + Eigen::Matrix3d cur_control_rot_inv = cur_control_transform_inv(Eigen::seq(0,2), Eigen::seq(0,2)); + Eigen::Matrix3d cur_control_rot = cur_control_transform(Eigen::seq(0,2), Eigen::seq(0,2)); + Eigen::Matrix3d cur_sensor_rot = cur_sensor_transform(Eigen::seq(0,2), Eigen::seq(0,2)); + Eigen::Matrix3d cog_rot = cog_transform(Eigen::seq(0,2), Eigen::seq(0,2)); desired_ee_vel(Eigen::seq(0,2)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(0,2)); desired_ee_vel(Eigen::seq(3,5)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(3,5)); @@ -127,9 +137,16 @@ namespace admittance_controller { // apply filter and update wrench vector process_wrench_measurements(measured_wrench); // transform into control frame + Eigen::Vector wrench_base; Eigen::Vector wrench_control; - wrench_control(Eigen::seq(0, 2)) = cur_control_rot_inv * cur_sensor_rot * wrench(Eigen::seq(0, 2)); - wrench_control(Eigen::seq(3, 5)) = cur_control_rot_inv * cur_sensor_rot * wrench(Eigen::seq(3, 5)); + wrench_base(Eigen::seq(0, 2)) = cur_sensor_rot * wrench(Eigen::seq(0, 2)); + wrench_base(Eigen::seq(3, 5)) = cur_sensor_rot * wrench(Eigen::seq(3, 5)); + + wrench_base[2] -= ee_weight[2]; + wrench_base(Eigen::seq(3, 5)) -= (cog_rot*cog_).cross(ee_weight); + + wrench_control(Eigen::seq(0, 2)) = cur_control_rot_inv * wrench_base(Eigen::seq(0, 2)); + wrench_control(Eigen::seq(3, 5)) = cur_control_rot_inv * wrench_base(Eigen::seq(3, 5)); // Compute admittance control law: F = M*a + D*v + S*(x - x_d) calculate_admittance_rule(wrench_control, period); From 1a266bcb3191d85891a75731b504c0d55db19284 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 20 Jun 2022 12:38:48 -0600 Subject: [PATCH 017/111] replaced Eigen 3.4 API with Eigen 3.3 --- .../admittance_controller/admittance_rule.hpp | 23 +++----- .../admittance_rule_impl.hpp | 54 ++++++++++--------- 2 files changed, 35 insertions(+), 42 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 9a7f431242..4aa201f802 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -299,15 +299,6 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states); -// controller_interface::return_type update( -// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, -// const geometry_msgs::msg::Wrench & measured_wrench, -// const geometry_msgs::msg::PoseStamped & reference_pose, -// const geometry_msgs::msg::WrenchStamped & reference_force, -// const rclcpp::Duration & period, -// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states -// ); - controller_interface::return_type get_controller_state( control_msgs::msg::AdmittanceControllerState &state_message ); @@ -331,7 +322,7 @@ namespace admittance_controller { * All values are in he controller frame */ void calculate_admittance_rule( - const Eigen::Vector &wrench, + const Eigen::Matrix &wrench, const rclcpp::Duration &period ); @@ -350,9 +341,9 @@ namespace admittance_controller { // int num_joints_; - Eigen::Vector admittance_acceleration_; + Eigen::Matrix admittance_acceleration_; std::vector admittance_acceleration_vec; - Eigen::Vector admittance_velocity_; + Eigen::Matrix admittance_velocity_; std::vector admittance_velocity_vec; Eigen::Matrix admittance_position_; std::vector admittance_position_vec; @@ -368,10 +359,10 @@ namespace admittance_controller { Eigen::Matrix cog_transform; std::vector cog_transform_vec; - Eigen::Vector wrench; - Eigen::Vector desired_ee_vel; - Eigen::Vector cog_; - Eigen::Vector ee_weight; + Eigen::Matrix wrench; + Eigen::Matrix desired_ee_vel; + Eigen::Matrix cog_; + Eigen::Matrix ee_weight; std::vector desired_ee_vel_vec; std::vector joint_vel; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index beabbd8e78..a4f91ad22a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -126,41 +126,42 @@ namespace admittance_controller { memcpy(cog_transform.data(),cog_transform_vec.data(), 16*sizeof(double)); Eigen::Matrix4d cur_control_transform_inv = invert_transform(cur_control_transform); - Eigen::Matrix3d cur_control_rot_inv = cur_control_transform_inv(Eigen::seq(0,2), Eigen::seq(0,2)); - Eigen::Matrix3d cur_control_rot = cur_control_transform(Eigen::seq(0,2), Eigen::seq(0,2)); - Eigen::Matrix3d cur_sensor_rot = cur_sensor_transform(Eigen::seq(0,2), Eigen::seq(0,2)); - Eigen::Matrix3d cog_rot = cog_transform(Eigen::seq(0,2), Eigen::seq(0,2)); + Eigen::Matrix3d cur_control_rot = cur_control_transform.block<3,3>(0,0); + Eigen::Matrix3d cur_control_rot_inv = cur_control_transform_inv.block<3,3>(0,0); + Eigen::Matrix3d cur_sensor_rot = cur_sensor_transform.block<3,3>(0,0); + Eigen::Matrix3d cog_rot = cog_transform.block<3,3>(0,0); - desired_ee_vel(Eigen::seq(0,2)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(0,2)); - desired_ee_vel(Eigen::seq(3,5)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(3,5)); + // TODO fix this +// desired_ee_vel(Eigen::seq(0,2)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(0,2)); +// desired_ee_vel(Eigen::seq(3,5)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(3,5)); // apply filter and update wrench vector process_wrench_measurements(measured_wrench); // transform into control frame - Eigen::Vector wrench_base; - Eigen::Vector wrench_control; - wrench_base(Eigen::seq(0, 2)) = cur_sensor_rot * wrench(Eigen::seq(0, 2)); - wrench_base(Eigen::seq(3, 5)) = cur_sensor_rot * wrench(Eigen::seq(3, 5)); + Eigen::Matrix wrench_base; + Eigen::Matrix wrench_control; + wrench_base.block<3,1>(0,0) = cur_sensor_rot * wrench.block<3,1>(0,0); + wrench_base.block<3,1>(3,0) = cur_sensor_rot * wrench.block<3,1>(3,0); wrench_base[2] -= ee_weight[2]; - wrench_base(Eigen::seq(3, 5)) -= (cog_rot*cog_).cross(ee_weight); + wrench_base.block<3,1>(3,0)-= (cog_rot*cog_).cross(ee_weight); - wrench_control(Eigen::seq(0, 2)) = cur_control_rot_inv * wrench_base(Eigen::seq(0, 2)); - wrench_control(Eigen::seq(3, 5)) = cur_control_rot_inv * wrench_base(Eigen::seq(3, 5)); + wrench_control.block<3,1>(0,0) = cur_control_rot_inv * wrench_base.block<3,1>(0,0); + wrench_control.block<3,1>(3,0) = cur_control_rot_inv * wrench_base.block<3,1>(3,0); // Compute admittance control law: F = M*a + D*v + S*(x - x_d) calculate_admittance_rule(wrench_control, period); // transform admittance values back to base frame // velocity - Eigen::Vector3d admittance_velocity_cf = cur_control_rot*admittance_velocity_(Eigen::seq(0,2)); + Eigen::Vector3d admittance_velocity_cf = cur_control_rot*admittance_velocity_.block<3,1>(0,0); memcpy(admittance_velocity_vec.data(),admittance_velocity_cf.data(), 3*sizeof(double)); - Eigen::Vector3d admittance_angular_vel_cf = cur_control_rot*admittance_velocity_(Eigen::seq(3,5)); + Eigen::Vector3d admittance_angular_vel_cf = cur_control_rot*admittance_velocity_.block<3,1>(3,0); memcpy(admittance_velocity_vec.data()+3, admittance_angular_vel_cf.data(), 3*sizeof(double)); // velocity acceleration - Eigen::Vector3d admittance_accel_cf = cur_control_rot * admittance_acceleration_(Eigen::seq(0, 2)); + Eigen::Vector3d admittance_accel_cf = cur_control_rot * admittance_acceleration_.block<3,1>(0,0); memcpy(admittance_acceleration_vec.data(),admittance_accel_cf.data(), 3*sizeof(double)); - Eigen::Vector3d admittance_angular_accel_cf = cur_control_rot * admittance_acceleration_(Eigen::seq(3, 5)); + Eigen::Vector3d admittance_angular_accel_cf = cur_control_rot * admittance_acceleration_.block<3,1>(3,0); memcpy(admittance_acceleration_vec.data()+3, admittance_angular_vel_cf.data(), 3*sizeof(double)); // calculate robot joint velocity from admittance velocity @@ -189,7 +190,7 @@ namespace admittance_controller { } void AdmittanceRule::calculate_admittance_rule( - const Eigen::Vector &wrench, + const Eigen::Matrix &wrench, const rclcpp::Duration &period ) { // Compute admittance control law: F = M*a + D*v + S*(x - x_d) @@ -208,7 +209,8 @@ namespace admittance_controller { } } - auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); +// auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); + auto R = admittance_position_.block<3,3>(0,0); Eigen::Vector3d V = get_rotation_axis(R); double theta = acos((1.0/2.0)*(R.trace()-1)); // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped @@ -237,7 +239,7 @@ namespace admittance_controller { Eigen::Matrix3d R_dot = skew_symmetric*R; R += R_dot*(1.0 / 1000); R = normalize_rotation(R); - admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)) = R; + admittance_position_.block<3,3>(0,0) = R; } Eigen::Vector3d AdmittanceRule::get_rotation_axis(const Eigen::Matrix3d& R) const{ @@ -305,11 +307,11 @@ namespace admittance_controller { Eigen::Matrix3d AdmittanceRule::normalize_rotation(Eigen::Matrix3d R){ // enforce orthonormal constraint - Eigen::Vector3d R_0 = R(Eigen::seq(0,2), Eigen::seq(0,0)); + Eigen::Vector3d R_0 = R.block<3,1>(0,0); R_0.normalize(); - Eigen::Vector3d R_1 = R(Eigen::seq(0,2), Eigen::seq(1,1)); + Eigen::Vector3d R_1 = R.block<3,1>(0,1); R_1.normalize(); - Eigen::Vector3d R_2 = R(Eigen::seq(0,2), Eigen::seq(2,2)); + Eigen::Vector3d R_2 = R.block<3,1>(0,2); R_2.normalize(); double drift = R_0.dot(R_1); @@ -328,10 +330,10 @@ namespace admittance_controller { } Eigen::Matrix4d AdmittanceRule::invert_transform(const Eigen::Matrix4d & T){ + Eigen::Matrix3d R = T.block<3,3>(0,0); Eigen::Matrix4d T_inv = T; - Eigen::Matrix3d R = T_inv(Eigen::seq(0,2), Eigen::seq(0,2)); - T_inv(Eigen::seq(0,2), Eigen::seq(0,2)) = R.transpose(); - T_inv(Eigen::seq(0,2), Eigen::seq(3,3)) = -R.transpose()*T_inv(Eigen::seq(0,2), Eigen::seq(3,3)); + T_inv.block<3,3>(0,0) = R.transpose(); + T_inv.block<3,1>(0,3) = -R.transpose()*T_inv.block<3,1>(0,3); return T_inv; } From 8dc9b37eb8d2d3ab31edce010907dc708bae5bb9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 20 Jun 2022 17:12:42 -0600 Subject: [PATCH 018/111] clean up and TODOs --- .../admittance_controller.hpp | 13 +- .../admittance_controller/admittance_rule.hpp | 93 +++-- .../admittance_rule_impl.hpp | 332 +++++++++--------- .../src/admittance_controller.cpp | 101 +++--- 4 files changed, 256 insertions(+), 283 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index c65bba1968..f804b18b2f 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -52,7 +52,6 @@ namespace admittance_controller using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; struct RTBuffers{ - realtime_tools::RealtimeBuffer> input_wrench_command_; realtime_tools::RealtimeBuffer> input_joint_command_; std::unique_ptr> state_publisher_; }; @@ -113,6 +112,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt bool on_set_chained_mode(bool chained_mode) override; int num_joints_{}; + std::vector> joint_position_command_interface_; std::vector> joint_velocity_command_interface_; std::vector> joint_acceleration_command_interface_; @@ -120,11 +120,10 @@ class AdmittanceController : public controller_interface::ChainableControllerInt std::vector> joint_position_state_interface_; std::vector> joint_velocity_state_interface_; std::vector> joint_acceleration_state_interface_; -// std::vector> joint_position_chainable_interface_; -// std::vector> joint_velocity_chainable_interface_; + std::vector position_reference_; std::vector velocity_reference_; - std::vector> joint_acceleration_chainable_interface_; + // Admittance rule and dependent variables; std::unique_ptr admittance_; // joint limiter TODO @@ -132,12 +131,9 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // controller parameters filled by ROS ParameterStruct params; // ROS subscribers - rclcpp::Subscription::SharedPtr input_wrench_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; rclcpp::Publisher::SharedPtr s_publisher_ = nullptr; // ROS messages - std::shared_ptr traj_command_msg; - std::shared_ptr wrench_msg; std::shared_ptr joint_command_msg; // real-time buffer RTBuffers rtBuffers; @@ -159,11 +155,10 @@ class AdmittanceController : public controller_interface::ChainableControllerInt std::vector open_loop_buffer; // helper methods - void wrench_stamped_callback(const std::shared_ptr msg); void joint_command_callback(const std::shared_ptr msg); void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state); void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); - void read_state_reference_from_chainable_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 4aa201f802..21d8f2096d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -37,6 +37,7 @@ namespace { // Utility namespace static constexpr double WRENCH_EPSILON = 1e-10; static constexpr double POSE_ERROR_EPSILON = 1e-12; static constexpr double POSE_EPSILON = 1e-15; + const double ROT_AXIS_EPSILON = 0.001; } // utility namespace @@ -46,9 +47,9 @@ namespace admittance_controller { class AdmittanceParameters : public control_toolbox::ParameterHandler { public: AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 28, 7) { - add_string_parameter("IK.base", false); - add_string_parameter("IK.group_name", false); - add_string_parameter("IK.plugin_name", false); + add_string_parameter("kinematics.base", false); + add_string_parameter("kinematics.group_name", false); + add_string_parameter("kinematics.plugin_name", false); add_string_parameter("control_frame", true); add_string_parameter("sensor_frame", false); add_string_parameter("end_effector_name", false); @@ -157,18 +158,18 @@ namespace admittance_controller { } void update_storage() override { - ik_base_frame_ = string_parameters_[0].second; + kinematics_base_frame_ = string_parameters_[0].second; RCUTILS_LOG_INFO_NAMED( logger_name_.c_str(), - "IK Base frame: %s", ik_base_frame_.c_str()); - ik_group_name_ = string_parameters_[1].second; + "IK Base frame: %s", kinematics_base_frame_.c_str()); + kinematics_group_name_ = string_parameters_[1].second; RCUTILS_LOG_INFO_NAMED( logger_name_.c_str(), - "IK group name frame: %s", ik_group_name_.c_str()); - ik_plugin_name_ = string_parameters_[2].second; + "IK group name frame: %s", kinematics_group_name_.c_str()); + kinematics_plugin_name_ = string_parameters_[2].second; RCUTILS_LOG_INFO_NAMED( logger_name_.c_str(), - "IK plugin name: %s", ik_plugin_name_.c_str()); + "IK plugin name: %s", kinematics_plugin_name_.c_str()); control_frame_ = string_parameters_[3].second; RCUTILS_LOG_INFO_NAMED( logger_name_.c_str(), @@ -235,9 +236,9 @@ namespace admittance_controller { } // IK parameters - std::string ik_base_frame_; - std::string ik_group_name_; - std::string ik_plugin_name_; + std::string kinematics_base_frame_; + std::string kinematics_group_name_; + std::string kinematics_plugin_name_; std::string end_effector_name_; // Depends on the scenario: usually base_link, tool or end-effector std::string control_frame_; @@ -264,25 +265,8 @@ namespace admittance_controller { AdmittanceRule() = default; controller_interface::return_type configure(std::shared_ptr node, int num_joint); - controller_interface::return_type reset(); -// controller_interface::return_type update( -// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, -// const geometry_msgs::msg::Wrench & measured_wrench, -// const geometry_msgs::msg::PoseStamped & reference_pose, -// const rclcpp::Duration & period, -// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states -// ); -// -// controller_interface::return_type update( -// const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, -// const geometry_msgs::msg::Wrench & measured_wrench, -// const std::array & reference_joint_deltas, -// const rclcpp::Duration & period, -// trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states -// ); - /** * Calculate 'desired joint states' based on the 'measured force' and 'reference joint state'. * @@ -306,7 +290,7 @@ namespace admittance_controller { public: // TODO(destogl): Add parameter for this - bool feedforward_commanded_input_ = true; + bool use_feedforward_commanded_input_ = true; // Dynamic admittance parameters @@ -314,57 +298,62 @@ namespace admittance_controller { // Filter parameter for exponential smoothing const double alpha = 0.1; // TODO make a ros param - const double EPSILON = 0.001; + protected: /** - * All values are in he controller frame + * All values are in the controller frame */ void calculate_admittance_rule( const Eigen::Matrix &wrench, - const rclcpp::Duration &period + const Eigen::Matrix &desired_vel, + const double dt ); - void process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench + Eigen::Matrix process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix& last_wrench ); - Eigen::Matrix4d invert_transform(const Eigen::Matrix4d &T); + void normalize_rotation(Eigen::Matrix& R); + Eigen::Matrix invert_transform(Eigen::Matrix &T); + Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool & success); Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d& R) const; - Eigen::Matrix3d normalize_rotation(Eigen::Matrix3d R); + void convert_cartesian_deltas_to_joint_deltas(const std::vector& positions, + const Eigen::Matrix & cartesian_delta, std::vector& joint_delta, bool & success); - // Differential IK algorithm (loads a plugin) - std::shared_ptr> ik_loader_; - std::unique_ptr ik_; + // Kinematics interface plugin loader + std::shared_ptr> kinematics_loader_; + std::unique_ptr kinematics_; - // + // number of robot joint int num_joints_; + // buffers to pass data to kinematics interface + std::vector transform_buffer_vec; + std::vector joint_buffer_vec; + std::vector cart_buffer_vec; + + // admittance controller values Eigen::Matrix admittance_acceleration_; - std::vector admittance_acceleration_vec; Eigen::Matrix admittance_velocity_; - std::vector admittance_velocity_vec; Eigen::Matrix admittance_position_; - std::vector admittance_position_vec; + // transforms Eigen::Matrix cur_ee_transform; - std::vector cur_ee_transform_vec; - Eigen::Matrix desired_ee_transform; - std::vector desired_ee_transform_vec; + Eigen::Matrix reference_ee_transform; Eigen::Matrix cur_sensor_transform; - std::vector cur_sensor_transform_vec; Eigen::Matrix cur_control_transform; - std::vector cur_control_transform_vec; Eigen::Matrix cog_transform; - std::vector cog_transform_vec; + // external force Eigen::Matrix wrench; - Eigen::Matrix desired_ee_vel; + // position of center of gravity in cog_frame Eigen::Matrix cog_; + // force applied to sensor due to weight of end effector Eigen::Matrix ee_weight; - std::vector desired_ee_vel_vec; + // admittance controller values in joint space std::vector joint_vel; std::vector joint_acc; std::vector joint_pos; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index a4f91ad22a..d8f52c0fa9 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \authors: Denis Stogl, Andy Zelenak +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ @@ -27,29 +27,34 @@ namespace admittance_controller { controller_interface::return_type AdmittanceRule::configure(std::shared_ptr node, int num_joints) { - // initialize memory (non-realtime function) + // configure admittance rule using num_joints and load kinematics interface num_joints_ = num_joints; - // initialize all value to zero + // initialize memory and values to zero (non-realtime function) reset(); + // set parameters values + memcpy(cog_.data(), parameters_.cog_.data(), 3 * sizeof(double)); + ee_weight.setZero(); + ee_weight[2] = -parameters_.force_; + // Load the differential IK plugin - if (!parameters_.ik_plugin_name_.empty()) { + if (!parameters_.kinematics_plugin_name_.empty()) { try { - // TODO(destogl): add "ik_interface" into separate package and then rename the package in - // the next line from "admittance_controller" to "ik_base_plugin" - ik_loader_ = std::make_shared>( + // TODO(destogl): add "kinematics_interface" into separate package and then rename the package in + // the next line from "admittance_controller" to "kinematics_base_plugin" + kinematics_loader_ = std::make_shared>( "kdl_plugin", "kinematics_interface::KinematicsBaseClass"); - ik_ = std::unique_ptr( - ik_loader_->createUnmanagedInstance(parameters_.ik_plugin_name_)); - if (!ik_->initialize(node, parameters_.end_effector_name_)) { + kinematics_ = std::unique_ptr( + kinematics_loader_->createUnmanagedInstance(parameters_.kinematics_plugin_name_)); + if (!kinematics_->initialize(node, parameters_.end_effector_name_)) { return controller_interface::return_type::ERROR; } } catch (pluginlib::PluginlibException &ex) { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", - parameters_.ik_plugin_name_.c_str(), ex.what()); + parameters_.kinematics_plugin_name_.c_str(), ex.what()); return controller_interface::return_type::ERROR; } } else { @@ -64,37 +69,28 @@ namespace admittance_controller { controller_interface::return_type AdmittanceRule::reset() { // reset all values back to zero - // admittance state vectors - admittance_position_vec.resize(16, 0.0); - admittance_velocity_vec.resize(6, 0.0); - admittance_acceleration_vec.resize(6, 0.0); + //allocate dynamic buffers + joint_buffer_vec.assign(num_joints_, 0.0); + transform_buffer_vec.assign(16, 0.0); + cart_buffer_vec.assign(6, 0.0); // admittance state vectors in joint space joint_pos.resize(num_joints_, 0.0); joint_vel.resize(num_joints_, 0.0); joint_acc.resize(num_joints_, 0.0); - // link transforms in base link frame - cur_ee_transform_vec.resize(16, 0.0); - desired_ee_transform_vec.resize(16, 0.0); - cur_sensor_transform_vec.resize(16, 0.0); - cur_control_transform_vec.resize(16, 0.0); - cog_transform_vec.resize(16, 0.0); - - cur_ee_transform.setZero(); - desired_ee_transform.setZero(); - cur_sensor_transform.setZero(); - cur_control_transform.setZero(); - cog_transform.setZero(); + // transforms + cur_ee_transform.setIdentity(); + reference_ee_transform.setIdentity(); + cur_sensor_transform.setIdentity(); + cur_control_transform.setIdentity(); + cog_transform.setIdentity(); + + // admittance values admittance_position_.setIdentity(); admittance_velocity_.setZero(); admittance_acceleration_.setZero(); wrench.setZero(); - desired_ee_vel.setZero(); - - memcpy(cog_.data(), parameters_.cog_.data(), 3*sizeof(double)); - ee_weight.setZero(); - ee_weight[2] = -parameters_.force_; return controller_interface::return_type::OK; } @@ -107,78 +103,67 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { - desired_joint_state = reference_joint_state; - bool no_failure = true; - - no_failure &= ik_->calculate_link_transform(desired_joint_state.positions, parameters_.end_effector_name_, desired_ee_transform_vec); - memcpy(desired_ee_transform.data(),desired_ee_transform_vec.data(), 16*sizeof(double)); - - no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.end_effector_name_, cur_ee_transform_vec); - memcpy(cur_ee_transform.data(),cur_ee_transform_vec.data(), 16*sizeof(double)); - - no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.sensor_frame_, cur_sensor_transform_vec); - memcpy(cur_sensor_transform.data(),cur_sensor_transform_vec.data(), 16*sizeof(double)); - - no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.control_frame_, cur_control_transform_vec); - memcpy(cur_control_transform.data(),cur_control_transform_vec.data(), 16*sizeof(double)); + // keep track of failed kinematics interface calls + bool success = true; + double dt = period.seconds()+((double) period.nanoseconds())*1E-9; - no_failure &= ik_->calculate_link_transform(current_joint_state.positions, parameters_.control_frame_, cog_transform_vec); - memcpy(cog_transform.data(),cog_transform_vec.data(), 16*sizeof(double)); + // get all needed transforms + reference_ee_transform = get_transform(reference_joint_state.positions, parameters_.end_effector_name_, success); + cur_ee_transform = get_transform(current_joint_state.positions, parameters_.end_effector_name_, success); + cur_sensor_transform = get_transform(current_joint_state.positions, parameters_.sensor_frame_, success); + cur_control_transform = get_transform(current_joint_state.positions, parameters_.control_frame_, success); + cog_transform = get_transform(current_joint_state.positions, parameters_.cog_frame_, success); + auto cur_control_transform_inv = invert_transform(cur_control_transform); - Eigen::Matrix4d cur_control_transform_inv = invert_transform(cur_control_transform); - Eigen::Matrix3d cur_control_rot = cur_control_transform.block<3,3>(0,0); - Eigen::Matrix3d cur_control_rot_inv = cur_control_transform_inv.block<3,3>(0,0); - Eigen::Matrix3d cur_sensor_rot = cur_sensor_transform.block<3,3>(0,0); - Eigen::Matrix3d cog_rot = cog_transform.block<3,3>(0,0); - - // TODO fix this -// desired_ee_vel(Eigen::seq(0,2)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(0,2)); -// desired_ee_vel(Eigen::seq(3,5)) = cur_control_rot_inv*desired_ee_vel(Eigen::seq(3,5)); + // get all needed rotations + auto cur_control_rot = cur_control_transform.block<3, 3>(0, 0); + auto cur_control_rot_inv = cur_control_transform_inv.block<3, 3>(0, 0); + auto cur_sensor_rot = cur_sensor_transform.block<3, 3>(0, 0); + auto cog_rot = cog_transform.block<3, 3>(0, 0); // apply filter and update wrench vector - process_wrench_measurements(measured_wrench); - // transform into control frame - Eigen::Matrix wrench_base; - Eigen::Matrix wrench_control; - wrench_base.block<3,1>(0,0) = cur_sensor_rot * wrench.block<3,1>(0,0); - wrench_base.block<3,1>(3,0) = cur_sensor_rot * wrench.block<3,1>(3,0); + wrench = process_wrench_measurements(measured_wrench, wrench); + + // transform wrench into base frame + Eigen::Matrix wrench_base; + wrench_base.block<3, 1>(0, 0) = cur_sensor_rot * wrench.block<3, 1>(0, 0); + wrench_base.block<3, 1>(3, 0) = cur_sensor_rot * wrench.block<3, 1>(3, 0); + // apply gravity compensation wrench_base[2] -= ee_weight[2]; - wrench_base.block<3,1>(3,0)-= (cog_rot*cog_).cross(ee_weight); + wrench_base.block<3, 1>(3, 0) -= (cog_rot * cog_).cross(ee_weight); + + // transform wrench into control frame + Eigen::Matrix wrench_control; + wrench_control.block<3, 1>(0, 0) = cur_control_rot_inv * wrench_base.block<3, 1>(0, 0); + wrench_control.block<3, 1>(3, 0) = cur_control_rot_inv * wrench_base.block<3, 1>(3, 0); - wrench_control.block<3,1>(0,0) = cur_control_rot_inv * wrench_base.block<3,1>(0,0); - wrench_control.block<3,1>(3,0) = cur_control_rot_inv * wrench_base.block<3,1>(3,0); + // calculate desired velocity in control frame + auto desired_vel = cur_control_rot_inv * admittance_velocity_.reshaped(3, 2) * use_feedforward_commanded_input_*0; // TODO fix this // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - calculate_admittance_rule(wrench_control, period); + calculate_admittance_rule(wrench_control, desired_vel.reshaped(6, 1), dt); // transform admittance values back to base frame - // velocity - Eigen::Vector3d admittance_velocity_cf = cur_control_rot*admittance_velocity_.block<3,1>(0,0); - memcpy(admittance_velocity_vec.data(),admittance_velocity_cf.data(), 3*sizeof(double)); - Eigen::Vector3d admittance_angular_vel_cf = cur_control_rot*admittance_velocity_.block<3,1>(3,0); - memcpy(admittance_velocity_vec.data()+3, admittance_angular_vel_cf.data(), 3*sizeof(double)); - // velocity acceleration - Eigen::Vector3d admittance_accel_cf = cur_control_rot * admittance_acceleration_.block<3,1>(0,0); - memcpy(admittance_acceleration_vec.data(),admittance_accel_cf.data(), 3*sizeof(double)); - Eigen::Vector3d admittance_angular_accel_cf = cur_control_rot * admittance_acceleration_.block<3,1>(3,0); - memcpy(admittance_acceleration_vec.data()+3, admittance_angular_vel_cf.data(), 3*sizeof(double)); - - // calculate robot joint velocity from admittance velocity - no_failure &= ik_->convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_vec, joint_vel); - // calculate robot joint acceleration from admittance velocity - no_failure &= ik_->convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_vec, joint_acc); - - if (!no_failure){ + auto admittance_velocity_base = cur_control_rot * admittance_velocity_.reshaped(3, 2); + convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_base.reshaped(6, 1), + joint_vel, success); + auto admittance_acceleration_base = cur_control_rot * admittance_acceleration_.reshaped(3, 2); + convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_base.reshaped(6, 1), + joint_acc, success); + + // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference + if (!success) { desired_joint_state = reference_joint_state; return controller_interface::return_type::ERROR; } // update joint desired joint state for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { - joint_pos[i] += joint_vel[i] * (1.0 / 1000.0);//- .2 * joint_pos[i] * (1.0 / 1000.0); + joint_pos[i] += joint_vel[i] * dt;//- .2 * joint_pos[i] * (1.0 / 1000.0); desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos[i]; } + for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel[i]; } @@ -190,59 +175,62 @@ namespace admittance_controller { } void AdmittanceRule::calculate_admittance_rule( - const Eigen::Matrix &wrench, - const rclcpp::Duration &period + const Eigen::Matrix &wrench, + const Eigen::Matrix &desired_vel, + const double dt ) { // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - std::array pose_error = {}; - for (size_t axis = 0; axis < 3; ++axis) { //TODO 6 + + for (size_t axis = 0; axis < 3; ++axis) { if (parameters_.selected_axes_[axis]) { - pose_error[axis] = -admittance_position_(axis, 3); + double pose_error = -admittance_position_(axis, 3); admittance_acceleration_[axis] = (1.0 / parameters_.mass_[axis]) * (wrench[axis] + (parameters_.damping_[axis] * - (desired_ee_vel[axis] - - admittance_velocity_[axis])) + + (desired_vel[axis] - + admittance_velocity_[axis])) + (parameters_.stiffness_[axis] * - pose_error[axis])); - admittance_velocity_[axis] += admittance_acceleration_[axis] * (1.0 / 1000);//period.nanoseconds() TODO fix time isssue - admittance_position_(axis, 3) += admittance_velocity_[axis] * (1.0 / 1000); + pose_error)); + admittance_velocity_[axis] += + admittance_acceleration_[axis] * dt; + admittance_position_(axis, 3) += admittance_velocity_[axis] * dt; } } // auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); - auto R = admittance_position_.block<3,3>(0,0); + Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); Eigen::Vector3d V = get_rotation_axis(R); - double theta = acos((1.0/2.0)*(R.trace()-1)); + double theta = acos((1.0 / 2.0) * (R.trace() - 1)); // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped - auto tmp = V[0]*(R(1,2)-R(2,1))+V[1]*(R(2,0)-R(0,2)) - +V[2]*(R(0,1)-R(1,0)); + auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) + + V[2] * (R(0, 1) - R(1, 0)); double sign = (tmp >= 0) ? 1.0 : -1.0; for (size_t axis = 3; axis < 6; ++axis) { if (parameters_.selected_axes_[axis]) { - pose_error[axis] = sign*theta*V(axis-3); + double pose_error = sign * theta * V(axis - 3); admittance_acceleration_[axis] = (1.0 / parameters_.mass_[axis]) * (wrench[axis] + (parameters_.damping_[axis] * - (desired_ee_vel[axis] - - admittance_velocity_[axis])) + + (desired_vel[axis] - + admittance_velocity_[axis])) + (parameters_.stiffness_[axis] * - pose_error[axis])); - admittance_velocity_[axis] += admittance_acceleration_[axis] * (1.0 / 1000);//period.nanoseconds() TODO fix time issue + pose_error)); + admittance_velocity_[axis] += + admittance_acceleration_[axis] * dt; } } Eigen::Matrix3d skew_symmetric; - skew_symmetric << 0, -admittance_velocity_[2+3], admittance_velocity_[1+3], - admittance_velocity_[2+3], 0, -admittance_velocity_[0+3], - -admittance_velocity_[1+3], admittance_velocity_[0+3], 0; - - Eigen::Matrix3d R_dot = skew_symmetric*R; - R += R_dot*(1.0 / 1000); - R = normalize_rotation(R); - admittance_position_.block<3,3>(0,0) = R; + skew_symmetric << 0, -admittance_velocity_[2 + 3], admittance_velocity_[1 + 3], + admittance_velocity_[2 + 3], 0, -admittance_velocity_[0 + 3], + -admittance_velocity_[1 + 3], admittance_velocity_[0 + 3], 0; + + Eigen::Matrix3d R_dot = skew_symmetric * R; + R += R_dot * dt; + normalize_rotation(R); + admittance_position_.block<3, 3>(0, 0) = R; } - Eigen::Vector3d AdmittanceRule::get_rotation_axis(const Eigen::Matrix3d& R) const{ + Eigen::Vector3d AdmittanceRule::get_rotation_axis(const Eigen::Matrix3d &R) const { Eigen::Vector3d V; bool solved = false; @@ -251,11 +239,11 @@ namespace admittance_controller { double R_buffer[9]; // rotate matrix rows and columns to hit every edge case - for (int i = 0; i < 3; i++){ + for (int i = 0; i < 3; i++) { - for(int r = 0; r < 3; r++){ - for(int c = 0; c < 3; c++){ - R_buffer[((c*3 - 3*i) % 9) + ((r - i) % 3)] = R(r,c); + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + R_buffer[((c * 3 - 3 * i) % 9) + ((r - i) % 3)] = R(r, c); } } auto R11 = R_buffer[0]; @@ -264,36 +252,36 @@ namespace admittance_controller { auto R13 = R_buffer[2]; auto R33 = R_buffer[8]; // degenerate: one axis rotation - if (abs(R12+R13) < EPSILON && R11 > 0){ + if (abs(R12 + R13) < ROT_AXIS_EPSILON && R11 > 0) { v2 = 0; v3 = 0; V[i % 3] = v1; - V[(i+1) % 3] = v2; - V[(i+2) % 3] = v3; + V[(i + 1) % 3] = v2; + V[(i + 2) % 3] = v3; solved = true; break; } // degenerate: two axis rotation - if (abs(R12 + R13) < EPSILON && R11 < 0){ + if (abs(R12 + R13) < ROT_AXIS_EPSILON && R11 < 0) { v1 = 0; v2 = 1; - v3 = R32/(1-R33); + v3 = R32 / (1 - R33); V[i % 3] = v1; - V[(i+1) % 3] = v2; - V[(i+2) % 3] = v3; + V[(i + 1) % 3] = v2; + V[(i + 2) % 3] = v3; solved = true; break; } } // general case: three axis rotation - if (!solved){ - v3 = (-R(0,1) -((R(1,1)-1)*(1-R(0,0)))/R(1,0)); + if (!solved) { + v3 = (-R(0, 1) - ((R(1, 1) - 1) * (1 - R(0, 0))) / R(1, 0)); // if v3 is zero, special case - if (abs(v3) > EPSILON) { - v3 = v3 / (R(2,1) - ((R(1,1) - 1) * (R(2,0))) / (R(1,0))); + if (abs(v3) > ROT_AXIS_EPSILON) { + v3 = v3 / (R(2, 1) - ((R(1, 1) - 1) * (R(2, 0))) / (R(1, 0))); } - v2 = (1-R(0,0)-R(2,0)*v3)/R(1,0); + v2 = (1 - R(0, 0) - R(2, 0) * v3) / R(1, 0); V[0] = v1; V[1] = v2; @@ -304,76 +292,94 @@ namespace admittance_controller { return V; } - Eigen::Matrix3d AdmittanceRule::normalize_rotation(Eigen::Matrix3d R){ + void AdmittanceRule::normalize_rotation(Eigen::Matrix3d& R) { // enforce orthonormal constraint - Eigen::Vector3d R_0 = R.block<3,1>(0,0); + Eigen::Vector3d R_0 = R.block<3, 1>(0, 0); R_0.normalize(); - Eigen::Vector3d R_1 = R.block<3,1>(0,1); + Eigen::Vector3d R_1 = R.block<3, 1>(0, 1); R_1.normalize(); - Eigen::Vector3d R_2 = R.block<3,1>(0,2); + Eigen::Vector3d R_2 = R.block<3, 1>(0, 2); R_2.normalize(); double drift = R_0.dot(R_1); - R_1 += R_0*(-drift); + R_1 += R_0 * (-drift); R_1.normalize(); R_2 = R_0.cross(R_1); R_2.normalize(); Eigen::Matrix3d R_out; - R_out.col(0) << R_0; - R_out.col(1) << R_1; - R_out.col(2) << R_2; + R.col(0) << R_0; + R.col(1) << R_1; + R.col(2) << R_2; - return R_out; } - Eigen::Matrix4d AdmittanceRule::invert_transform(const Eigen::Matrix4d & T){ - Eigen::Matrix3d R = T.block<3,3>(0,0); + Eigen::Matrix + AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool &success) { + success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec); + Eigen::Matrix transform; + memcpy(transform.data(), transform_buffer_vec.data(), 16 * sizeof(double)); + return transform; + + } + + Eigen::Matrix + AdmittanceRule::invert_transform(Eigen::Matrix &T) { + Eigen::Matrix3d R = T.block<3, 3>(0, 0); Eigen::Matrix4d T_inv = T; - T_inv.block<3,3>(0,0) = R.transpose(); - T_inv.block<3,1>(0,3) = -R.transpose()*T_inv.block<3,1>(0,3); + T_inv.block<3, 3>(0, 0) = R.transpose(); + T_inv.block<3, 1>(0, 3) = -R.transpose() * T_inv.block<3, 1>(0, 3); return T_inv; + } + void AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, + const Eigen::Matrix &cartesian_delta, + std::vector &joint_delta, bool &success) { + memcpy(cart_buffer_vec.data(), cartesian_delta.data(), 6 * sizeof(double)); + success &= kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec, joint_delta); } controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState &state_message) { // TODO update this - // state_message.input_wrench_control_frame = reference_wrench_control_frame_; -// state_message.input_pose_control_frame = reference_pose_ik_base_frame_; +// state_message.input_wrench_control_frame = reference_wrench_control_frame_; +// state_message.input_pose_control_frame = reference_pose_kinematics_base_frame_; // state_message.measured_wrench = measured_wrench_; // state_message.measured_wrench_filtered = measured_wrench_filtered_; -// state_message.measured_wrench_control_frame = measured_wrench_ik_base_frame_; +// state_message.measured_wrench_control_frame = measured_wrench_kinematics_base_frame_; // // state_message.admittance_rule_calculated_values = admittance_rule_calculated_values_; // -// state_message.current_pose = current_pose_ik_base_frame_; -// state_message.desired_pose = admittance_pose_ik_base_frame_; +// state_message.current_pose = current_pose_kinematics_base_frame_; +// state_message.desired_pose = admittance_pose_kinematics_base_frame_; // // TODO(destogl): Enable this field for debugging. //// state_message.relative_admittance = sum_of_admittance_displacements_; -// state_message.relative_desired_pose = relative_admittance_pose_ik_base_frame_; +// state_message.relative_desired_pose = relative_admittance_pose_kinematics_base_frame_; return controller_interface::return_type::OK; } - void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench + Eigen::Matrix AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &last_wrench ) { - wrench[0] = filters::exponentialSmoothing( - measured_wrench.force.x, wrench[0], alpha); - wrench[1] = filters::exponentialSmoothing( - measured_wrench.force.y, wrench[1], alpha); - wrench[2] = filters::exponentialSmoothing( - measured_wrench.force.z, wrench[2], alpha); - - wrench[3] = filters::exponentialSmoothing( - measured_wrench.torque.x, wrench[3], alpha); - wrench[4]= filters::exponentialSmoothing( - measured_wrench.torque.y, wrench[4], alpha); - wrench[5] = filters::exponentialSmoothing( - measured_wrench.torque.z, wrench[5], alpha); + Eigen::Matrix new_wrench; + + new_wrench[0] = filters::exponentialSmoothing( + measured_wrench.force.x, last_wrench[0], alpha); + new_wrench[1] = filters::exponentialSmoothing( + measured_wrench.force.y, last_wrench[1], alpha); + new_wrench[2] = filters::exponentialSmoothing( + measured_wrench.force.z, last_wrench[2], alpha); + new_wrench[3] = filters::exponentialSmoothing( + measured_wrench.torque.x, last_wrench[3], alpha); + new_wrench[4] = filters::exponentialSmoothing( + measured_wrench.torque.y, last_wrench[4], alpha); + new_wrench[5] = filters::exponentialSmoothing( + measured_wrench.torque.z, last_wrench[5], alpha); + return new_wrench; + } } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index bd7c55a5b2..42472484c3 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -37,12 +37,6 @@ namespace admittance_controller { AdmittanceController::AdmittanceController() = default; - void AdmittanceController::wrench_stamped_callback(const std::shared_ptr msg) { - if (controller_is_active_) { - rtBuffers.input_wrench_command_.writeFromNonRT(msg); - } - } - void AdmittanceController::joint_command_callback(const std::shared_ptr msg) { if (controller_is_active_) { @@ -51,11 +45,7 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_init() { -// // set chained mode -// set_chained_mode(true); - // initialize RTbuffers - rtBuffers.input_joint_command_.writeFromNonRT(std::shared_ptr()); - rtBuffers.input_wrench_command_.writeFromNonRT(std::shared_ptr()); + // initialize controller parameters admittance_ = std::make_unique(); admittance_->parameters_.initialize(get_node()); @@ -81,19 +71,24 @@ namespace admittance_controller { RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); } for (const auto &tmp: params.chainable_command_interface_types_) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "interface type %s is not supported. Supported types are %s and %s", + tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } } try { admittance_->parameters_.declare_parameters(); } catch (const std::exception &e) { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } num_joints_ = params.joint_names_.size(); - return CallbackReturn::SUCCESS; } @@ -145,7 +140,7 @@ namespace admittance_controller { position_reference_.clear(); velocity_reference_.clear(); - std::unordered_map *> chainable_interface_map = { + std::unordered_map *> chainable_interface_map = { {hardware_interface::HW_IF_POSITION, &position_reference_}, {hardware_interface::HW_IF_VELOCITY, &velocity_reference_}}; @@ -153,10 +148,11 @@ namespace admittance_controller { for (const auto &interface: params.chainable_command_interface_types_) { for (const auto &joint: params.joint_names_) { chainable_interface_map[interface]->push_back(reference_interfaces_.data() + index); - chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(std::string(get_node()->get_name()), - joint + "/" + interface, - reference_interfaces_.data() + - index)); + chainable_command_interfaces.emplace_back( + hardware_interface::CommandInterface(std::string(get_node()->get_name()), + joint + "/" + interface, + reference_interfaces_.data() + + index)); index++; } } @@ -166,24 +162,24 @@ namespace admittance_controller { controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { // update input reference from ros subscriber message - return controller_interface::return_type::OK; //TODO fix this joint_command_msg = *rtBuffers.input_joint_command_.readFromRT(); - for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { - *position_reference_[i] = joint_command_msg->positions[i]; - } - for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { - *velocity_reference_[i] = joint_command_msg->velocities[i]; + + // ensure joint command msg has a message + if (joint_command_msg.get()){ + for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { + *position_reference_[i] = joint_command_msg->positions[i]; + } + for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { + *velocity_reference_[i] = joint_command_msg->velocities[i]; + } } -// for (auto i = 0ul; i < joint_command_msg->accelerations.size(); i++) { -// *velocity_acceleration_[i] = joint_command_msg->accelerations[i]; -// } return controller_interface::return_type::OK; } bool AdmittanceController::on_set_chained_mode(bool chained_mode) { - // this method set chained mode to value of chained_mode if true is returned + // this method sets the chained mode to value of argument chained_mode if true is returned. return true; } @@ -220,9 +216,6 @@ namespace admittance_controller { } // setup subscribers and publishers - input_wrench_command_subscriber_ = get_node()->create_subscription( - "~/ft_data", rclcpp::SystemDefaultsQoS(), - std::bind(&AdmittanceController::wrench_stamped_callback, this, std::placeholders::_1)); input_joint_command_subscriber_ = get_node()->create_subscription( "~/pose_commands", rclcpp::SystemDefaultsQoS(), std::bind(&AdmittanceController::joint_command_callback, this, std::placeholders::_1)); @@ -252,18 +245,19 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { - // on_activate is called when the lifecycle activates. Realtime constraints are required. + // on_activate is called when the lifecycle activates. + controller_is_active_ = true; joint_position_command_interface_.clear(); - joint_velocity_command_interface_.clear(); + joint_velocity_command_interface_.clear(); joint_position_state_interface_.clear(); - joint_velocity_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + // assign state interfaces std::unordered_map> *> command_interface_map = { {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} }; - for (auto i = 0ul; i < params.command_interface_types_.size(); i++) { for (auto j = 0ul; j < params.joint_names_.size(); j++) { command_interface_map[params.command_interface_types_[i]]->emplace_back( @@ -275,7 +269,6 @@ namespace admittance_controller { {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} }; - for (auto i = 0ul; i < params.state_interface_types_.size(); i++) { for (auto j = 0ul; j < params.joint_names_.size(); j++) { state_interface_map[params.state_interface_types_[i]]->emplace_back(state_interfaces_[i * num_joints_ + j]); @@ -284,27 +277,23 @@ namespace admittance_controller { // Initialize interface of the FTS semantic semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + // Initialize Admittance Rule from current states admittance_->reset(); // if in open loop mode, the position state interface should be ignored if (params.open_loop_control_) { -// state_offset_.positions.resize(joint_position_state_interface_.size(), 0.0); -// for (int i=0; i < num_joints_; i++){ -// state_offset_.positions[i] = joint_position_state_interface_[i].get().get_value(); -// } -// open_loop_buffer.resize(joint_position_state_interface_.size(), 0.0); - open_loop_buffer.resize(joint_position_state_interface_.size(), 0.0); for (int i = 0; i < num_joints_; i++) { open_loop_buffer[i] = joint_position_state_interface_[i].get().get_value(); } } - - // Handle state after restart or initial startup + // handle state after restart or initial startup read_state_from_hardware(last_state_reference_); - // if last_state_reference_ is empty, we have no information about the state, assume zero - if (last_state_reference_.positions.empty()) last_state_reference_.positions.resize(num_joints_, 0.0); + // if last_state_reference_ is empty, we have no information about the state, sending a command is dangerous + if (last_state_reference_.positions.empty()){ + return CallbackReturn::ERROR; + } read_state_from_command_interfaces(last_commanded_state_); // if last_commanded_state_ is empty, then our safest option is to set it to the last reference if (last_commanded_state_.positions.empty()) last_commanded_state_.positions = last_state_reference_.positions; @@ -314,8 +303,10 @@ namespace admittance_controller { if (last_state_reference_.accelerations.empty()) last_state_reference_.accelerations.assign(num_joints_, 0.0); if (last_commanded_state_.velocities.empty()) last_commanded_state_.velocities.assign(num_joints_, 0.0); if (last_commanded_state_.accelerations.empty()) last_commanded_state_.accelerations.assign(num_joints_, 0.0); + // initialize state reference state_reference_ = last_state_reference_; + state_desired_ = state_reference_; // if there are no state position interfaces, then force use of open loop control if (joint_position_state_interface_.empty() && !params.open_loop_control_) { @@ -332,7 +323,7 @@ namespace admittance_controller { // Realtime constraints are required in this function // update input reference from chainable interfaces - read_state_reference_from_chainable_interfaces(state_reference_); + read_state_reference_interfaces(state_reference_); // sense: get all controller inputs geometry_msgs::msg::Wrench ft_values; @@ -355,7 +346,7 @@ namespace admittance_controller { // action server out of tolerance for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { joint_position_command_interface_[i].get().set_value(state_desired_.positions[i]); - if(params.open_loop_control_){ + if (params.open_loop_control_) { open_loop_buffer[i] = state_desired_.positions[i]; } last_commanded_state_.positions[i] = state_desired_.positions[i]; @@ -365,7 +356,7 @@ namespace admittance_controller { joint_velocity_command_interface_[i].get().set_value(state_desired_.velocities[i]); last_commanded_state_.velocities[i] = state_desired_.velocities[i]; last_commanded_state_.positions[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! - if(params.open_loop_control_){ + if (params.open_loop_control_) { open_loop_buffer[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! } } @@ -476,14 +467,13 @@ namespace admittance_controller { } } - void AdmittanceController::read_state_reference_from_chainable_interfaces( + void AdmittanceController::read_state_reference_interfaces( trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { // Fill fields of state argument from hardware command interfaces. If the interface does not exist or // the values are nan, that corresponding state field will be set to empty state_reference.positions.resize(position_reference_.size(), 0.0); state_reference.velocities.resize(velocity_reference_.size(), 0.0); - state_reference.accelerations.resize(joint_acceleration_chainable_interface_.size(), 0.0); for (auto i = 0ul; i < position_reference_.size(); i++) { state_reference.positions[i] = *position_reference_[i]; @@ -499,13 +489,6 @@ namespace admittance_controller { break; } } - for (auto i = 0ul; i < joint_acceleration_chainable_interface_.size(); i++) { - state_reference.accelerations[i] = joint_acceleration_chainable_interface_[i].get().get_value(); - if (std::isnan(state_reference.accelerations[i])) { - state_reference.accelerations = last_state_reference_.accelerations; - break; - } - } } From 1563113a3053de2dc57fb3639499c7496f249790 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 21 Jun 2022 15:53:15 -0600 Subject: [PATCH 019/111] removed reshaped operation and fix forward velocity --- .../admittance_controller/admittance_rule.hpp | 23 ++-- .../admittance_rule_impl.hpp | 124 +++++++++--------- 2 files changed, 77 insertions(+), 70 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 21d8f2096d..49f849c58f 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -297,8 +297,7 @@ namespace admittance_controller { AdmittanceParameters parameters_; // Filter parameter for exponential smoothing - const double alpha = 0.1; // TODO make a ros param - + const double alpha = 0.1;//0.005; // TODO make a ros param protected: @@ -306,13 +305,13 @@ namespace admittance_controller { * All values are in the controller frame */ void calculate_admittance_rule( - const Eigen::Matrix &wrench, - const Eigen::Matrix &desired_vel, + const Eigen::Matrix &wrench, + const Eigen::Matrix &desired_vel, const double dt ); - Eigen::Matrix process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix& last_wrench + Eigen::Matrix process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix& last_wrench ); void normalize_rotation(Eigen::Matrix& R); @@ -320,8 +319,10 @@ namespace admittance_controller { Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool & success); Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d& R) const; void convert_cartesian_deltas_to_joint_deltas(const std::vector& positions, - const Eigen::Matrix & cartesian_delta, std::vector& joint_delta, bool & success); - + const Eigen::Matrix & cartesian_delta, std::vector& joint_delta, bool & success); + Eigen::Matrix convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, + const std::vector &joint_delta, + bool &success); // Kinematics interface plugin loader std::shared_ptr> kinematics_loader_; std::unique_ptr kinematics_; @@ -335,8 +336,8 @@ namespace admittance_controller { std::vector cart_buffer_vec; // admittance controller values - Eigen::Matrix admittance_acceleration_; - Eigen::Matrix admittance_velocity_; + Eigen::Matrix admittance_acceleration_; + Eigen::Matrix admittance_velocity_; Eigen::Matrix admittance_position_; // transforms @@ -347,7 +348,7 @@ namespace admittance_controller { Eigen::Matrix cog_transform; // external force - Eigen::Matrix wrench; + Eigen::Matrix wrench_; // position of center of gravity in cog_frame Eigen::Matrix cog_; // force applied to sensor due to weight of end effector diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index d8f52c0fa9..737bfe7f35 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -90,7 +90,7 @@ namespace admittance_controller { admittance_position_.setIdentity(); admittance_velocity_.setZero(); admittance_acceleration_.setZero(); - wrench.setZero(); + wrench_.setZero(); return controller_interface::return_type::OK; } @@ -121,35 +121,33 @@ namespace admittance_controller { auto cur_sensor_rot = cur_sensor_transform.block<3, 3>(0, 0); auto cog_rot = cog_transform.block<3, 3>(0, 0); - // apply filter and update wrench vector - wrench = process_wrench_measurements(measured_wrench, wrench); - - // transform wrench into base frame - Eigen::Matrix wrench_base; - wrench_base.block<3, 1>(0, 0) = cur_sensor_rot * wrench.block<3, 1>(0, 0); - wrench_base.block<3, 1>(3, 0) = cur_sensor_rot * wrench.block<3, 1>(3, 0); + // apply filter and update wrench_ vector + wrench_ = process_wrench_measurements(measured_wrench, wrench_); + // transform wrench_ into base frame + Eigen::Matrix wrench_base = cur_sensor_rot * wrench_; // apply gravity compensation - wrench_base[2] -= ee_weight[2]; - wrench_base.block<3, 1>(3, 0) -= (cog_rot * cog_).cross(ee_weight); + wrench_base(2, 0) -= ee_weight[2]; + wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); + + // transform wrench_ into control frame + Eigen::Matrix wrench_control = cur_control_rot_inv * wrench_base; - // transform wrench into control frame - Eigen::Matrix wrench_control; - wrench_control.block<3, 1>(0, 0) = cur_control_rot_inv * wrench_base.block<3, 1>(0, 0); - wrench_control.block<3, 1>(3, 0) = cur_control_rot_inv * wrench_base.block<3, 1>(3, 0); + // calculate desired cartesian velocity in control frame + Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, current_joint_state.velocities, + success); - // calculate desired velocity in control frame - auto desired_vel = cur_control_rot_inv * admittance_velocity_.reshaped(3, 2) * use_feedforward_commanded_input_*0; // TODO fix this + desired_vel = cur_control_rot_inv * desired_vel * use_feedforward_commanded_input_; //TODO fix this // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - calculate_admittance_rule(wrench_control, desired_vel.reshaped(6, 1), dt); + calculate_admittance_rule(wrench_control, desired_vel, dt); // transform admittance values back to base frame - auto admittance_velocity_base = cur_control_rot * admittance_velocity_.reshaped(3, 2); - convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_base.reshaped(6, 1), + auto admittance_velocity_base = cur_control_rot * admittance_velocity_; + convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_base, joint_vel, success); - auto admittance_acceleration_base = cur_control_rot * admittance_acceleration_.reshaped(3, 2); - convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_base.reshaped(6, 1), + auto admittance_acceleration_base = cur_control_rot * admittance_acceleration_; + convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_base, joint_acc, success); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference @@ -175,8 +173,8 @@ namespace admittance_controller { } void AdmittanceRule::calculate_admittance_rule( - const Eigen::Matrix &wrench, - const Eigen::Matrix &desired_vel, + const Eigen::Matrix &wrench, + const Eigen::Matrix &desired_vel, const double dt ) { // Compute admittance control law: F = M*a + D*v + S*(x - x_d) @@ -184,15 +182,15 @@ namespace admittance_controller { for (size_t axis = 0; axis < 3; ++axis) { if (parameters_.selected_axes_[axis]) { double pose_error = -admittance_position_(axis, 3); - admittance_acceleration_[axis] = (1.0 / parameters_.mass_[axis]) * (wrench[axis] + + admittance_acceleration_(axis, 0) = (1.0 / parameters_.mass_[axis]) * (wrench(axis, 0) + (parameters_.damping_[axis] * - (desired_vel[axis] - - admittance_velocity_[axis])) + + (desired_vel(axis, 0) - + admittance_velocity_(axis, 0))) + (parameters_.stiffness_[axis] * pose_error)); - admittance_velocity_[axis] += - admittance_acceleration_[axis] * dt; - admittance_position_(axis, 3) += admittance_velocity_[axis] * dt; + admittance_velocity_(axis, 0) += + admittance_acceleration_(axis, 0) * dt; + admittance_position_(axis, 3) += admittance_velocity_(axis, 0) * dt; } } @@ -205,24 +203,24 @@ namespace admittance_controller { + V[2] * (R(0, 1) - R(1, 0)); double sign = (tmp >= 0) ? 1.0 : -1.0; - for (size_t axis = 3; axis < 6; ++axis) { - if (parameters_.selected_axes_[axis]) { - double pose_error = sign * theta * V(axis - 3); - admittance_acceleration_[axis] = (1.0 / parameters_.mass_[axis]) * (wrench[axis] + - (parameters_.damping_[axis] * - (desired_vel[axis] - - admittance_velocity_[axis])) + - (parameters_.stiffness_[axis] * + for (size_t axis = 0; axis < 3; ++axis) { + if (parameters_.selected_axes_[axis+3]) { + double pose_error = sign * theta * V(axis); + admittance_acceleration_(axis, 1) = (1.0 / parameters_.mass_[axis+3]) * (wrench(axis, 1) + + (parameters_.damping_[axis+3] * + (desired_vel(axis, 1) - + admittance_velocity_(axis, 1))) + + (parameters_.stiffness_[axis+3] * pose_error)); - admittance_velocity_[axis] += - admittance_acceleration_[axis] * dt; + admittance_velocity_(axis, 1) += + admittance_acceleration_(axis, 1) * dt; } } Eigen::Matrix3d skew_symmetric; - skew_symmetric << 0, -admittance_velocity_[2 + 3], admittance_velocity_[1 + 3], - admittance_velocity_[2 + 3], 0, -admittance_velocity_[0 + 3], - -admittance_velocity_[1 + 3], admittance_velocity_[0 + 3], 0; + skew_symmetric << 0, -admittance_velocity_(2, 1), admittance_velocity_(1, 1), + admittance_velocity_(2, 1), 0, -admittance_velocity_(0, 1), + -admittance_velocity_(1, 1), admittance_velocity_(0, 1), 0; Eigen::Matrix3d R_dot = skew_symmetric * R; R += R_dot * dt; @@ -335,12 +333,20 @@ namespace admittance_controller { } void AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, - const Eigen::Matrix &cartesian_delta, + const Eigen::Matrix &cartesian_delta, std::vector &joint_delta, bool &success) { memcpy(cart_buffer_vec.data(), cartesian_delta.data(), 6 * sizeof(double)); success &= kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec, joint_delta); } + Eigen::Matrix AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, + const std::vector &joint_delta, + bool &success) { + Eigen::Matrix cartesian_delta; + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, cart_buffer_vec); + memcpy(cartesian_delta.data(), cart_buffer_vec.data(), 6 * sizeof(double)); + } + controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState &state_message) { // TODO update this @@ -361,23 +367,23 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - Eigen::Matrix AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &last_wrench + Eigen::Matrix AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &last_wrench ) { - Eigen::Matrix new_wrench; - - new_wrench[0] = filters::exponentialSmoothing( - measured_wrench.force.x, last_wrench[0], alpha); - new_wrench[1] = filters::exponentialSmoothing( - measured_wrench.force.y, last_wrench[1], alpha); - new_wrench[2] = filters::exponentialSmoothing( - measured_wrench.force.z, last_wrench[2], alpha); - new_wrench[3] = filters::exponentialSmoothing( - measured_wrench.torque.x, last_wrench[3], alpha); - new_wrench[4] = filters::exponentialSmoothing( - measured_wrench.torque.y, last_wrench[4], alpha); - new_wrench[5] = filters::exponentialSmoothing( - measured_wrench.torque.z, last_wrench[5], alpha); + Eigen::Matrix new_wrench; + + new_wrench(0,0) = filters::exponentialSmoothing( + measured_wrench.force.x, last_wrench(0,0), alpha); + new_wrench(1,0) = filters::exponentialSmoothing( + measured_wrench.force.y, last_wrench(1,0), alpha); + new_wrench(2,0) = filters::exponentialSmoothing( + measured_wrench.force.z, last_wrench(2,0), alpha); + new_wrench(0,1) = filters::exponentialSmoothing( + measured_wrench.torque.x, last_wrench(0,1), alpha); + new_wrench(1,1) = filters::exponentialSmoothing( + measured_wrench.torque.y, last_wrench(1,1), alpha); + new_wrench(2,1) = filters::exponentialSmoothing( + measured_wrench.torque.z, last_wrench(2,1), alpha); return new_wrench; } From 5d5942710f8c5d1147797c446c53f847de07fbbc Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 22 Jun 2022 12:40:46 -0600 Subject: [PATCH 020/111] code cleanup and refactored gravity compensation --- .../admittance_controller.hpp | 90 +++++---- .../admittance_controller/admittance_rule.hpp | 6 +- .../admittance_rule_impl.hpp | 51 +++-- .../src/admittance_controller.cpp | 178 ++++++++---------- 4 files changed, 152 insertions(+), 173 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index f804b18b2f..1f8fc07657 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -46,34 +46,27 @@ using namespace std::chrono_literals; -namespace admittance_controller -{ - using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - struct RTBuffers{ - realtime_tools::RealtimeBuffer> input_joint_command_; - std::unique_ptr> state_publisher_; - }; - - struct ParameterStruct{ - std::vector joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - std::vector chainable_command_interface_types_; - std::string ft_sensor_name_; - bool use_joint_commands_as_input_; - std::string joint_limiter_type_; - bool allow_partial_joints_goal_; - bool allow_integration_in_goal_trajectories_; - double action_monitor_rate_; - bool open_loop_control_; - }; - - -class AdmittanceController : public controller_interface::ChainableControllerInterface -{ -public: +namespace admittance_controller { + using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + struct RTBuffers { + realtime_tools::RealtimeBuffer> input_joint_command_; + std::unique_ptr> state_publisher_; + }; + + struct ParameterStruct { + std::vector joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + std::vector chainable_command_interface_types_; + std::string ft_sensor_name_; + std::string joint_limiter_type_; + }; + + + class AdmittanceController : public controller_interface::ChainableControllerInterface { + public: ADMITTANCE_CONTROLLER_PUBLIC AdmittanceController(); @@ -87,28 +80,30 @@ class AdmittanceController : public controller_interface::ChainableControllerInt controller_interface::InterfaceConfiguration state_interface_configuration() const override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + const rclcpp::Time &time, const rclcpp::Duration &period) override; -protected: + protected: std::vector on_export_reference_interfaces() override; - controller_interface::return_type update_reference_from_subscribers() override; + + controller_interface::return_type update_reference_from_subscribers() override; + bool on_set_chained_mode(bool chained_mode) override; int num_joints_{}; @@ -121,8 +116,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt std::vector> joint_velocity_state_interface_; std::vector> joint_acceleration_state_interface_; - std::vector position_reference_; - std::vector velocity_reference_; + std::vector position_reference_; + std::vector velocity_reference_; // Admittance rule and dependent variables; std::unique_ptr admittance_; @@ -132,7 +127,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt ParameterStruct params; // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; - rclcpp::Publisher::SharedPtr s_publisher_ = nullptr; + rclcpp::Publisher::SharedPtr s_publisher_ = nullptr; // ROS messages std::shared_ptr joint_command_msg; // real-time buffer @@ -141,8 +136,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // controller running state bool controller_is_active_{}; const std::set allowed_state_interface_types_ = { - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, }; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; trajectory_msgs::msg::JointTrajectoryPoint last_state_reference_; @@ -150,17 +145,20 @@ class AdmittanceController : public controller_interface::ChainableControllerInt trajectory_msgs::msg::JointTrajectoryPoint prev_trajectory_point_; // control loop data trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, - state_error_; + state_error_; trajectory_msgs::msg::JointTrajectory pre_admittance_point; std::vector open_loop_buffer; // helper methods void joint_command_callback(const std::shared_ptr msg); - void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state); - void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); - void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); -}; + void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); + + void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); + + void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); + + }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 49f849c58f..33d2451075 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -297,7 +297,7 @@ namespace admittance_controller { AdmittanceParameters parameters_; // Filter parameter for exponential smoothing - const double alpha = 0.1;//0.005; // TODO make a ros param + const double alpha = 0.005; // TODO make a ros param protected: @@ -310,8 +310,8 @@ namespace admittance_controller { const double dt ); - Eigen::Matrix process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix& last_wrench + void process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix& sensor_rot, const Eigen::Matrix& cog_rot ); void normalize_rotation(Eigen::Matrix& R); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 737bfe7f35..82e18e0849 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -122,16 +122,10 @@ namespace admittance_controller { auto cog_rot = cog_transform.block<3, 3>(0, 0); // apply filter and update wrench_ vector - wrench_ = process_wrench_measurements(measured_wrench, wrench_); - - // transform wrench_ into base frame - Eigen::Matrix wrench_base = cur_sensor_rot * wrench_; - // apply gravity compensation - wrench_base(2, 0) -= ee_weight[2]; - wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); + process_wrench_measurements(measured_wrench, cur_sensor_rot, cog_rot); // transform wrench_ into control frame - Eigen::Matrix wrench_control = cur_control_rot_inv * wrench_base; + Eigen::Matrix wrench_control = cur_control_rot_inv * wrench_; // calculate desired cartesian velocity in control frame Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, current_joint_state.velocities, @@ -345,6 +339,7 @@ namespace admittance_controller { Eigen::Matrix cartesian_delta; success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, cart_buffer_vec); memcpy(cartesian_delta.data(), cart_buffer_vec.data(), 6 * sizeof(double)); + return cartesian_delta; } controller_interface::return_type AdmittanceRule::get_controller_state( @@ -367,26 +362,30 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - Eigen::Matrix AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &last_wrench + void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &cur_sensor_rot, const Eigen::Matrix& cog_rot ) { - Eigen::Matrix new_wrench; - - new_wrench(0,0) = filters::exponentialSmoothing( - measured_wrench.force.x, last_wrench(0,0), alpha); - new_wrench(1,0) = filters::exponentialSmoothing( - measured_wrench.force.y, last_wrench(1,0), alpha); - new_wrench(2,0) = filters::exponentialSmoothing( - measured_wrench.force.z, last_wrench(2,0), alpha); - new_wrench(0,1) = filters::exponentialSmoothing( - measured_wrench.torque.x, last_wrench(0,1), alpha); - new_wrench(1,1) = filters::exponentialSmoothing( - measured_wrench.torque.y, last_wrench(1,1), alpha); - new_wrench(2,1) = filters::exponentialSmoothing( - measured_wrench.torque.z, last_wrench(2,1), alpha); - return new_wrench; + Eigen::Matrix new_wrench; + new_wrench(0,0) = measured_wrench.force.x; + new_wrench(1,0) = measured_wrench.force.y; + new_wrench(2,0) = measured_wrench.force.z; + new_wrench(0,1) = measured_wrench.torque.x; + new_wrench(1,1) = measured_wrench.torque.y; + new_wrench(2,1) = measured_wrench.torque.z; - } + // transform to base frame + Eigen::Matrix new_wrench_base = cur_sensor_rot * new_wrench; + + // apply gravity compensation + new_wrench_base(2, 0) -= ee_weight[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); + + for (auto i = 0; i < 6; i++){ + wrench_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_(i), alpha); + } + + } } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 42472484c3..5266e53b8e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -56,12 +56,7 @@ namespace admittance_controller { get_node()->get_parameter("state_interfaces").as_string_array(), get_node()->get_parameter("chainable_command_interfaces").as_string_array(), get_node()->get_parameter("ft_sensor_name").as_string(), - get_node()->get_parameter("use_joint_commands_as_input").as_bool(), get_node()->get_parameter("joint_limiter_type").as_string(), - get_node()->get_parameter("allow_partial_joints_goal").as_bool(), - get_node()->get_parameter("allow_integration_in_goal_trajectories").as_bool(), - get_node()->get_parameter("action_monitor_rate").as_double(), - get_node()->get_parameter("open_loop_control").as_bool(), }; for (const auto &tmp: params.state_interface_types_) { @@ -152,8 +147,7 @@ namespace admittance_controller { hardware_interface::CommandInterface(std::string(get_node()->get_name()), joint + "/" + interface, reference_interfaces_.data() + - index)); - index++; + index++)); } } @@ -165,8 +159,8 @@ namespace admittance_controller { joint_command_msg = *rtBuffers.input_joint_command_.readFromRT(); - // ensure joint command msg has a message - if (joint_command_msg.get()){ + // if message exists, load values into references + if (joint_command_msg.get()) { for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { *position_reference_[i] = joint_command_msg->positions[i]; } @@ -206,15 +200,6 @@ namespace admittance_controller { get_interface_list(params.command_interface_types_).c_str(), get_interface_list(params.state_interface_types_).c_str()); - RCLCPP_INFO(get_node()->get_logger(), "Action status changes will be monitored at %.2f Hz.", - params.action_monitor_rate_); - if (params.use_joint_commands_as_input_) { - RCLCPP_INFO(get_node()->get_logger(), "Using Joint input mode."); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "Admittance controller does not support non-joint input modes."); - return CallbackReturn::ERROR; - } - // setup subscribers and publishers input_joint_command_subscriber_ = get_node()->create_subscription( "~/pose_commands", rclcpp::SystemDefaultsQoS(), @@ -230,6 +215,7 @@ namespace admittance_controller { rtBuffers.state_publisher_->msg_.desired_joint_state.positions.resize(num_joints_, 0.0); rtBuffers.state_publisher_->msg_.error_joint_state.positions.resize(num_joints_, 0.0); rtBuffers.state_publisher_->unlock(); + // Initialize FTS semantic semantic_component force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor(params.ft_sensor_name_)); @@ -245,9 +231,10 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { - // on_activate is called when the lifecycle activates. - + // on_activate is called when the lifecycle node activates. controller_is_active_ = true; + + // clear out vectors in case of restart joint_position_command_interface_.clear(); joint_velocity_command_interface_.clear(); joint_position_state_interface_.clear(); @@ -264,7 +251,7 @@ namespace admittance_controller { command_interfaces_[i * num_joints_ + j]); } } - + // assign command interfaces std::unordered_map> *> state_interface_map = { {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} @@ -275,47 +262,46 @@ namespace admittance_controller { } } - // Initialize interface of the FTS semantic semantic component + // if there are no state position interfaces, then closed loop control cannot be used + if (joint_position_state_interface_.empty() && !admittance_->parameters_.open_loop_control_) { + RCLCPP_ERROR(get_node()->get_logger(), + "Open loop control set to false but no position state interfaces were provided. Please add a position state interface"); + return CallbackReturn::ERROR; + } + + // initialize interface of the FTS semantic semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); // Initialize Admittance Rule from current states admittance_->reset(); - // if in open loop mode, the position state interface should be ignored - if (params.open_loop_control_) { - open_loop_buffer.resize(joint_position_state_interface_.size(), 0.0); - for (int i = 0; i < num_joints_; i++) { - open_loop_buffer[i] = joint_position_state_interface_[i].get().get_value(); - } - } // handle state after restart or initial startup read_state_from_hardware(last_state_reference_); - // if last_state_reference_ is empty, we have no information about the state, sending a command is dangerous - if (last_state_reference_.positions.empty()){ + + // if last_state_reference_ positions is empty, we have no information about the state, sending a command is dangerous + if (last_state_reference_.positions.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), + "Failed to read from position from hardware state interface. Please ensure that a position state interface exist."); return CallbackReturn::ERROR; } - read_state_from_command_interfaces(last_commanded_state_); - // if last_commanded_state_ is empty, then our safest option is to set it to the last reference - if (last_commanded_state_.positions.empty()) last_commanded_state_.positions = last_state_reference_.positions; + + // if in open loop mode, the position state interface should be ignored + if (admittance_->parameters_.open_loop_control_) { + open_loop_buffer = last_state_reference_.positions; + } // reset dynamic fields in case non-zero - if (last_state_reference_.velocities.empty()) last_state_reference_.velocities.assign(num_joints_, 0.0); - if (last_state_reference_.accelerations.empty()) last_state_reference_.accelerations.assign(num_joints_, 0.0); - if (last_commanded_state_.velocities.empty()) last_commanded_state_.velocities.assign(num_joints_, 0.0); - if (last_commanded_state_.accelerations.empty()) last_commanded_state_.accelerations.assign(num_joints_, 0.0); + last_state_reference_.velocities.assign(num_joints_, 0.0); + last_state_reference_.accelerations.assign(num_joints_, 0.0); + + // initialize last_commanded_state_ last read state reference + last_commanded_state_ = last_state_reference_; // initialize state reference state_reference_ = last_state_reference_; - state_desired_ = state_reference_; + state_desired_ = state_reference_; - // if there are no state position interfaces, then force use of open loop control - if (joint_position_state_interface_.empty() && !params.open_loop_control_) { - params.open_loop_control_ = true; - RCLCPP_INFO(get_node()->get_logger(), - "control loop control set to true because no position state interface was provided."); - } - - return CallbackReturn::SUCCESS;; + return CallbackReturn::SUCCESS; } controller_interface::return_type @@ -330,34 +316,28 @@ namespace admittance_controller { force_torque_sensor_->get_values_as_message(ft_values); read_state_from_hardware(state_current_); - // if values are not available, assume that the current state is the last commanded - if (state_current_.positions.empty()) { state_current_.positions = last_commanded_state_.positions; } - if (state_current_.velocities.empty()) { state_current_.velocities = last_commanded_state_.velocities; } - if (state_current_.accelerations.empty()) { state_current_.accelerations = last_commanded_state_.accelerations; } - - // command: determine desired state from trajectory or pose goal // and apply admittance controller admittance_->update(state_current_, ft_values, state_reference_, period, state_desired_); // write calculated values to joint interfaces - // at goal time (end of trajectory), check goal reference error and send fail to - // action server out of tolerance for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { joint_position_command_interface_[i].get().set_value(state_desired_.positions[i]); - if (params.open_loop_control_) { + // if in open loop, feed the commanded state back into the input (open_loop_buffer) + if (admittance_->parameters_.open_loop_control_) { open_loop_buffer[i] = state_desired_.positions[i]; } last_commanded_state_.positions[i] = state_desired_.positions[i]; } + double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { - double dt = 1.0 / 60; // hack! joint_velocity_command_interface_[i].get().set_value(state_desired_.velocities[i]); last_commanded_state_.velocities[i] = state_desired_.velocities[i]; - last_commanded_state_.positions[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! - if (params.open_loop_control_) { + // if in open loop and there are no position command interfaces, + if (admittance_->parameters_.open_loop_control_ && joint_position_command_interface_.empty()) { open_loop_buffer[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! + last_commanded_state_.positions[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! } } for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { @@ -398,7 +378,7 @@ namespace admittance_controller { void AdmittanceController::read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint &state_current) { - // Fill fields of state argument from hardware state interfaces. If the hardware does not exist, + // Fill fields of state_current argument from hardware state interfaces. If the hardware does not exist, // the values are nan, that corresponding state field will be set to empty. If running in open loop // control, all states fields will be empty @@ -407,70 +387,72 @@ namespace admittance_controller { state_current.accelerations.resize(joint_acceleration_state_interface_.size()); // fill state message with values from hardware state interfaces + // if any interface has nan values, assume state_current is the last command state for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { - if (params.open_loop_control_) { + if (admittance_->parameters_.open_loop_control_) { state_current.positions[i] = open_loop_buffer[i]; } else { state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); } if (std::isnan(state_current.positions[i])) { - state_current.positions.clear(); + state_current.positions = last_commanded_state_.positions; break; } } for (auto i = 0ul; i < joint_velocity_state_interface_.size(); i++) { state_current.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); if (std::isnan(state_current.velocities[i])) { - state_current.velocities.clear(); + state_current.velocities = last_commanded_state_.velocities; break; } } for (auto i = 0ul; i < joint_acceleration_state_interface_.size(); i++) { state_current.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); if (std::isnan(state_current.accelerations[i])) { - state_current.accelerations.clear(); + state_current.accelerations = last_commanded_state_.accelerations; break; } } - } - - void AdmittanceController::read_state_from_command_interfaces( - trajectory_msgs::msg::JointTrajectoryPoint &commanded_state) { - // Fill fields of state argument from hardware command interfaces. If the interface does not exist or - // the values are nan, that corresponding state field will be set to empty - - commanded_state.positions.resize(joint_position_command_interface_.size(), 0.0); - commanded_state.velocities.resize(joint_velocity_command_interface_.size(), 0.0); - commanded_state.accelerations.resize(joint_acceleration_command_interface_.size(), 0.0); - // fill state message with values from hardware command interfaces - for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { - commanded_state.positions[i] = joint_position_command_interface_[i].get().get_value(); - if (std::isnan(commanded_state.positions[i])) { - commanded_state.positions.clear(); - break; - } - } - for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { - commanded_state.velocities[i] = joint_velocity_command_interface_[i].get().get_value(); - if (std::isnan(commanded_state.velocities[i])) { - commanded_state.velocities.clear(); - break; - } - } - for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { - commanded_state.accelerations[i] = joint_acceleration_command_interface_[i].get().get_value(); - if (std::isnan(commanded_state.accelerations[i])) { - commanded_state.accelerations.clear(); - break; - } - } } +// void AdmittanceController::read_state_from_command_interfaces( +// trajectory_msgs::msg::JointTrajectoryPoint &commanded_state) { +// // Fill fields of commanded_state argument from hardware command interfaces. If the interface does not exist or +// // the values are nan, that corresponding state field will be set to empty +// +// commanded_state.positions.resize(joint_position_command_interface_.size(), 0.0); +// commanded_state.velocities.resize(joint_velocity_command_interface_.size(), 0.0); +// commanded_state.accelerations.resize(joint_acceleration_command_interface_.size(), 0.0); +// +// // fill state message with values from hardware command interfaces +// for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { +// commanded_state.positions[i] = joint_position_command_interface_[i].get().get_value(); +// if (std::isnan(commanded_state.positions[i])) { +// commanded_state.positions.clear(); +// break; +// } +// } +// for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { +// commanded_state.velocities[i] = joint_velocity_command_interface_[i].get().get_value(); +// if (std::isnan(commanded_state.velocities[i])) { +// commanded_state.velocities.clear(); +// break; +// } +// } +// for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { +// commanded_state.accelerations[i] = joint_acceleration_command_interface_[i].get().get_value(); +// if (std::isnan(commanded_state.accelerations[i])) { +// commanded_state.accelerations.clear(); +// break; +// } +// } +// } + void AdmittanceController::read_state_reference_interfaces( trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { - // Fill fields of state argument from hardware command interfaces. If the interface does not exist or - // the values are nan, that corresponding state field will be set to empty + // Fill fields of state_reference argument from controller references. If the interface does not exist or + // the values are nan, that corresponding field will be set to empty state_reference.positions.resize(position_reference_.size(), 0.0); state_reference.velocities.resize(velocity_reference_.size(), 0.0); From 18097a2cbdfff6a26973090d5c3be5a210f1479f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 24 Jun 2022 18:38:53 -0600 Subject: [PATCH 021/111] replaced parameter handler with auto-generated struct --- admittance_controller/CMakeLists.txt | 20 +- .../admittance_controller.hpp | 6 +- .../admittance_controller/admittance_rule.hpp | 264 ++---------- .../admittance_rule_impl.hpp | 220 +++++++--- .../config/admittance_controller.yaml | 93 +++++ .../config/admittance_struct.h | 278 +++++++++++++ .../parameter_handler.hpp | 388 ------------------ admittance_controller/package.xml | 1 + .../src/admittance_controller.cpp | 52 +-- .../src/parameter_handler.cpp | 216 ---------- 10 files changed, 593 insertions(+), 945 deletions(-) create mode 100644 admittance_controller/include/admittance_controller/config/admittance_controller.yaml create mode 100644 admittance_controller/include/admittance_controller/config/admittance_struct.h delete mode 100644 admittance_controller/include/admittance_controller/parameter_handler.hpp delete mode 100644 admittance_controller/src/parameter_handler.cpp diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 089d9a6dcb..294dd0a839 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -31,14 +31,26 @@ find_package(tf2_ros REQUIRED) find_package(trajectory_msgs REQUIRED) find_package(angles REQUIRED) find_package(rcutils REQUIRED) - +find_package(tf2_kdl REQUIRED) +find_package(gen_param_struct REQUIRED) # The admittance controller add_library(admittance_controller SHARED src/admittance_controller.cpp - src/parameter_handler.cpp ) + +# specify which yaml root name to convert to struct +set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config/admittance_controller.yaml) +# specify which yaml root name to convert to struct +set(YAML_TARGET admittance_struct) +# specify which yaml root name to convert to struct +set(OUT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config) + +generate_param_struct_header(admittance_controller ${OUT_DIR} ${YAML_FILE} ${YAML_TARGET}) + + + target_include_directories( admittance_controller PRIVATE @@ -52,7 +64,8 @@ ament_target_dependencies( control_toolbox controller_interface kinematics_interface - ${Eigen_LIBRARIES} +# ${Eigen_LIBRARIES} + Eigen3 geometry_msgs hardware_interface joint_trajectory_controller @@ -62,6 +75,7 @@ ament_target_dependencies( realtime_tools tf2 tf2_eigen + tf2_kdl tf2_geometry_msgs tf2_ros angles diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 1f8fc07657..b0467f08ca 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -60,8 +60,6 @@ namespace admittance_controller { std::vector command_interface_types_; std::vector state_interface_types_; std::vector chainable_command_interface_types_; - std::string ft_sensor_name_; - std::string joint_limiter_type_; }; @@ -123,7 +121,7 @@ namespace admittance_controller { std::unique_ptr admittance_; // joint limiter TODO std::unique_ptr force_torque_sensor_; - // controller parameters filled by ROS + // controller config filled by ROS ParameterStruct params; // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; @@ -154,7 +152,7 @@ namespace admittance_controller { void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); - void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); +// void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 33d2451075..86c46a53cb 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -20,17 +20,22 @@ #include #include +#include +#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include +#include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" -#include "admittance_controller/parameter_handler.hpp" #include "controller_interface/controller_interface.hpp" #include "control_toolbox/filters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" - +#include "config/admittance_struct.h" // kinematics plugins #include "kinematics_interface/kinematics_interface_base.hpp" #include "pluginlib/class_loader.hpp" + namespace { // Utility namespace // Numerical accuracy checks. Used as deadbands. @@ -44,222 +49,6 @@ namespace { // Utility namespace namespace admittance_controller { - class AdmittanceParameters : public control_toolbox::ParameterHandler { - public: - AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 28, 7) { - add_string_parameter("kinematics.base", false); - add_string_parameter("kinematics.group_name", false); - add_string_parameter("kinematics.plugin_name", false); - add_string_parameter("control_frame", true); - add_string_parameter("sensor_frame", false); - add_string_parameter("end_effector_name", false); - add_string_parameter("CoG.frame", false); - - add_bool_parameter("open_loop_control", true); - add_bool_parameter("enable_parameter_update_without_reactivation", false); - - add_bool_parameter("admittance.selected_axes.x", true); - add_bool_parameter("admittance.selected_axes.y", true); - add_bool_parameter("admittance.selected_axes.z", true); - add_bool_parameter("admittance.selected_axes.rx", true); - add_bool_parameter("admittance.selected_axes.ry", true); - add_bool_parameter("admittance.selected_axes.rz", true); - - add_double_parameter("admittance.mass.x", true); - add_double_parameter("admittance.mass.y", true); - add_double_parameter("admittance.mass.z", true); - add_double_parameter("admittance.mass.rx", true); - add_double_parameter("admittance.mass.ry", true); - add_double_parameter("admittance.mass.rz", true); - add_double_parameter("admittance.stiffness.x", true); - add_double_parameter("admittance.stiffness.y", true); - add_double_parameter("admittance.stiffness.z", true); - add_double_parameter("admittance.stiffness.rx", true); - add_double_parameter("admittance.stiffness.ry", true); - add_double_parameter("admittance.stiffness.rz", true); - add_double_parameter("admittance.damping.x", true); - add_double_parameter("admittance.damping.y", true); - add_double_parameter("admittance.damping.z", true); - add_double_parameter("admittance.damping.rx", true); - add_double_parameter("admittance.damping.ry", true); - add_double_parameter("admittance.damping.rz", true); - add_double_parameter("admittance.damping_ratio.x", true); - add_double_parameter("admittance.damping_ratio.y", true); - add_double_parameter("admittance.damping_ratio.z", true); - add_double_parameter("admittance.damping_ratio.rx", true); - add_double_parameter("admittance.damping_ratio.ry", true); - add_double_parameter("admittance.damping_ratio.rz", true); - - add_double_parameter("CoG.x", false); - add_double_parameter("CoG.y", false); - add_double_parameter("CoG.z", false); - add_double_parameter("CoG.force", false); - } - - bool check_if_parameters_are_valid() override { - bool ret = true; - - // Check if any string parameter is empty - ret = !empty_parameter_in_list(string_parameters_); - - int index = 0; - int offset_index_bool = 2; - // check if parameters are all properly set for selected axes - for (size_t i = 0; i < 6; ++i) { - if (bool_parameters_[offset_index_bool + i].second) { - // check mass parameters - index = i; - if (std::isnan(double_parameters_[index].second)) { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), - "Parameter '%s' has to be set", double_parameters_[index].first.name.c_str()); - ret = false; - } - // Check stiffness parameters - index = i + 6; - if (std::isnan(double_parameters_[index].second)) { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), - "Parameter '%s' has to be set", double_parameters_[index].first.name.c_str()); - ret = false; - } - // Check damping or damping_ratio parameters - index = i + 12; - if (std::isnan(double_parameters_[index].second) && - std::isnan(double_parameters_[index + 6].second)) { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), - "Either parameter '%s' of '%s' has to be set", - double_parameters_[index].first.name.c_str(), - double_parameters_[index + 6].first.name.c_str() - ); - ret = false; - } - } - } - - return ret; - } - - /** - * Conversion to damping when damping_ratio (zeta) parameter is used. - * Using formula: D = damping_ratio * 2 * sqrt( M * S ) - */ - void convert_damping_ratio_to_damping() { - for (auto i = 0ul; i < damping_ratio_.size(); ++i) { - if (!std::isnan(damping_ratio_[i])) { - damping_[i] = damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); - } else { - RCUTILS_LOG_DEBUG_NAMED( - logger_name_.c_str(), - "Damping ratio for axis %zu not used because it is NaN.", i); - } - } - } - - void update_storage() override { - kinematics_base_frame_ = string_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "IK Base frame: %s", kinematics_base_frame_.c_str()); - kinematics_group_name_ = string_parameters_[1].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "IK group name frame: %s", kinematics_group_name_.c_str()); - kinematics_plugin_name_ = string_parameters_[2].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "IK plugin name: %s", kinematics_plugin_name_.c_str()); - control_frame_ = string_parameters_[3].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Control frame: %s", control_frame_.c_str()); - sensor_frame_ = string_parameters_[4].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Sensor frame: %s", sensor_frame_.c_str()); - end_effector_name_ = string_parameters_[5].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "end effector name: %s", sensor_frame_.c_str()); - cog_frame_ = string_parameters_[6].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "COG frame: %s", cog_frame_.c_str()); - - open_loop_control_ = bool_parameters_[0].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Using open loop: %s", (open_loop_control_ ? "True" : "False")); - enable_parameter_update_without_reactivation_ = bool_parameters_[1].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Using update without reactivation: %s", (enable_parameter_update_without_reactivation_ ? "True" : "False")); - - - int offset_index_bool = 2; // 2 because there are already two parameters used above - for (size_t i = 0; i < 6; ++i) { - selected_axes_[i] = bool_parameters_[i + offset_index_bool].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Axis %zu is %sselected", i, (selected_axes_[i] ? "" : "not ")); - - mass_[i] = double_parameters_[i].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Mass for the axis %zu is %e", i, mass_[i]); - stiffness_[i] = double_parameters_[i + 6].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Stiffness for the axis %zu is %e", i, stiffness_[i]); - damping_[i] = double_parameters_[i + 12].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Damping for the axis %zu is %e", i, damping_[i]); - damping_ratio_[i] = double_parameters_[i + 18].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Damping_ratio for the axis %zu is %e", i, damping_ratio_[i]); - } - convert_damping_ratio_to_damping(); - - for (int i = 0; i < 3; i++){ - cog_[i] = double_parameters_[24 + i].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "Damping_ratio for the axis %zu is %e", i, cog_[i]); - } - force_ = double_parameters_[27].second; - RCUTILS_LOG_INFO_NAMED( - logger_name_.c_str(), - "CoG force %e", force_); - } - - // IK parameters - std::string kinematics_base_frame_; - std::string kinematics_group_name_; - std::string kinematics_plugin_name_; - std::string end_effector_name_; - // Depends on the scenario: usually base_link, tool or end-effector - std::string control_frame_; - // Admittance calculations (displacement etc) are done in this frame. - // Frame where wrench measurements are taken - std::string sensor_frame_; - std::string cog_frame_; - - bool open_loop_control_; - bool enable_parameter_update_without_reactivation_; - - std::array damping_; - std::array damping_ratio_; - std::array cog_; - std::array mass_; - std::array selected_axes_; - std::array stiffness_; - double force_; - }; - - class AdmittanceRule { public: AdmittanceRule() = default; @@ -283,18 +72,16 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states); - controller_interface::return_type get_controller_state( - control_msgs::msg::AdmittanceControllerState &state_message - ); + control_msgs::msg::AdmittanceControllerState + get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message); public: // TODO(destogl): Add parameter for this bool use_feedforward_commanded_input_ = true; - - // Dynamic admittance parameters - AdmittanceParameters parameters_; + // Dynamic admittance config + std::shared_ptr parameters_; // Filter parameter for exponential smoothing const double alpha = 0.005; // TODO make a ros param @@ -314,15 +101,19 @@ namespace admittance_controller { const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix& sensor_rot, const Eigen::Matrix& cog_rot ); - void normalize_rotation(Eigen::Matrix& R); - Eigen::Matrix invert_transform(Eigen::Matrix &T); - Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool & success); Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d& R) const; void convert_cartesian_deltas_to_joint_deltas(const std::vector& positions, const Eigen::Matrix & cartesian_delta, std::vector& joint_delta, bool & success); Eigen::Matrix convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, const std::vector &joint_delta, bool &success); + void normalize_rotation(Eigen::Matrix& R); + Eigen::Matrix invert_transform(Eigen::Matrix &T); + Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool & success); + void eigen_to_msg(const Eigen::Matrix& wrench, const std::string& frame_id, geometry_msgs::msg::WrenchStamped& wrench_msg); + Eigen::Matrix + find_transform(const std::string &target_frame, const std::string &source_frame, bool &success); + // Kinematics interface plugin loader std::shared_ptr> kinematics_loader_; std::unique_ptr kinematics_; @@ -341,11 +132,12 @@ namespace admittance_controller { Eigen::Matrix admittance_position_; // transforms - Eigen::Matrix cur_ee_transform; + Eigen::Matrix ee_transform; Eigen::Matrix reference_ee_transform; - Eigen::Matrix cur_sensor_transform; - Eigen::Matrix cur_control_transform; + Eigen::Matrix sensor_transform; + Eigen::Matrix control_transform; Eigen::Matrix cog_transform; + Eigen::Matrix base_link_transform_; // external force Eigen::Matrix wrench_; @@ -359,9 +151,17 @@ namespace admittance_controller { std::vector joint_acc; std::vector joint_pos; - trajectory_msgs::msg::JointTrajectoryPoint admittance_rule_calculated_values_; + std::vector damping_; + std::vector mass_; + std::vector selected_axes_; + std::vector stiffness_; - private: + // ROS + trajectory_msgs::msg::JointTrajectoryPoint admittance_rule_calculated_values_; + control_msgs::msg::AdmittanceControllerState state_message_; + std::shared_ptr clock_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 82e18e0849..5875b1c794 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -17,11 +17,11 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ +#include #include "admittance_controller/admittance_rule.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" -#include "tf2_eigen/tf2_eigen.hpp" namespace admittance_controller { @@ -31,30 +31,30 @@ namespace admittance_controller { num_joints_ = num_joints; + // initialize ROS functions + clock_ = node->get_clock(); + tf_buffer_ = std::make_shared(clock_); + tf_listener_ = std::make_shared(*tf_buffer_); + // initialize memory and values to zero (non-realtime function) reset(); - // set parameters values - memcpy(cog_.data(), parameters_.cog_.data(), 3 * sizeof(double)); - ee_weight.setZero(); - ee_weight[2] = -parameters_.force_; - // Load the differential IK plugin - if (!parameters_.kinematics_plugin_name_.empty()) { + if (!parameters_->kinematics_.plugin_name_.empty()) { try { // TODO(destogl): add "kinematics_interface" into separate package and then rename the package in // the next line from "admittance_controller" to "kinematics_base_plugin" kinematics_loader_ = std::make_shared>( "kdl_plugin", "kinematics_interface::KinematicsBaseClass"); kinematics_ = std::unique_ptr( - kinematics_loader_->createUnmanagedInstance(parameters_.kinematics_plugin_name_)); - if (!kinematics_->initialize(node, parameters_.end_effector_name_)) { + kinematics_loader_->createUnmanagedInstance(parameters_->kinematics_.plugin_name_)); + if (!kinematics_->initialize(node, parameters_->kinematics_.tip_)) { return controller_interface::return_type::ERROR; } } catch (pluginlib::PluginlibException &ex) { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", - parameters_.kinematics_plugin_name_.c_str(), ex.what()); + parameters_->kinematics_.plugin_name_.c_str(), ex.what()); return controller_interface::return_type::ERROR; } } else { @@ -74,17 +74,23 @@ namespace admittance_controller { transform_buffer_vec.assign(16, 0.0); cart_buffer_vec.assign(6, 0.0); + damping_.assign(6, 0.0); + mass_.assign(6, 0.0); + stiffness_.assign(6, 0.0); + selected_axes_.assign(6, 0.0); + // admittance state vectors in joint space joint_pos.resize(num_joints_, 0.0); joint_vel.resize(num_joints_, 0.0); joint_acc.resize(num_joints_, 0.0); // transforms - cur_ee_transform.setIdentity(); + ee_transform.setIdentity(); reference_ee_transform.setIdentity(); - cur_sensor_transform.setIdentity(); - cur_control_transform.setIdentity(); + sensor_transform.setIdentity(); + control_transform.setIdentity(); cog_transform.setIdentity(); + base_link_transform_.setIdentity(); // admittance values admittance_position_.setIdentity(); @@ -92,6 +98,20 @@ namespace admittance_controller { admittance_acceleration_.setZero(); wrench_.setZero(); + // set config values + memcpy(cog_.data(), parameters_->gravity_compensation_.CoG_.pos_.data(), 3*sizeof(double )); + ee_weight.setZero(); + ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; + + mass_ = parameters_->admittance_.mass_; + selected_axes_= parameters_->admittance_.selected_axes_; + stiffness_ = parameters_->admittance_.stiffness_; + for (auto i = 0ul; i < damping_.size(); ++i) + { + damping_[i] = parameters_->admittance_.damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); + } + + return controller_interface::return_type::OK; } @@ -105,33 +125,47 @@ namespace admittance_controller { // keep track of failed kinematics interface calls bool success = true; - double dt = period.seconds()+((double) period.nanoseconds())*1E-9; + double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; // get all needed transforms - reference_ee_transform = get_transform(reference_joint_state.positions, parameters_.end_effector_name_, success); - cur_ee_transform = get_transform(current_joint_state.positions, parameters_.end_effector_name_, success); - cur_sensor_transform = get_transform(current_joint_state.positions, parameters_.sensor_frame_, success); - cur_control_transform = get_transform(current_joint_state.positions, parameters_.control_frame_, success); - cog_transform = get_transform(current_joint_state.positions, parameters_.cog_frame_, success); - auto cur_control_transform_inv = invert_transform(cur_control_transform); + // since the base_link_transform transform in the fixed_world_frame_ cannot be determined from the URDF, + // we must resort to TF + + // TODO check if world frame is external to robot +// if (parameters_.fixed_world_frame_ != parameters_.kinematics_base_frame_){ +// base_link_transform_ = find_transform(parameters_->fixed_world_frame_.id_, +// parameters_->kinematics_.base_, success); + base_link_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.base_, success); +// } +// +// control_transform = find_transform(parameters_->fixed_world_frame_.id_, +// parameters_->control_.frame_.id_, success); + control_transform = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, success); + + reference_ee_transform = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, success); + ee_transform = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, success); + sensor_transform = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, success); + cog_transform = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, success); // get all needed rotations - auto cur_control_rot = cur_control_transform.block<3, 3>(0, 0); - auto cur_control_rot_inv = cur_control_transform_inv.block<3, 3>(0, 0); - auto cur_sensor_rot = cur_sensor_transform.block<3, 3>(0, 0); + auto cur_control_rot = control_transform.block<3, 3>(0, 0); + auto cur_control_rot_inv = cur_control_rot.transpose(); + auto cur_sensor_rot = sensor_transform.block<3, 3>(0, 0); auto cog_rot = cog_transform.block<3, 3>(0, 0); + auto base_link_rot = base_link_transform_.block<3, 3>(0, 0); // apply filter and update wrench_ vector - process_wrench_measurements(measured_wrench, cur_sensor_rot, cog_rot); + process_wrench_measurements(measured_wrench, base_link_rot * cur_sensor_rot, base_link_rot * cog_rot); // transform wrench_ into control frame Eigen::Matrix wrench_control = cur_control_rot_inv * wrench_; // calculate desired cartesian velocity in control frame - Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, current_joint_state.velocities, - success); + Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, + current_joint_state.velocities, + success); - desired_vel = cur_control_rot_inv * desired_vel * use_feedforward_commanded_input_; //TODO fix this + desired_vel = cur_control_rot_inv * desired_vel * use_feedforward_commanded_input_; // Compute admittance control law: F = M*a + D*v + S*(x - x_d) calculate_admittance_rule(wrench_control, desired_vel, dt); @@ -163,6 +197,35 @@ namespace admittance_controller { desired_joint_state.accelerations[i] = joint_acc[i]; } + // update controller message state + eigen_to_msg(wrench_, "base_link", state_message_.input_wrench_command); +// state_message_.input_wrench_control_frame.wrench = wrench_control; + +// geometry_msgs/WrenchStamped input_wrench_command #commanded input wrench_ for the controller +// geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller +// trajectory_msgs/JointTrajectory input_joint_command # commanded input JointTrajectory (used only first point) +// +// geometry_msgs/WrenchStamped input_wrench_control_frame # input wrench_ transformed into control frame +// geometry_msgs/PoseStamped input_pose_control_frame # input pose transformed into control frame +// +// geometry_msgs/WrenchStamped measured_wrench # measured wrench_ from the sensor (sensor frame) +// geometry_msgs/WrenchStamped measured_wrench_filtered # measured wrench_ after low-pass and gravity compensation filters in sensor frame +// geometry_msgs/WrenchStamped measured_wrench_control_frame # transformed measured wrench_ to control frame +// geometry_msgs/WrenchStamped measured_wrench_endeffector_frame # measured wrench_ transformed to endeffector +// +// trajectory_msgs/JointTrajectoryPoint admittance_rule_calculated_values # Values calculated by admittance rule (Cartesian space: [x, y, z, rx, ry, rz]) - position = pose_error; effort = measured_wrench +// +// geometry_msgs/PoseStamped current_pose # Current pose from FK +// geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot +// geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule +// +// string[] joint_names +// trajectory_msgs/JointTrajectoryPoint desired_joint_state # result of IK from goal_pose_command +// trajectory_msgs/JointTrajectoryPoint actual_joint_state # read from the hardware +// trajectory_msgs/JointTrajectoryPoint error_joint_state + + + return controller_interface::return_type::OK; } @@ -174,14 +237,14 @@ namespace admittance_controller { // Compute admittance control law: F = M*a + D*v + S*(x - x_d) for (size_t axis = 0; axis < 3; ++axis) { - if (parameters_.selected_axes_[axis]) { + if (selected_axes_[axis]) { double pose_error = -admittance_position_(axis, 3); - admittance_acceleration_(axis, 0) = (1.0 / parameters_.mass_[axis]) * (wrench(axis, 0) + - (parameters_.damping_[axis] * - (desired_vel(axis, 0) - - admittance_velocity_(axis, 0))) + - (parameters_.stiffness_[axis] * - pose_error)); + admittance_acceleration_(axis, 0) = (1.0 / mass_[axis]) * (wrench(axis, 0) + + (damping_[axis] * + (desired_vel(axis, 0) - + admittance_velocity_(axis, 0))) + + (stiffness_[axis] * + pose_error)); admittance_velocity_(axis, 0) += admittance_acceleration_(axis, 0) * dt; admittance_position_(axis, 3) += admittance_velocity_(axis, 0) * dt; @@ -189,7 +252,7 @@ namespace admittance_controller { } // auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); - Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); + Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); Eigen::Vector3d V = get_rotation_axis(R); double theta = acos((1.0 / 2.0) * (R.trace() - 1)); // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped @@ -198,14 +261,14 @@ namespace admittance_controller { double sign = (tmp >= 0) ? 1.0 : -1.0; for (size_t axis = 0; axis < 3; ++axis) { - if (parameters_.selected_axes_[axis+3]) { + if (selected_axes_[axis + 3]) { double pose_error = sign * theta * V(axis); - admittance_acceleration_(axis, 1) = (1.0 / parameters_.mass_[axis+3]) * (wrench(axis, 1) + - (parameters_.damping_[axis+3] * - (desired_vel(axis, 1) - - admittance_velocity_(axis, 1))) + - (parameters_.stiffness_[axis+3] * - pose_error)); + admittance_acceleration_(axis, 1) = (1.0 / mass_[axis + 3]) * (wrench(axis, 1) + + (damping_[axis + 3] * + (desired_vel(axis, 1) - + admittance_velocity_(axis, 1))) + + (stiffness_[axis + 3] * + pose_error)); admittance_velocity_(axis, 1) += admittance_acceleration_(axis, 1) * dt; } @@ -284,7 +347,7 @@ namespace admittance_controller { return V; } - void AdmittanceRule::normalize_rotation(Eigen::Matrix3d& R) { + void AdmittanceRule::normalize_rotation(Eigen::Matrix3d &R) { // enforce orthonormal constraint Eigen::Vector3d R_0 = R.block<3, 1>(0, 0); @@ -317,6 +380,29 @@ namespace admittance_controller { } + Eigen::Matrix + AdmittanceRule::find_transform(const std::string &target_frame, const std::string &source_frame, bool &success) { + //Parameters + // target_frame: The frame to which data should be transformed + //source_frame: The frame where the data originated + + Eigen::Matrix transform; + transform.setIdentity(); + try { + auto transform_msg = tf_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + transform.block<3, 3>(0, 0) = Eigen::Matrix(tf2::transformToKDL(transform_msg).M.data); + transform(0, 3) = transform_msg.transform.translation.x; + transform(1, 3) = transform_msg.transform.translation.y; + transform(2, 3) = transform_msg.transform.translation.z; + } catch (const tf2::TransformException &e) { + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); + success = false; + } + return transform; + + } + Eigen::Matrix AdmittanceRule::invert_transform(Eigen::Matrix &T) { Eigen::Matrix3d R = T.block<3, 3>(0, 0); @@ -333,18 +419,19 @@ namespace admittance_controller { success &= kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec, joint_delta); } - Eigen::Matrix AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, - const std::vector &joint_delta, - bool &success) { + Eigen::Matrix + AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, + const std::vector &joint_delta, + bool &success) { Eigen::Matrix cartesian_delta; success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, cart_buffer_vec); - memcpy(cartesian_delta.data(), cart_buffer_vec.data(), 6 * sizeof(double)); + memcpy(cartesian_delta.data(), cart_buffer_vec.data(), 6 * sizeof(double)); return cartesian_delta; } - controller_interface::return_type AdmittanceRule::get_controller_state( - control_msgs::msg::AdmittanceControllerState &state_message) { - // TODO update this + control_msgs::msg::AdmittanceControllerState + AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { +// // TODO update this // state_message.input_wrench_control_frame = reference_wrench_control_frame_; // state_message.input_pose_control_frame = reference_pose_kinematics_base_frame_; // state_message.measured_wrench = measured_wrench_; @@ -359,19 +446,20 @@ namespace admittance_controller { //// state_message.relative_admittance = sum_of_admittance_displacements_; // state_message.relative_desired_pose = relative_admittance_pose_kinematics_base_frame_; - return controller_interface::return_type::OK; + return state_message_;//controller_interface::return_type::OK; } void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &cur_sensor_rot, const Eigen::Matrix& cog_rot + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &cur_sensor_rot, + const Eigen::Matrix &cog_rot ) { Eigen::Matrix new_wrench; - new_wrench(0,0) = measured_wrench.force.x; - new_wrench(1,0) = measured_wrench.force.y; - new_wrench(2,0) = measured_wrench.force.z; - new_wrench(0,1) = measured_wrench.torque.x; - new_wrench(1,1) = measured_wrench.torque.y; - new_wrench(2,1) = measured_wrench.torque.z; + new_wrench(0, 0) = measured_wrench.force.x; + new_wrench(1, 0) = measured_wrench.force.y; + new_wrench(2, 0) = measured_wrench.force.z; + new_wrench(0, 1) = measured_wrench.torque.x; + new_wrench(1, 1) = measured_wrench.torque.y; + new_wrench(2, 1) = measured_wrench.torque.z; // transform to base frame Eigen::Matrix new_wrench_base = cur_sensor_rot * new_wrench; @@ -380,12 +468,24 @@ namespace admittance_controller { new_wrench_base(2, 0) -= ee_weight[2]; new_wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); - for (auto i = 0; i < 6; i++){ + for (auto i = 0; i < 6; i++) { wrench_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_(i), alpha); } - } + } + + void AdmittanceRule::eigen_to_msg(const Eigen::Matrix &wrench, const std::string &frame_id, + geometry_msgs::msg::WrenchStamped &wrench_msg) { + wrench_msg.wrench.force.x = wrench(0, 0); + wrench_msg.wrench.force.y = wrench(1, 0); + wrench_msg.wrench.force.z = wrench(2, 0); + wrench_msg.wrench.torque.x = wrench(0, 1); + wrench_msg.wrench.torque.y = wrench(1, 1); + wrench_msg.wrench.torque.z = wrench(2, 1); + wrench_msg.header.frame_id = frame_id; + wrench_msg.header.stamp = clock_->now(); + } } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml new file mode 100644 index 0000000000..0673505325 --- /dev/null +++ b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml @@ -0,0 +1,93 @@ +admittance_struct: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + chainable_command_interfaces: + - position + - velocity + + kinematics: + plugin_name: kdl_plugin/KDLKinematics + base: base_link # Assumed to be stationary + tip: ee_link + group_name: ur5e_manipulator + + ft_sensor: + name: tcp_fts_sensor + frame: + id: ee_link # ee_link Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + + control: + frame: + id: ee_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: ee_link + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 3.0 # x + - 3.0 # y + - 3.0 # z + - 0.05 # rx + - 0.05 # ry + - 0.05 # rz + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 50.0 # x + - 50.0 # y + - 50.0 # z + - 1.0 # rx + - 1.0 # ry + - 1.0 # rz + + # general settings + enable_parameter_update_without_reactivation: false + joint_limiter_type: "joint_limits/SimpleJointLimiter" + state_publish_rate: 200.0 # Defaults to 50 diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h new file mode 100644 index 0000000000..4ba7c21633 --- /dev/null +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -0,0 +1,278 @@ +// this is auto-generated code + +#include +#include +#include + + +namespace admittance_struct_parameters { + + struct admittance_struct { + // if true, prevent config from updating + bool lock_params_ = false; + std::shared_ptr handle_; + + admittance_struct(const std::shared_ptr ¶meters_interface) { + declare_params(parameters_interface); + auto update_param_cb = [this](const std::vector ¶meters) { + return this->update(parameters); + }; + handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); + } + + std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", + "wrist_2_joint", "wrist_3_joint"}; + std::vector command_interfaces_ = {"position"}; + std::vector state_interfaces_ = {"position", "velocity"}; + std::vector chainable_command_interfaces_ = {"position", "velocity"}; + struct kinematics { + std::string plugin_name_ = "kdl_plugin/KDLKinematics"; + std::string base_ = "base_link"; + std::string tip_ = "ee_link"; + std::string group_name_ = "ur5e_manipulator"; + } kinematics_; + struct ft_sensor { + std::string name_ = "tcp_fts_sensor"; + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + } ft_sensor_; + struct control { + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + } control_; + struct fixed_world_frame { + std::string id_ = "base_link"; + bool external_ = false; + } fixed_world_frame_; + struct gravity_compensation { + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + struct CoG { + std::vector pos_ = {0.1, 0.0, 0.0}; + double force_ = 23.0; + } CoG_; + } gravity_compensation_; + struct admittance { + std::vector selected_axes_ = {true, true, true, true, true, true}; + std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; + std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; + std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; + } admittance_; + bool enable_parameter_update_without_reactivation_ = false; + std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; + double state_publish_rate_ = 200.0; + + + rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = !lock_params_; + if (lock_params_) { + result.reason = "The config can not be updated because they are currently locked."; + return result; + } + + result.reason = "success"; + for (const auto ¶m: parameters) { + if (param.get_name() == "joints") { + joints_ = param.as_string_array(); + } + if (param.get_name() == "command_interfaces") { + command_interfaces_ = param.as_string_array(); + } + if (param.get_name() == "state_interfaces") { + state_interfaces_ = param.as_string_array(); + } + if (param.get_name() == "chainable_command_interfaces") { + chainable_command_interfaces_ = param.as_string_array(); + } + if (param.get_name() == "plugin_name") { + kinematics_.plugin_name_ = param.as_string(); + } + if (param.get_name() == "base") { + kinematics_.base_ = param.as_string(); + } + if (param.get_name() == "tip") { + kinematics_.tip_ = param.as_string(); + } + if (param.get_name() == "group_name") { + kinematics_.group_name_ = param.as_string(); + } + if (param.get_name() == "name") { + ft_sensor_.name_ = param.as_string(); + } + if (param.get_name() == "id") { + ft_sensor_.frame_.id_ = param.as_string(); + } + if (param.get_name() == "external") { + ft_sensor_.frame_.external_ = param.as_bool(); + } + if (param.get_name() == "id") { + control_.frame_.id_ = param.as_string(); + } + if (param.get_name() == "external") { + control_.frame_.external_ = param.as_bool(); + } + if (param.get_name() == "id") { + fixed_world_frame_.id_ = param.as_string(); + } + if (param.get_name() == "external") { + fixed_world_frame_.external_ = param.as_bool(); + } + if (param.get_name() == "id") { + gravity_compensation_.frame_.id_ = param.as_string(); + } + if (param.get_name() == "external") { + gravity_compensation_.frame_.external_ = param.as_bool(); + } + if (param.get_name() == "pos") { + gravity_compensation_.CoG_.pos_ = param.as_double_array(); + } + if (param.get_name() == "force") { + gravity_compensation_.CoG_.force_ = param.as_double(); + } + if (param.get_name() == "selected_axes") { + admittance_.selected_axes_ = param.as_bool_array(); + } + if (param.get_name() == "mass") { + admittance_.mass_ = param.as_double_array(); + } + if (param.get_name() == "damping_ratio") { + admittance_.damping_ratio_ = param.as_double_array(); + } + if (param.get_name() == "stiffness") { + admittance_.stiffness_ = param.as_double_array(); + } + if (param.get_name() == "enable_parameter_update_without_reactivation") { + enable_parameter_update_without_reactivation_ = param.as_bool(); + } + if (param.get_name() == "joint_limiter_type") { + joint_limiter_type_ = param.as_string(); + } + if (param.get_name() == "state_publish_rate") { + state_publish_rate_ = param.as_double(); + } + + } + return result; + } + + void declare_params(const std::shared_ptr ¶meters_interface) { + if (!parameters_interface->has_parameter("joints")) { + auto p_joints = rclcpp::ParameterValue(joints_); + parameters_interface->declare_parameter("joints", p_joints); + } + if (!parameters_interface->has_parameter("command_interfaces")) { + auto p_command_interfaces = rclcpp::ParameterValue(command_interfaces_); + parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); + } + if (!parameters_interface->has_parameter("state_interfaces")) { + auto p_state_interfaces = rclcpp::ParameterValue(state_interfaces_); + parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); + } + if (!parameters_interface->has_parameter("chainable_command_interfaces")) { + auto p_chainable_command_interfaces = rclcpp::ParameterValue(chainable_command_interfaces_); + parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); + } + if (!parameters_interface->has_parameter("kinematics.plugin_name")) { + auto p_kinematics_plugin_name = rclcpp::ParameterValue(kinematics_.plugin_name_); + parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); + } + if (!parameters_interface->has_parameter("kinematics.base")) { + auto p_kinematics_base = rclcpp::ParameterValue(kinematics_.base_); + parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); + } + if (!parameters_interface->has_parameter("kinematics.tip")) { + auto p_kinematics_tip = rclcpp::ParameterValue(kinematics_.tip_); + parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); + } + if (!parameters_interface->has_parameter("kinematics.group_name")) { + auto p_kinematics_group_name = rclcpp::ParameterValue(kinematics_.group_name_); + parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); + } + if (!parameters_interface->has_parameter("ft_sensor.name")) { + auto p_ft_sensor_name = rclcpp::ParameterValue(ft_sensor_.name_); + parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); + } + if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { + auto p_ft_sensor_frame_id = rclcpp::ParameterValue(ft_sensor_.frame_.id_); + parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); + } + if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { + auto p_ft_sensor_frame_external = rclcpp::ParameterValue(ft_sensor_.frame_.external_); + parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); + } + if (!parameters_interface->has_parameter("control.frame.id")) { + auto p_control_frame_id = rclcpp::ParameterValue(control_.frame_.id_); + parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); + } + if (!parameters_interface->has_parameter("control.frame.external")) { + auto p_control_frame_external = rclcpp::ParameterValue(control_.frame_.external_); + parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); + } + if (!parameters_interface->has_parameter("fixed_world_frame.id")) { + auto p_fixed_world_frame_id = rclcpp::ParameterValue(fixed_world_frame_.id_); + parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); + } + if (!parameters_interface->has_parameter("fixed_world_frame.external")) { + auto p_fixed_world_frame_external = rclcpp::ParameterValue(fixed_world_frame_.external_); + parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); + } + if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { + auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(gravity_compensation_.frame_.id_); + parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); + } + if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { + auto p_gravity_compensation_frame_external = rclcpp::ParameterValue(gravity_compensation_.frame_.external_); + parameters_interface->declare_parameter("gravity_compensation.frame.external", + p_gravity_compensation_frame_external); + } + if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { + auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(gravity_compensation_.CoG_.pos_); + parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); + } + if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { + auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(gravity_compensation_.CoG_.force_); + parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); + } + if (!parameters_interface->has_parameter("admittance.selected_axes")) { + auto p_admittance_selected_axes = rclcpp::ParameterValue(admittance_.selected_axes_); + parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); + } + if (!parameters_interface->has_parameter("admittance.mass")) { + auto p_admittance_mass = rclcpp::ParameterValue(admittance_.mass_); + parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); + } + if (!parameters_interface->has_parameter("admittance.damping_ratio")) { + auto p_admittance_damping_ratio = rclcpp::ParameterValue(admittance_.damping_ratio_); + parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); + } + if (!parameters_interface->has_parameter("admittance.stiffness")) { + auto p_admittance_stiffness = rclcpp::ParameterValue(admittance_.stiffness_); + parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); + } + if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { + auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( + enable_parameter_update_without_reactivation_); + parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", + p_enable_parameter_update_without_reactivation); + } + if (!parameters_interface->has_parameter("joint_limiter_type")) { + auto p_joint_limiter_type = rclcpp::ParameterValue(joint_limiter_type_); + parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); + } + if (!parameters_interface->has_parameter("state_publish_rate")) { + auto p_state_publish_rate = rclcpp::ParameterValue(state_publish_rate_); + parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); + } + + } + }; + + +} // namespace admittance_struct_parameters diff --git a/admittance_controller/include/admittance_controller/parameter_handler.hpp b/admittance_controller/include/admittance_controller/parameter_handler.hpp deleted file mode 100644 index 7f7037547c..0000000000 --- a/admittance_controller/include/admittance_controller/parameter_handler.hpp +++ /dev/null @@ -1,388 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. -// -/// \author: Denis Stogl - -#ifndef CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ -#define CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ - -#include -#include -#include -#include -#include - -#include "rclcpp/node.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/parameter_value.hpp" -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -using rclcpp::ParameterType; - -namespace impl -{ -inline std::string normalize_params_prefix(std::string prefix) -{ - if (!prefix.empty()) - { - if ('.' != prefix.back()) - { - prefix += '.'; - } - } - return prefix; -} -} // namespace impl - -struct Parameter -{ - Parameter() = default; - - explicit Parameter(const std::string & name, const uint8_t type, const bool configurable = false) - : name(name), type(type), configurable(configurable) - { - } - - std::string name; - uint8_t type; - bool configurable; -}; - -class ParameterHandler -{ -public: - ParameterHandler( - const std::string & params_prefix = "", int nr_bool_params = 0, int nr_integer_params = 0, - int nr_double_params = 0, int nr_string_params = 0, int nr_byte_array_params = 0, - int nr_bool_array_params = 0, int nr_integer_array_params = 0, int nr_double_array_params = 0, - int nr_string_array_params = 0); - - virtual ~ParameterHandler() = default; - - void initialize(std::shared_ptr node) - { - initialize(node->get_node_parameters_interface(), node->get_logger().get_name()); - } - - void initialize( - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params, - const std::string & logger_name = "::ControllerParameters") - { - params_interface_ = node_params; - logger_name_ = logger_name + "." + "parameters"; - } - - void declare_parameters(); - - /** - * Gets all defined parameters from parameter server after parameters are declared. - * Optionally, check parameters' validity and update storage. - * - * \param[in] check_validity check validity after getting parameters (default: **true**). - * \param[in] update update parameters in storage after getting them (default: **true**). - * \return true if all parameters are read successfully, false if a parameter is not provided or - * parameter configuration is wrong. - */ - bool get_parameters(bool check_validity = true, bool update = true); - - /** - * Update all storage variables from the parameter maps. - * Parameters will be only updated if they are previously read from parameter server or dynamic - * reconfigure callback occurred. - * - * \param[in] check_validity check validity before updating parameters (default: **true**). - * \return is update was successful, i.e., parameters are valid. If \p check_validity is set - * **false**, the result is always **true**. - */ - bool update(bool check_validity = true); - - rcl_interfaces::msg::SetParametersResult set_parameter_callback( - const std::vector & parameters); - -protected: - /** - * Override this method to implement custom parameters check. - * Default implementation does not checks anything, always returns true. - * - * \return **true** if parameters are valid, **false** otherwise. - */ - virtual bool check_if_parameters_are_valid() { return true; } - - /** - * Mapping between parameter storage and implementation members. - * The order of parameter in storage is the order of adding parameters to the storage. - * E.g., to access the value of 5-th string parameter added to storage use: - * `fifth_string_param_ = string_parameters_[4].second; - * where `fifth_string_param_` is the name of the member of a child class. - */ - virtual void update_storage() = 0; - -protected: - void add_bool_parameter( - const std::string & name, const bool & configurable = false, const bool & default_value = false) - { - bool_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL, configurable), - default_value}); - } - - void add_integer_parameter( - const std::string & name, const bool & configurable = false, - const int & default_value = std::numeric_limits::quiet_NaN()) - { - integer_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER, configurable), - default_value}); - } - - void add_double_parameter( - const std::string & name, const bool & configurable = false, - const double & default_value = std::numeric_limits::quiet_NaN()) - { - double_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE, configurable), - default_value}); - } - - void add_string_parameter( - const std::string & name, const bool & configurable = false, - const std::string & default_value = "") - { - string_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING, configurable), - default_value}); - } - - void add_byte_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - byte_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BYTE_ARRAY, configurable), - default_value}); - } - - void add_bool_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - bool_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_BOOL_ARRAY, configurable), - default_value}); - } - - void add_integer_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - integer_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_INTEGER_ARRAY, configurable), - default_value}); - } - void add_double_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - double_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_DOUBLE_ARRAY, configurable), - default_value}); - } - - void add_string_array_parameter( - const std::string & name, const bool & configurable = false, - const std::vector & default_value = {}) - { - string_array_parameters_.push_back( - {Parameter(params_prefix_ + name, ParameterType::PARAMETER_STRING_ARRAY, configurable), - default_value}); - } - - template - bool empty_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (parameter.second.empty()) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is empty", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - bool empty_parameter_in_list(const std::vector> & parameters) - { - return empty_numeric_parameter_in_list(parameters); - } - - template - bool find_and_assign_parameter_value( - std::vector> & parameter_list, - const rclcpp::Parameter & input_parameter) - { - auto is_in_list = [&](const auto & parameter) { - return parameter.first.name == input_parameter.get_name(); - }; - - auto it = std::find_if(parameter_list.begin(), parameter_list.end(), is_in_list); - - if (it != parameter_list.end()) - { - if (!it->first.configurable) - { - throw std::runtime_error( - "Parameter " + input_parameter.get_name() + " is not configurable."); - } - else - { - it->second = input_parameter.get_value(); - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is updated to value: %s", it->first.name.c_str(), - input_parameter.value_to_string().c_str()); - return true; - } - } - else - { - return false; - } - } - - // Storage members - std::vector> bool_parameters_; - std::vector> integer_parameters_; - std::vector> double_parameters_; - std::vector> string_parameters_; - std::vector>> byte_array_parameters_; - std::vector>> bool_array_parameters_; - std::vector>> integer_array_parameters_; - std::vector>> double_array_parameters_; - std::vector>> string_array_parameters_; - - // Functional members - bool declared_; - bool up_to_date_; - std::string params_prefix_; - - // Input/Output members to ROS 2 - std::string logger_name_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_; - -private: - template - void declare_parameters_from_list( - rclcpp::Node::SharedPtr node, const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - node->declare_parameter(parameter.first.name, parameter.second); - } - } - - template - void declare_parameters_from_list(const std::vector> & parameters) - { - for (const auto & parameter : parameters) - { - // declare parameter only if it does not already exists - if (!params_interface_->has_parameter(parameter.first.name)) - { - rclcpp::ParameterValue default_parameter_value(parameter.second); - rcl_interfaces::msg::ParameterDescriptor descr; - descr.name = parameter.first.name; - descr.type = parameter.first.type; - descr.read_only = false; - - params_interface_->declare_parameter(parameter.first.name, default_parameter_value, descr); - } - } - } - - template - bool empty_numeric_parameter_in_list(const std::vector> & parameters) - { - bool ret = false; - for (const auto & parameter : parameters) - { - if (std::isnan(parameter.second)) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter is not set", parameter.first.name.c_str()); - ret = true; - } - } - return ret; - } - - template - bool get_parameters_from_list( - const rclcpp::Node::SharedPtr node, std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = node->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } - - template - bool get_parameters_from_list(std::vector> & parameters) - { - bool ret = true; - for (auto & parameter : parameters) - { - try - { - rclcpp::Parameter input_parameter; // TODO(destogl): will this be allocated each time? - ret = params_interface_->get_parameter(parameter.first.name, input_parameter); - parameter.second = input_parameter.get_value(); - } - catch (rclcpp::ParameterTypeException & e) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "'%s' parameter has wrong type", parameter.first.name.c_str()); - ret = false; - break; - } - } - return ret; - } -}; - -} // namespace control_toolbox - -#endif // CONTROL_TOOLBOX__PARAMETER_HANDLER_HPP_ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 85adb21b1a..c001b65888 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -29,6 +29,7 @@ tf2_geometry_msgs tf2_ros trajectory_msgs + gen_param_struct diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5266e53b8e..6c4e2770a1 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -46,17 +46,15 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_init() { - // initialize controller parameters + // initialize controller config admittance_ = std::make_unique(); - admittance_->parameters_.initialize(get_node()); +// admittance_->parameters_.initialize(get_node()); // TODO need error handling params = { get_node()->get_parameter("joints").as_string_array(), get_node()->get_parameter("command_interfaces").as_string_array(), get_node()->get_parameter("state_interfaces").as_string_array(), get_node()->get_parameter("chainable_command_interfaces").as_string_array(), - get_node()->get_parameter("ft_sensor_name").as_string(), - get_node()->get_parameter("joint_limiter_type").as_string(), }; for (const auto &tmp: params.state_interface_types_) { @@ -76,7 +74,7 @@ namespace admittance_controller { } try { - admittance_->parameters_.declare_parameters(); + admittance_->parameters_ = std::make_shared(get_node()->get_node_parameters_interface()); } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; @@ -178,12 +176,6 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { - // load and set all ROS parameters - if (!admittance_->parameters_.get_parameters()) { - RCLCPP_ERROR(get_node()->get_logger(), "Error happened during reading parameters"); - return CallbackReturn::ERROR; - } - // Print output so users can be sure the interface setup is correct auto get_interface_list = [](const std::vector &interface_types) { std::stringstream ss_command_interfaces; @@ -218,13 +210,13 @@ namespace admittance_controller { // Initialize FTS semantic semantic_component force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(params.ft_sensor_name_)); + semantic_components::ForceTorqueSensor(admittance_->parameters_->ft_sensor_.name_)); // configure admittance rule admittance_->configure(get_node(), num_joints_); - // HACK: This is workaround because it seems that updating parameters only in `on_activate` does - // not work properly: why? - admittance_->parameters_.update(); +// // HACK: This is workaround because it seems that updating config only in `on_activate` does +// // not work properly: why? +// admittance_->parameters_.update(); return LifecycleNodeInterface::on_configure(previous_state); } @@ -234,6 +226,9 @@ namespace admittance_controller { // on_activate is called when the lifecycle node activates. controller_is_active_ = true; + // Initialize Admittance Rule and update admittance config + admittance_->reset(); + // clear out vectors in case of restart joint_position_command_interface_.clear(); joint_velocity_command_interface_.clear(); @@ -262,19 +257,9 @@ namespace admittance_controller { } } - // if there are no state position interfaces, then closed loop control cannot be used - if (joint_position_state_interface_.empty() && !admittance_->parameters_.open_loop_control_) { - RCLCPP_ERROR(get_node()->get_logger(), - "Open loop control set to false but no position state interfaces were provided. Please add a position state interface"); - return CallbackReturn::ERROR; - } - // initialize interface of the FTS semantic semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); - // Initialize Admittance Rule from current states - admittance_->reset(); - // handle state after restart or initial startup read_state_from_hardware(last_state_reference_); @@ -285,11 +270,6 @@ namespace admittance_controller { return CallbackReturn::ERROR; } - // if in open loop mode, the position state interface should be ignored - if (admittance_->parameters_.open_loop_control_) { - open_loop_buffer = last_state_reference_.positions; - } - // reset dynamic fields in case non-zero last_state_reference_.velocities.assign(num_joints_, 0.0); last_state_reference_.accelerations.assign(num_joints_, 0.0); @@ -325,20 +305,12 @@ namespace admittance_controller { for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { joint_position_command_interface_[i].get().set_value(state_desired_.positions[i]); // if in open loop, feed the commanded state back into the input (open_loop_buffer) - if (admittance_->parameters_.open_loop_control_) { - open_loop_buffer[i] = state_desired_.positions[i]; - } last_commanded_state_.positions[i] = state_desired_.positions[i]; } double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { joint_velocity_command_interface_[i].get().set_value(state_desired_.velocities[i]); last_commanded_state_.velocities[i] = state_desired_.velocities[i]; - // if in open loop and there are no position command interfaces, - if (admittance_->parameters_.open_loop_control_ && joint_position_command_interface_.empty()) { - open_loop_buffer[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! - last_commanded_state_.positions[i] = state_current_.positions[i] + state_desired_.velocities[i] * dt; // hack! - } } for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { joint_acceleration_command_interface_[i].get().set_value(state_desired_.accelerations[i]); @@ -389,11 +361,7 @@ namespace admittance_controller { // fill state message with values from hardware state interfaces // if any interface has nan values, assume state_current is the last command state for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { - if (admittance_->parameters_.open_loop_control_) { - state_current.positions[i] = open_loop_buffer[i]; - } else { state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); - } if (std::isnan(state_current.positions[i])) { state_current.positions = last_commanded_state_.positions; break; diff --git a/admittance_controller/src/parameter_handler.cpp b/admittance_controller/src/parameter_handler.cpp deleted file mode 100644 index e2eedf1276..0000000000 --- a/admittance_controller/src/parameter_handler.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. -// -/// \author: Denis Stogl - -#include "admittance_controller/parameter_handler.hpp" - -#include -#include - -#include "rcutils/logging_macros.h" - -namespace control_toolbox -{ -ParameterHandler::ParameterHandler( - const std::string & params_prefix, int nr_bool_params, int nr_integer_params, - int nr_double_params, int nr_string_params, int nr_byte_array_params, int nr_bool_array_params, - int nr_integer_array_params, int nr_double_array_params, int nr_string_array_params) -: declared_(false), up_to_date_(false), params_prefix_("") -{ - params_prefix_ = impl::normalize_params_prefix(params_prefix); - - bool_parameters_.reserve(nr_bool_params); - integer_parameters_.reserve(nr_integer_params); - double_parameters_.reserve(nr_double_params); - string_parameters_.reserve(nr_string_params); - byte_array_parameters_.reserve(nr_byte_array_params); - bool_array_parameters_.reserve(nr_bool_array_params); - integer_array_parameters_.reserve(nr_integer_array_params); - double_array_parameters_.reserve(nr_double_array_params); - string_array_parameters_.reserve(nr_string_array_params); -} - -void ParameterHandler::declare_parameters() -{ - if (!declared_) - { - declare_parameters_from_list(bool_parameters_); - declare_parameters_from_list(integer_parameters_); - declare_parameters_from_list(double_parameters_); - declare_parameters_from_list(string_parameters_); - declare_parameters_from_list(byte_array_parameters_); - declare_parameters_from_list(bool_array_parameters_); - declare_parameters_from_list(integer_array_parameters_); - declare_parameters_from_list(double_array_parameters_); - declare_parameters_from_list(string_array_parameters_); - - declared_ = true; - } - else - { - RCUTILS_LOG_WARN_NAMED( - logger_name_.c_str(), - "Parameters already declared. Declaration should be done only once. " - "Nothing bad will happen, but please correct your code-logic."); - } -} - -bool ParameterHandler::get_parameters(bool check_validity, bool update) -{ - // TODO(destogl): Should we "auto-declare" parameters here? - if (!declared_) - { - RCUTILS_LOG_ERROR_NAMED( - logger_name_.c_str(), "Can not get parameters. Please declare them first."); - return false; - } - - bool ret = false; - - // If parameters are updated using dynamic reconfigure callback then there is no need to read - // them again. To ignore multiple manual reads - ret = - get_parameters_from_list(bool_parameters_) && get_parameters_from_list(integer_parameters_) && - get_parameters_from_list(double_parameters_) && get_parameters_from_list(string_parameters_) && - get_parameters_from_list(byte_array_parameters_) && - get_parameters_from_list(bool_array_parameters_) && - get_parameters_from_list(integer_array_parameters_) && - get_parameters_from_list(double_array_parameters_) && - get_parameters_from_list(string_array_parameters_); - - if (ret && check_validity) - { - ret = this->check_if_parameters_are_valid(); - } - - // If it is all good until now the parameters are not up to date anymore - if (ret) - { - up_to_date_ = false; - } - - if (ret && update) - { - ret = this->update(false); - } - - return ret; -} - -bool ParameterHandler::update(bool check_validity) -{ - bool ret = true; - - // Let's make this efficient and execute code only if parameters are updated - if (!up_to_date_) - { - if (check_validity) - { - ret = this->check_if_parameters_are_valid(); - } - - if (ret) - { - this->update_storage(); - } - else - { - RCUTILS_LOG_WARN_NAMED( - logger_name_.c_str(), "Parameters are not valid and therefore will not be updated"); - } - // reset variable to update parameters only when this is needed - up_to_date_ = true; - } - - return ret; -} - -rcl_interfaces::msg::SetParametersResult ParameterHandler::set_parameter_callback( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - // TODO(destogl): this is probably executed in another thread --> mutex protection is needed. - for (const auto & input_parameter : parameters) - { - bool found = false; - - try - { - found = find_and_assign_parameter_value(bool_parameters_, input_parameter); - if (!found) - { - found = find_and_assign_parameter_value(integer_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(byte_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(bool_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(integer_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(double_array_parameters_, input_parameter); - } - if (!found) - { - found = find_and_assign_parameter_value(string_array_parameters_, input_parameter); - } - - RCUTILS_LOG_INFO_EXPRESSION_NAMED( - found, logger_name_.c_str(), - "Dynamic parameters got changed! Maybe you have to restart controller to update the " - "parameters internally."); - - if (found) - { - up_to_date_ = false; - } - } - catch (const rclcpp::exceptions::InvalidParameterTypeException & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - catch (const std::runtime_error & e) - { - result.successful = false; - result.reason = e.what(); - RCUTILS_LOG_ERROR_NAMED(logger_name_.c_str(), "%s", result.reason.c_str()); - break; - } - } - - return result; -} - -} // namespace control_toolbox From dfc27bf0d4823f115ea0d703ef285554b05bfca8 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sat, 25 Jun 2022 12:20:44 -0600 Subject: [PATCH 022/111] dynamic parameter updating enabled --- .../admittance_controller/admittance_rule.hpp | 4 +- .../admittance_rule_impl.hpp | 29 ++-- .../config/admittance_controller.yaml | 3 + .../config/admittance_struct.h | 133 +++++++++++++++--- 4 files changed, 130 insertions(+), 39 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 86c46a53cb..8eda246aa1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -78,13 +78,13 @@ namespace admittance_controller { public: // TODO(destogl): Add parameter for this - bool use_feedforward_commanded_input_ = true; +// bool use_feedforward_commanded_input_ = true; // Dynamic admittance config std::shared_ptr parameters_; // Filter parameter for exponential smoothing - const double alpha = 0.005; // TODO make a ros param +// const double alpha = 0.005; // TODO make a ros param protected: diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 5875b1c794..0c1f7fd6d7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -98,7 +98,18 @@ namespace admittance_controller { admittance_acceleration_.setZero(); wrench_.setZero(); - // set config values + return controller_interface::return_type::OK; + } + +// Update from reference joint states + controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, + const geometry_msgs::msg::Wrench &measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, + const rclcpp::Duration &period, + trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { + + // update param values memcpy(cog_.data(), parameters_->gravity_compensation_.CoG_.pos_.data(), 3*sizeof(double )); ee_weight.setZero(); ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; @@ -111,18 +122,6 @@ namespace admittance_controller { damping_[i] = parameters_->admittance_.damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); } - - return controller_interface::return_type::OK; - } - -// Update from reference joint states - controller_interface::return_type AdmittanceRule::update( - const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const geometry_msgs::msg::Wrench &measured_wrench, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, - const rclcpp::Duration &period, - trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { - // keep track of failed kinematics interface calls bool success = true; double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; @@ -165,7 +164,7 @@ namespace admittance_controller { current_joint_state.velocities, success); - desired_vel = cur_control_rot_inv * desired_vel * use_feedforward_commanded_input_; + desired_vel = cur_control_rot_inv * desired_vel * parameters_->use_feedforward_commanded_input_; // Compute admittance control law: F = M*a + D*v + S*(x - x_d) calculate_admittance_rule(wrench_control, desired_vel, dt); @@ -470,7 +469,7 @@ namespace admittance_controller { for (auto i = 0; i < 6; i++) { wrench_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_(i), alpha); + new_wrench_base(i), wrench_(i), parameters_->ft_sensor_.filter_coefficient_); } } diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml index 0673505325..7864219b77 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml +++ b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml @@ -24,12 +24,14 @@ admittance_struct: base: base_link # Assumed to be stationary tip: ee_link group_name: ur5e_manipulator + alpha: 0.000005 ft_sensor: name: tcp_fts_sensor frame: id: ee_link # ee_link Wrench measurements are in this frame external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 control: frame: @@ -89,5 +91,6 @@ admittance_struct: # general settings enable_parameter_update_without_reactivation: false + use_feedforward_commanded_input: true joint_limiter_type: "joint_limits/SimpleJointLimiter" state_publish_rate: 200.0 # Defaults to 50 diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 4ba7c21633..3c21e5b146 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -8,7 +8,7 @@ namespace admittance_struct_parameters { struct admittance_struct { - // if true, prevent config from updating + // if true, prevent parameters from updating bool lock_params_ = false; std::shared_ptr handle_; @@ -30,6 +30,7 @@ namespace admittance_struct_parameters { std::string base_ = "base_link"; std::string tip_ = "ee_link"; std::string group_name_ = "ur5e_manipulator"; + double alpha_ = 5e-06; } kinematics_; struct ft_sensor { std::string name_ = "tcp_fts_sensor"; @@ -37,6 +38,7 @@ namespace admittance_struct_parameters { std::string id_ = "ee_link"; bool external_ = false; } frame_; + double filter_coefficient_ = 0.005; } ft_sensor_; struct control { struct frame { @@ -65,6 +67,7 @@ namespace admittance_struct_parameters { std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; } admittance_; bool enable_parameter_update_without_reactivation_ = false; + bool use_feedforward_commanded_input_ = true; std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; double state_publish_rate_ = 200.0; @@ -73,7 +76,7 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::SetParametersResult result; result.successful = !lock_params_; if (lock_params_) { - result.reason = "The config can not be updated because they are currently locked."; + result.reason = "The parameters can not be updated because they are currently locked."; return result; } @@ -91,66 +94,75 @@ namespace admittance_struct_parameters { if (param.get_name() == "chainable_command_interfaces") { chainable_command_interfaces_ = param.as_string_array(); } - if (param.get_name() == "plugin_name") { + if (param.get_name() == "kinematics.plugin_name") { kinematics_.plugin_name_ = param.as_string(); } - if (param.get_name() == "base") { + if (param.get_name() == "kinematics.base") { kinematics_.base_ = param.as_string(); } - if (param.get_name() == "tip") { + if (param.get_name() == "kinematics.tip") { kinematics_.tip_ = param.as_string(); } - if (param.get_name() == "group_name") { + if (param.get_name() == "kinematics.group_name") { kinematics_.group_name_ = param.as_string(); } - if (param.get_name() == "name") { + if (param.get_name() == "kinematics.alpha") { + kinematics_.alpha_ = param.as_double(); + } + if (param.get_name() == "ft_sensor.name") { ft_sensor_.name_ = param.as_string(); } - if (param.get_name() == "id") { + if (param.get_name() == "ft_sensor.frame.id") { ft_sensor_.frame_.id_ = param.as_string(); } - if (param.get_name() == "external") { + if (param.get_name() == "ft_sensor.frame.external") { ft_sensor_.frame_.external_ = param.as_bool(); } - if (param.get_name() == "id") { + if (param.get_name() == "ft_sensor.filter_coefficient") { + ft_sensor_.filter_coefficient_ = param.as_double(); + } + if (param.get_name() == "control.frame.id") { control_.frame_.id_ = param.as_string(); } - if (param.get_name() == "external") { + if (param.get_name() == "control.frame.external") { control_.frame_.external_ = param.as_bool(); } - if (param.get_name() == "id") { + if (param.get_name() == "fixed_world_frame.id") { fixed_world_frame_.id_ = param.as_string(); } - if (param.get_name() == "external") { + if (param.get_name() == "fixed_world_frame.external") { fixed_world_frame_.external_ = param.as_bool(); } - if (param.get_name() == "id") { + if (param.get_name() == "gravity_compensation.frame.id") { gravity_compensation_.frame_.id_ = param.as_string(); } - if (param.get_name() == "external") { + if (param.get_name() == "gravity_compensation.frame.external") { gravity_compensation_.frame_.external_ = param.as_bool(); } - if (param.get_name() == "pos") { + if (param.get_name() == "gravity_compensation.CoG.pos") { gravity_compensation_.CoG_.pos_ = param.as_double_array(); } - if (param.get_name() == "force") { + if (param.get_name() == "gravity_compensation.CoG.force") { gravity_compensation_.CoG_.force_ = param.as_double(); } - if (param.get_name() == "selected_axes") { + if (param.get_name() == "admittance.selected_axes") { admittance_.selected_axes_ = param.as_bool_array(); } - if (param.get_name() == "mass") { + if (param.get_name() == "admittance.mass") { admittance_.mass_ = param.as_double_array(); } - if (param.get_name() == "damping_ratio") { + if (param.get_name() == "admittance.damping_ratio") { admittance_.damping_ratio_ = param.as_double_array(); } - if (param.get_name() == "stiffness") { + if (param.get_name() == "admittance.stiffness") { admittance_.stiffness_ = param.as_double_array(); } if (param.get_name() == "enable_parameter_update_without_reactivation") { enable_parameter_update_without_reactivation_ = param.as_bool(); } + if (param.get_name() == "use_feedforward_commanded_input") { + use_feedforward_commanded_input_ = param.as_bool(); + } if (param.get_name() == "joint_limiter_type") { joint_limiter_type_ = param.as_string(); } @@ -166,113 +178,190 @@ namespace admittance_struct_parameters { if (!parameters_interface->has_parameter("joints")) { auto p_joints = rclcpp::ParameterValue(joints_); parameters_interface->declare_parameter("joints", p_joints); + } else { + joints_ = parameters_interface->get_parameter("joints").as_string_array(); } if (!parameters_interface->has_parameter("command_interfaces")) { auto p_command_interfaces = rclcpp::ParameterValue(command_interfaces_); parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); + } else { + command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); } if (!parameters_interface->has_parameter("state_interfaces")) { auto p_state_interfaces = rclcpp::ParameterValue(state_interfaces_); parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); + } else { + state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); } if (!parameters_interface->has_parameter("chainable_command_interfaces")) { auto p_chainable_command_interfaces = rclcpp::ParameterValue(chainable_command_interfaces_); parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); + } else { + chainable_command_interfaces_ = parameters_interface->get_parameter( + "chainable_command_interfaces").as_string_array(); } if (!parameters_interface->has_parameter("kinematics.plugin_name")) { auto p_kinematics_plugin_name = rclcpp::ParameterValue(kinematics_.plugin_name_); parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); + } else { + kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); } if (!parameters_interface->has_parameter("kinematics.base")) { auto p_kinematics_base = rclcpp::ParameterValue(kinematics_.base_); parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); + } else { + kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); } if (!parameters_interface->has_parameter("kinematics.tip")) { auto p_kinematics_tip = rclcpp::ParameterValue(kinematics_.tip_); parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); + } else { + kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); } if (!parameters_interface->has_parameter("kinematics.group_name")) { auto p_kinematics_group_name = rclcpp::ParameterValue(kinematics_.group_name_); parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); + } else { + kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); + } + if (!parameters_interface->has_parameter("kinematics.alpha")) { + auto p_kinematics_alpha = rclcpp::ParameterValue(kinematics_.alpha_); + parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); + } else { + kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); } if (!parameters_interface->has_parameter("ft_sensor.name")) { auto p_ft_sensor_name = rclcpp::ParameterValue(ft_sensor_.name_); parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); + } else { + ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); } if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { auto p_ft_sensor_frame_id = rclcpp::ParameterValue(ft_sensor_.frame_.id_); parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); + } else { + ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); } if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { auto p_ft_sensor_frame_external = rclcpp::ParameterValue(ft_sensor_.frame_.external_); parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); + } else { + ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); + } + if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { + auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(ft_sensor_.filter_coefficient_); + parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); + } else { + ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( + "ft_sensor.filter_coefficient").as_double(); } if (!parameters_interface->has_parameter("control.frame.id")) { auto p_control_frame_id = rclcpp::ParameterValue(control_.frame_.id_); parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); + } else { + control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); } if (!parameters_interface->has_parameter("control.frame.external")) { auto p_control_frame_external = rclcpp::ParameterValue(control_.frame_.external_); parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); + } else { + control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); } if (!parameters_interface->has_parameter("fixed_world_frame.id")) { auto p_fixed_world_frame_id = rclcpp::ParameterValue(fixed_world_frame_.id_); parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); + } else { + fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string(); } if (!parameters_interface->has_parameter("fixed_world_frame.external")) { auto p_fixed_world_frame_external = rclcpp::ParameterValue(fixed_world_frame_.external_); parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); + } else { + fixed_world_frame_.external_ = parameters_interface->get_parameter("fixed_world_frame.external").as_bool(); } if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(gravity_compensation_.frame_.id_); parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); + } else { + gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( + "gravity_compensation.frame.id").as_string(); } if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { auto p_gravity_compensation_frame_external = rclcpp::ParameterValue(gravity_compensation_.frame_.external_); parameters_interface->declare_parameter("gravity_compensation.frame.external", p_gravity_compensation_frame_external); + } else { + gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( + "gravity_compensation.frame.external").as_bool(); } if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(gravity_compensation_.CoG_.pos_); parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); + } else { + gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( + "gravity_compensation.CoG.pos").as_double_array(); } if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(gravity_compensation_.CoG_.force_); parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); + } else { + gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( + "gravity_compensation.CoG.force").as_double(); } if (!parameters_interface->has_parameter("admittance.selected_axes")) { auto p_admittance_selected_axes = rclcpp::ParameterValue(admittance_.selected_axes_); parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); + } else { + admittance_.selected_axes_ = parameters_interface->get_parameter("admittance.selected_axes").as_bool_array(); } if (!parameters_interface->has_parameter("admittance.mass")) { auto p_admittance_mass = rclcpp::ParameterValue(admittance_.mass_); parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); + } else { + admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); } if (!parameters_interface->has_parameter("admittance.damping_ratio")) { auto p_admittance_damping_ratio = rclcpp::ParameterValue(admittance_.damping_ratio_); parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); + } else { + admittance_.damping_ratio_ = parameters_interface->get_parameter("admittance.damping_ratio").as_double_array(); } if (!parameters_interface->has_parameter("admittance.stiffness")) { auto p_admittance_stiffness = rclcpp::ParameterValue(admittance_.stiffness_); parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); + } else { + admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); } if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( enable_parameter_update_without_reactivation_); parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", p_enable_parameter_update_without_reactivation); + } else { + enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( + "enable_parameter_update_without_reactivation").as_bool(); + } + if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { + auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(use_feedforward_commanded_input_); + parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); + } else { + use_feedforward_commanded_input_ = parameters_interface->get_parameter( + "use_feedforward_commanded_input").as_bool(); } if (!parameters_interface->has_parameter("joint_limiter_type")) { auto p_joint_limiter_type = rclcpp::ParameterValue(joint_limiter_type_); parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); + } else { + joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string(); } if (!parameters_interface->has_parameter("state_publish_rate")) { auto p_state_publish_rate = rclcpp::ParameterValue(state_publish_rate_); parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); + } else { + state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double(); } } }; - } // namespace admittance_struct_parameters From 2db281f16a6c9f0cd8936dc657ecd505741bed49 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 27 Jun 2022 15:18:40 -0600 Subject: [PATCH 023/111] enable/disable parameter update during execution + fill state message --- .../admittance_controller/admittance_rule.hpp | 28 +- .../admittance_rule_impl.hpp | 238 +++++++-------- .../config/admittance_controller.yaml | 2 +- .../config/admittance_struct.h | 278 +++++++++--------- .../src/admittance_controller.cpp | 3 +- 5 files changed, 280 insertions(+), 269 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 8eda246aa1..3c291bd189 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -39,10 +39,7 @@ namespace { // Utility namespace // Numerical accuracy checks. Used as deadbands. - static constexpr double WRENCH_EPSILON = 1e-10; - static constexpr double POSE_ERROR_EPSILON = 1e-12; - static constexpr double POSE_EPSILON = 1e-15; - const double ROT_AXIS_EPSILON = 0.001; + const double ROT_AXIS_EPSILON = 1e-6; } // utility namespace @@ -53,7 +50,7 @@ namespace admittance_controller { public: AdmittanceRule() = default; - controller_interface::return_type configure(std::shared_ptr node, int num_joint); + controller_interface::return_type configure(const std::shared_ptr& node, int num_joint); controller_interface::return_type reset(); /** @@ -72,8 +69,7 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states); - control_msgs::msg::AdmittanceControllerState - get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message); + void get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message); public: @@ -81,7 +77,8 @@ namespace admittance_controller { // bool use_feedforward_commanded_input_ = true; // Dynamic admittance config - std::shared_ptr parameters_; + std::shared_ptr parameter_handler; + std::shared_ptr parameters_; // Filter parameter for exponential smoothing // const double alpha = 0.005; // TODO make a ros param @@ -108,11 +105,8 @@ namespace admittance_controller { const std::vector &joint_delta, bool &success); void normalize_rotation(Eigen::Matrix& R); - Eigen::Matrix invert_transform(Eigen::Matrix &T); - Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool & success); + Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool external, bool & success); void eigen_to_msg(const Eigen::Matrix& wrench, const std::string& frame_id, geometry_msgs::msg::WrenchStamped& wrench_msg); - Eigen::Matrix - find_transform(const std::string &target_frame, const std::string &source_frame, bool &success); // Kinematics interface plugin loader std::shared_ptr> kinematics_loader_; @@ -137,10 +131,18 @@ namespace admittance_controller { Eigen::Matrix sensor_transform; Eigen::Matrix control_transform; Eigen::Matrix cog_transform; - Eigen::Matrix base_link_transform_; + Eigen::Matrix world_transform; + + // rotations + Eigen::Matrix ee_rot; + Eigen::Matrix control_rot; + Eigen::Matrix sensor_rot; + Eigen::Matrix cog_rot; + Eigen::Matrix world_rot; // external force Eigen::Matrix wrench_; + Eigen::Matrix measured_wrench_; // position of center of gravity in cog_frame Eigen::Matrix cog_; // force applied to sensor due to weight of end effector diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 0c1f7fd6d7..c11f6ef6a0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -26,7 +26,7 @@ namespace admittance_controller { controller_interface::return_type - AdmittanceRule::configure(std::shared_ptr node, int num_joints) { + AdmittanceRule::configure(const std::shared_ptr& node, int num_joints) { // configure admittance rule using num_joints and load kinematics interface num_joints_ = num_joints; @@ -42,8 +42,6 @@ namespace admittance_controller { // Load the differential IK plugin if (!parameters_->kinematics_.plugin_name_.empty()) { try { - // TODO(destogl): add "kinematics_interface" into separate package and then rename the package in - // the next line from "admittance_controller" to "kinematics_base_plugin" kinematics_loader_ = std::make_shared>( "kdl_plugin", "kinematics_interface::KinematicsBaseClass"); kinematics_ = std::unique_ptr( @@ -74,10 +72,16 @@ namespace admittance_controller { transform_buffer_vec.assign(16, 0.0); cart_buffer_vec.assign(6, 0.0); + state_message_.error_joint_state.positions.resize(num_joints_, 0.0); + state_message_.error_joint_state.velocities.resize(num_joints_, 0.0); + state_message_.admittance_rule_calculated_values.positions.resize(6, 0.0); + state_message_.admittance_rule_calculated_values.velocities.resize(6, 0.0); + state_message_.admittance_rule_calculated_values.accelerations.resize(6, 0.0); + damping_.assign(6, 0.0); mass_.assign(6, 0.0); stiffness_.assign(6, 0.0); - selected_axes_.assign(6, 0.0); + selected_axes_.assign(6, false); // admittance state vectors in joint space joint_pos.resize(num_joints_, 0.0); @@ -90,7 +94,7 @@ namespace admittance_controller { sensor_transform.setIdentity(); control_transform.setIdentity(); cog_transform.setIdentity(); - base_link_transform_.setIdentity(); + world_transform.setIdentity(); // admittance values admittance_position_.setIdentity(); @@ -98,6 +102,17 @@ namespace admittance_controller { admittance_acceleration_.setZero(); wrench_.setZero(); + // parameters + if (parameter_handler->params_.enable_parameter_update_without_reactivation_){ + // point to the dynamic data + std::shared_ptr tmp {¶meter_handler->params_}; + parameters_ = tmp; + } else{ + // make a copy of the data + parameters_ = std::make_shared(); + *parameters_ = parameter_handler->params_; + } + return controller_interface::return_type::OK; } @@ -109,11 +124,12 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { + double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; + // update param values memcpy(cog_.data(), parameters_->gravity_compensation_.CoG_.pos_.data(), 3*sizeof(double )); ee_weight.setZero(); ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; - mass_ = parameters_->admittance_.mass_; selected_axes_= parameters_->admittance_.selected_axes_; stiffness_ = parameters_->admittance_.stiffness_; @@ -124,56 +140,43 @@ namespace admittance_controller { // keep track of failed kinematics interface calls bool success = true; - double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; // get all needed transforms - // since the base_link_transform transform in the fixed_world_frame_ cannot be determined from the URDF, - // we must resort to TF - - // TODO check if world frame is external to robot -// if (parameters_.fixed_world_frame_ != parameters_.kinematics_base_frame_){ -// base_link_transform_ = find_transform(parameters_->fixed_world_frame_.id_, -// parameters_->kinematics_.base_, success); - base_link_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.base_, success); -// } -// -// control_transform = find_transform(parameters_->fixed_world_frame_.id_, -// parameters_->control_.frame_.id_, success); - control_transform = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, success); - - reference_ee_transform = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, success); - ee_transform = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, success); - sensor_transform = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, success); - cog_transform = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, success); + world_transform = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, parameters_->fixed_world_frame_.external_, success); + control_transform = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, parameters_->control_.frame_.external_, success); + reference_ee_transform = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, false, success); + ee_transform = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); + sensor_transform = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); + cog_transform = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, parameters_->gravity_compensation_.frame_.external_, success); // get all needed rotations - auto cur_control_rot = control_transform.block<3, 3>(0, 0); - auto cur_control_rot_inv = cur_control_rot.transpose(); - auto cur_sensor_rot = sensor_transform.block<3, 3>(0, 0); - auto cog_rot = cog_transform.block<3, 3>(0, 0); - auto base_link_rot = base_link_transform_.block<3, 3>(0, 0); + ee_rot = ee_transform.block<3, 3>(0, 0); + control_rot = control_transform.block<3, 3>(0, 0); + sensor_rot = sensor_transform.block<3, 3>(0, 0); + cog_rot = cog_transform.block<3, 3>(0, 0); + world_rot = world_transform.block<3, 3>(0, 0); // apply filter and update wrench_ vector - process_wrench_measurements(measured_wrench, base_link_rot * cur_sensor_rot, base_link_rot * cog_rot); + process_wrench_measurements(measured_wrench, world_rot * sensor_rot, world_rot * cog_rot); // transform wrench_ into control frame - Eigen::Matrix wrench_control = cur_control_rot_inv * wrench_; + Eigen::Matrix wrench_control = control_rot.transpose() * wrench_; // calculate desired cartesian velocity in control frame Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, current_joint_state.velocities, success); - desired_vel = cur_control_rot_inv * desired_vel * parameters_->use_feedforward_commanded_input_; + desired_vel = control_rot.transpose() * desired_vel * parameters_->use_feedforward_commanded_input_; // Compute admittance control law: F = M*a + D*v + S*(x - x_d) calculate_admittance_rule(wrench_control, desired_vel, dt); // transform admittance values back to base frame - auto admittance_velocity_base = cur_control_rot * admittance_velocity_; + auto admittance_velocity_base = control_rot * admittance_velocity_; convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_base, joint_vel, success); - auto admittance_acceleration_base = cur_control_rot * admittance_acceleration_; + auto admittance_acceleration_base = control_rot * admittance_acceleration_; convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_base, joint_acc, success); @@ -187,17 +190,24 @@ namespace admittance_controller { for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { joint_pos[i] += joint_vel[i] * dt;//- .2 * joint_pos[i] * (1.0 / 1000.0); desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos[i]; + state_message_.error_joint_state.positions[i] = reference_joint_state.positions[i] - current_joint_state.positions[i]; } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel[i]; + state_message_.error_joint_state.velocities[i] = reference_joint_state.velocities[i] - current_joint_state.velocities[i]; + } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { desired_joint_state.accelerations[i] = joint_acc[i]; } - // update controller message state - eigen_to_msg(wrench_, "base_link", state_message_.input_wrench_command); + // update admittance state message + state_message_.actual_joint_state = current_joint_state; + state_message_.desired_joint_state = desired_joint_state; + + + // state_message_.input_wrench_control_frame.wrench = wrench_control; // geometry_msgs/WrenchStamped input_wrench_command #commanded input wrench_ for the controller @@ -247,6 +257,10 @@ namespace admittance_controller { admittance_velocity_(axis, 0) += admittance_acceleration_(axis, 0) * dt; admittance_position_(axis, 3) += admittance_velocity_(axis, 0) * dt; + // store calculated values + state_message_.admittance_rule_calculated_values.positions[axis] = admittance_position_(axis, 3); + state_message_.admittance_rule_calculated_values.velocities[axis] = admittance_velocity_(axis, 0); + state_message_.admittance_rule_calculated_values.accelerations[axis] = admittance_acceleration_(axis, 0); } } @@ -270,6 +284,9 @@ namespace admittance_controller { pose_error)); admittance_velocity_(axis, 1) += admittance_acceleration_(axis, 1) * dt; + // store calculated values + state_message_.admittance_rule_calculated_values.velocities[axis+3] = admittance_velocity_(axis, 1); + state_message_.admittance_rule_calculated_values.accelerations[axis+3] = admittance_acceleration_(axis, 1); } } @@ -282,6 +299,62 @@ namespace admittance_controller { R += R_dot * dt; normalize_rotation(R); admittance_position_.block<3, 3>(0, 0) = R; + // store calculated values + state_message_.admittance_rule_calculated_values.positions[0] = atan2(admittance_position_(2,1), admittance_position_(2,2)); + state_message_.admittance_rule_calculated_values.positions[0] = asin(admittance_position_(2,0)); + state_message_.admittance_rule_calculated_values.positions[0] = -atan2(admittance_position_(1,0), admittance_position_(0,0)); + } + + Eigen::Matrix + AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, bool &success) { + Eigen::Matrix transform; + if (external){ + transform.setIdentity(); + try { + auto transform_msg = tf_buffer_->lookupTransform(parameters_->kinematics_.base_, link_name, tf2::TimePointZero); + transform.block<3, 3>(0, 0) = Eigen::Matrix(tf2::transformToKDL(transform_msg).M.data); + transform(0, 3) = transform_msg.transform.translation.x; + transform(1, 3) = transform_msg.transform.translation.y; + transform(2, 3) = transform_msg.transform.translation.z; + } catch (const tf2::TransformException &e) { + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); + success = false; + } + } else{ + success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec); + memcpy(transform.data(), transform_buffer_vec.data(), 16 * sizeof(double)); + } + return transform; + + } + + void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &cur_sensor_rot, + const Eigen::Matrix &cog_rot + ) { + Eigen::Matrix new_wrench; + new_wrench(0, 0) = measured_wrench.force.x; + new_wrench(1, 0) = measured_wrench.force.y; + new_wrench(2, 0) = measured_wrench.force.z; + new_wrench(0, 1) = measured_wrench.torque.x; + new_wrench(1, 1) = measured_wrench.torque.y; + new_wrench(2, 1) = measured_wrench.torque.z; + + // transform to base frame + Eigen::Matrix new_wrench_base = cur_sensor_rot * new_wrench; + // store value for state message + measured_wrench_ = new_wrench_base; + + // apply gravity compensation + new_wrench_base(2, 0) -= ee_weight[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); + + for (auto i = 0; i < 6; i++) { + wrench_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_(i), parameters_->ft_sensor_.filter_coefficient_); + } + } Eigen::Vector3d AdmittanceRule::get_rotation_axis(const Eigen::Matrix3d &R) const { @@ -370,47 +443,6 @@ namespace admittance_controller { } - Eigen::Matrix - AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool &success) { - success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec); - Eigen::Matrix transform; - memcpy(transform.data(), transform_buffer_vec.data(), 16 * sizeof(double)); - return transform; - - } - - Eigen::Matrix - AdmittanceRule::find_transform(const std::string &target_frame, const std::string &source_frame, bool &success) { - //Parameters - // target_frame: The frame to which data should be transformed - //source_frame: The frame where the data originated - - Eigen::Matrix transform; - transform.setIdentity(); - try { - auto transform_msg = tf_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); - transform.block<3, 3>(0, 0) = Eigen::Matrix(tf2::transformToKDL(transform_msg).M.data); - transform(0, 3) = transform_msg.transform.translation.x; - transform(1, 3) = transform_msg.transform.translation.y; - transform(2, 3) = transform_msg.transform.translation.z; - } catch (const tf2::TransformException &e) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE( - rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); - success = false; - } - return transform; - - } - - Eigen::Matrix - AdmittanceRule::invert_transform(Eigen::Matrix &T) { - Eigen::Matrix3d R = T.block<3, 3>(0, 0); - Eigen::Matrix4d T_inv = T; - T_inv.block<3, 3>(0, 0) = R.transpose(); - T_inv.block<3, 1>(0, 3) = -R.transpose() * T_inv.block<3, 1>(0, 3); - return T_inv; - } - void AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, const Eigen::Matrix &cartesian_delta, std::vector &joint_delta, bool &success) { @@ -428,49 +460,19 @@ namespace admittance_controller { return cartesian_delta; } - control_msgs::msg::AdmittanceControllerState - AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { -// // TODO update this -// state_message.input_wrench_control_frame = reference_wrench_control_frame_; -// state_message.input_pose_control_frame = reference_pose_kinematics_base_frame_; -// state_message.measured_wrench = measured_wrench_; -// state_message.measured_wrench_filtered = measured_wrench_filtered_; -// state_message.measured_wrench_control_frame = measured_wrench_kinematics_base_frame_; -// -// state_message.admittance_rule_calculated_values = admittance_rule_calculated_values_; -// -// state_message.current_pose = current_pose_kinematics_base_frame_; -// state_message.desired_pose = admittance_pose_kinematics_base_frame_; -// // TODO(destogl): Enable this field for debugging. -//// state_message.relative_admittance = sum_of_admittance_displacements_; -// state_message.relative_desired_pose = relative_admittance_pose_kinematics_base_frame_; - - return state_message_;//controller_interface::return_type::OK; - } - - void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &cur_sensor_rot, - const Eigen::Matrix &cog_rot - ) { - Eigen::Matrix new_wrench; - new_wrench(0, 0) = measured_wrench.force.x; - new_wrench(1, 0) = measured_wrench.force.y; - new_wrench(2, 0) = measured_wrench.force.z; - new_wrench(0, 1) = measured_wrench.torque.x; - new_wrench(1, 1) = measured_wrench.torque.y; - new_wrench(2, 1) = measured_wrench.torque.z; + void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { - // transform to base frame - Eigen::Matrix new_wrench_base = cur_sensor_rot * new_wrench; + eigen_to_msg(wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench_filtered); + eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench); + eigen_to_msg(control_rot.transpose()*world_rot.transpose()*measured_wrench_, parameters_->control_.frame_.id_, state_message.measured_wrench_control_frame); + eigen_to_msg(ee_rot.transpose()*world_rot.transpose()*measured_wrench_, parameters_->kinematics_.tip_, state_message.measured_wrench_endeffector_frame); - // apply gravity compensation - new_wrench_base(2, 0) -= ee_weight[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); + state_message.joint_names = parameters_->joints_; + state_message.desired_joint_state = state_message_.desired_joint_state; + state_message.actual_joint_state = state_message_.actual_joint_state; + state_message.error_joint_state = state_message_.error_joint_state; - for (auto i = 0; i < 6; i++) { - wrench_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_(i), parameters_->ft_sensor_.filter_coefficient_); - } + state_message.admittance_rule_calculated_values = state_message_.admittance_rule_calculated_values; } diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml index 7864219b77..b828cddaaf 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml +++ b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml @@ -90,7 +90,7 @@ admittance_struct: - 1.0 # rz # general settings - enable_parameter_update_without_reactivation: false + enable_parameter_update_without_reactivation: true use_feedforward_commanded_input: true joint_limiter_type: "joint_limits/SimpleJointLimiter" state_publish_rate: 200.0 # Defaults to 50 diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 3c21e5b146..944349368d 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -20,57 +20,59 @@ namespace admittance_struct_parameters { handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); } - std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", - "wrist_2_joint", "wrist_3_joint"}; - std::vector command_interfaces_ = {"position"}; - std::vector state_interfaces_ = {"position", "velocity"}; - std::vector chainable_command_interfaces_ = {"position", "velocity"}; - struct kinematics { - std::string plugin_name_ = "kdl_plugin/KDLKinematics"; - std::string base_ = "base_link"; - std::string tip_ = "ee_link"; - std::string group_name_ = "ur5e_manipulator"; - double alpha_ = 5e-06; - } kinematics_; - struct ft_sensor { - std::string name_ = "tcp_fts_sensor"; - struct frame { - std::string id_ = "ee_link"; + struct params { + std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", + "wrist_2_joint", "wrist_3_joint"}; + std::vector command_interfaces_ = {"position"}; + std::vector state_interfaces_ = {"position", "velocity"}; + std::vector chainable_command_interfaces_ = {"position", "velocity"}; + struct kinematics { + std::string plugin_name_ = "kdl_plugin/KDLKinematics"; + std::string base_ = "base_link"; + std::string tip_ = "ee_link"; + std::string group_name_ = "ur5e_manipulator"; + double alpha_ = 5e-06; + } kinematics_; + struct ft_sensor { + std::string name_ = "tcp_fts_sensor"; + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + double filter_coefficient_ = 0.005; + } ft_sensor_; + struct control { + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + } control_; + struct fixed_world_frame { + std::string id_ = "base_link"; bool external_ = false; - } frame_; - double filter_coefficient_ = 0.005; - } ft_sensor_; - struct control { - struct frame { - std::string id_ = "ee_link"; - bool external_ = false; - } frame_; - } control_; - struct fixed_world_frame { - std::string id_ = "base_link"; - bool external_ = false; - } fixed_world_frame_; - struct gravity_compensation { - struct frame { - std::string id_ = "ee_link"; - bool external_ = false; - } frame_; - struct CoG { - std::vector pos_ = {0.1, 0.0, 0.0}; - double force_ = 23.0; - } CoG_; - } gravity_compensation_; - struct admittance { - std::vector selected_axes_ = {true, true, true, true, true, true}; - std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; - std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; - std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; - } admittance_; - bool enable_parameter_update_without_reactivation_ = false; - bool use_feedforward_commanded_input_ = true; - std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; - double state_publish_rate_ = 200.0; + } fixed_world_frame_; + struct gravity_compensation { + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + struct CoG { + std::vector pos_ = {0.1, 0.0, 0.0}; + double force_ = 23.0; + } CoG_; + } gravity_compensation_; + struct admittance { + std::vector selected_axes_ = {true, true, true, true, true, true}; + std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; + std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; + std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; + } admittance_; + bool enable_parameter_update_without_reactivation_ = true; + bool use_feedforward_commanded_input_ = true; + std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; + double state_publish_rate_ = 200.0; + } params_; rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; @@ -83,91 +85,91 @@ namespace admittance_struct_parameters { result.reason = "success"; for (const auto ¶m: parameters) { if (param.get_name() == "joints") { - joints_ = param.as_string_array(); + params_.joints_ = param.as_string_array(); } if (param.get_name() == "command_interfaces") { - command_interfaces_ = param.as_string_array(); + params_.command_interfaces_ = param.as_string_array(); } if (param.get_name() == "state_interfaces") { - state_interfaces_ = param.as_string_array(); + params_.state_interfaces_ = param.as_string_array(); } if (param.get_name() == "chainable_command_interfaces") { - chainable_command_interfaces_ = param.as_string_array(); + params_.chainable_command_interfaces_ = param.as_string_array(); } if (param.get_name() == "kinematics.plugin_name") { - kinematics_.plugin_name_ = param.as_string(); + params_.kinematics_.plugin_name_ = param.as_string(); } if (param.get_name() == "kinematics.base") { - kinematics_.base_ = param.as_string(); + params_.kinematics_.base_ = param.as_string(); } if (param.get_name() == "kinematics.tip") { - kinematics_.tip_ = param.as_string(); + params_.kinematics_.tip_ = param.as_string(); } if (param.get_name() == "kinematics.group_name") { - kinematics_.group_name_ = param.as_string(); + params_.kinematics_.group_name_ = param.as_string(); } if (param.get_name() == "kinematics.alpha") { - kinematics_.alpha_ = param.as_double(); + params_.kinematics_.alpha_ = param.as_double(); } if (param.get_name() == "ft_sensor.name") { - ft_sensor_.name_ = param.as_string(); + params_.ft_sensor_.name_ = param.as_string(); } if (param.get_name() == "ft_sensor.frame.id") { - ft_sensor_.frame_.id_ = param.as_string(); + params_.ft_sensor_.frame_.id_ = param.as_string(); } if (param.get_name() == "ft_sensor.frame.external") { - ft_sensor_.frame_.external_ = param.as_bool(); + params_.ft_sensor_.frame_.external_ = param.as_bool(); } if (param.get_name() == "ft_sensor.filter_coefficient") { - ft_sensor_.filter_coefficient_ = param.as_double(); + params_.ft_sensor_.filter_coefficient_ = param.as_double(); } if (param.get_name() == "control.frame.id") { - control_.frame_.id_ = param.as_string(); + params_.control_.frame_.id_ = param.as_string(); } if (param.get_name() == "control.frame.external") { - control_.frame_.external_ = param.as_bool(); + params_.control_.frame_.external_ = param.as_bool(); } if (param.get_name() == "fixed_world_frame.id") { - fixed_world_frame_.id_ = param.as_string(); + params_.fixed_world_frame_.id_ = param.as_string(); } if (param.get_name() == "fixed_world_frame.external") { - fixed_world_frame_.external_ = param.as_bool(); + params_.fixed_world_frame_.external_ = param.as_bool(); } if (param.get_name() == "gravity_compensation.frame.id") { - gravity_compensation_.frame_.id_ = param.as_string(); + params_.gravity_compensation_.frame_.id_ = param.as_string(); } if (param.get_name() == "gravity_compensation.frame.external") { - gravity_compensation_.frame_.external_ = param.as_bool(); + params_.gravity_compensation_.frame_.external_ = param.as_bool(); } if (param.get_name() == "gravity_compensation.CoG.pos") { - gravity_compensation_.CoG_.pos_ = param.as_double_array(); + params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); } if (param.get_name() == "gravity_compensation.CoG.force") { - gravity_compensation_.CoG_.force_ = param.as_double(); + params_.gravity_compensation_.CoG_.force_ = param.as_double(); } if (param.get_name() == "admittance.selected_axes") { - admittance_.selected_axes_ = param.as_bool_array(); + params_.admittance_.selected_axes_ = param.as_bool_array(); } if (param.get_name() == "admittance.mass") { - admittance_.mass_ = param.as_double_array(); + params_.admittance_.mass_ = param.as_double_array(); } if (param.get_name() == "admittance.damping_ratio") { - admittance_.damping_ratio_ = param.as_double_array(); + params_.admittance_.damping_ratio_ = param.as_double_array(); } if (param.get_name() == "admittance.stiffness") { - admittance_.stiffness_ = param.as_double_array(); + params_.admittance_.stiffness_ = param.as_double_array(); } if (param.get_name() == "enable_parameter_update_without_reactivation") { - enable_parameter_update_without_reactivation_ = param.as_bool(); + params_.enable_parameter_update_without_reactivation_ = param.as_bool(); } if (param.get_name() == "use_feedforward_commanded_input") { - use_feedforward_commanded_input_ = param.as_bool(); + params_.use_feedforward_commanded_input_ = param.as_bool(); } if (param.get_name() == "joint_limiter_type") { - joint_limiter_type_ = param.as_string(); + params_.joint_limiter_type_ = param.as_string(); } if (param.get_name() == "state_publish_rate") { - state_publish_rate_ = param.as_double(); + params_.state_publish_rate_ = param.as_double(); } } @@ -176,189 +178,193 @@ namespace admittance_struct_parameters { void declare_params(const std::shared_ptr ¶meters_interface) { if (!parameters_interface->has_parameter("joints")) { - auto p_joints = rclcpp::ParameterValue(joints_); + auto p_joints = rclcpp::ParameterValue(params_.joints_); parameters_interface->declare_parameter("joints", p_joints); } else { - joints_ = parameters_interface->get_parameter("joints").as_string_array(); + params_.joints_ = parameters_interface->get_parameter("joints").as_string_array(); } if (!parameters_interface->has_parameter("command_interfaces")) { - auto p_command_interfaces = rclcpp::ParameterValue(command_interfaces_); + auto p_command_interfaces = rclcpp::ParameterValue(params_.command_interfaces_); parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); } else { - command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); + params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); } if (!parameters_interface->has_parameter("state_interfaces")) { - auto p_state_interfaces = rclcpp::ParameterValue(state_interfaces_); + auto p_state_interfaces = rclcpp::ParameterValue(params_.state_interfaces_); parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); } else { - state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); + params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); } if (!parameters_interface->has_parameter("chainable_command_interfaces")) { - auto p_chainable_command_interfaces = rclcpp::ParameterValue(chainable_command_interfaces_); + auto p_chainable_command_interfaces = rclcpp::ParameterValue(params_.chainable_command_interfaces_); parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); } else { - chainable_command_interfaces_ = parameters_interface->get_parameter( + params_.chainable_command_interfaces_ = parameters_interface->get_parameter( "chainable_command_interfaces").as_string_array(); } if (!parameters_interface->has_parameter("kinematics.plugin_name")) { - auto p_kinematics_plugin_name = rclcpp::ParameterValue(kinematics_.plugin_name_); + auto p_kinematics_plugin_name = rclcpp::ParameterValue(params_.kinematics_.plugin_name_); parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); } else { - kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); + params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); } if (!parameters_interface->has_parameter("kinematics.base")) { - auto p_kinematics_base = rclcpp::ParameterValue(kinematics_.base_); + auto p_kinematics_base = rclcpp::ParameterValue(params_.kinematics_.base_); parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); } else { - kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); + params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); } if (!parameters_interface->has_parameter("kinematics.tip")) { - auto p_kinematics_tip = rclcpp::ParameterValue(kinematics_.tip_); + auto p_kinematics_tip = rclcpp::ParameterValue(params_.kinematics_.tip_); parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); } else { - kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); + params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); } if (!parameters_interface->has_parameter("kinematics.group_name")) { - auto p_kinematics_group_name = rclcpp::ParameterValue(kinematics_.group_name_); + auto p_kinematics_group_name = rclcpp::ParameterValue(params_.kinematics_.group_name_); parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); } else { - kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); + params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); } if (!parameters_interface->has_parameter("kinematics.alpha")) { - auto p_kinematics_alpha = rclcpp::ParameterValue(kinematics_.alpha_); + auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); } else { - kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); + params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); } if (!parameters_interface->has_parameter("ft_sensor.name")) { - auto p_ft_sensor_name = rclcpp::ParameterValue(ft_sensor_.name_); + auto p_ft_sensor_name = rclcpp::ParameterValue(params_.ft_sensor_.name_); parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); } else { - ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); + params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); } if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { - auto p_ft_sensor_frame_id = rclcpp::ParameterValue(ft_sensor_.frame_.id_); + auto p_ft_sensor_frame_id = rclcpp::ParameterValue(params_.ft_sensor_.frame_.id_); parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); } else { - ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); + params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); } if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { - auto p_ft_sensor_frame_external = rclcpp::ParameterValue(ft_sensor_.frame_.external_); + auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); } else { - ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); + params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); } if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { - auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(ft_sensor_.filter_coefficient_); + auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); } else { - ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( + params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( "ft_sensor.filter_coefficient").as_double(); } if (!parameters_interface->has_parameter("control.frame.id")) { - auto p_control_frame_id = rclcpp::ParameterValue(control_.frame_.id_); + auto p_control_frame_id = rclcpp::ParameterValue(params_.control_.frame_.id_); parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); } else { - control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); + params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); } if (!parameters_interface->has_parameter("control.frame.external")) { - auto p_control_frame_external = rclcpp::ParameterValue(control_.frame_.external_); + auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); } else { - control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); + params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); } if (!parameters_interface->has_parameter("fixed_world_frame.id")) { - auto p_fixed_world_frame_id = rclcpp::ParameterValue(fixed_world_frame_.id_); + auto p_fixed_world_frame_id = rclcpp::ParameterValue(params_.fixed_world_frame_.id_); parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); } else { - fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string(); + params_.fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string(); } if (!parameters_interface->has_parameter("fixed_world_frame.external")) { - auto p_fixed_world_frame_external = rclcpp::ParameterValue(fixed_world_frame_.external_); + auto p_fixed_world_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.external_); parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); } else { - fixed_world_frame_.external_ = parameters_interface->get_parameter("fixed_world_frame.external").as_bool(); + params_.fixed_world_frame_.external_ = parameters_interface->get_parameter( + "fixed_world_frame.external").as_bool(); } if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { - auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(gravity_compensation_.frame_.id_); + auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.id_); parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); } else { - gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( + params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( "gravity_compensation.frame.id").as_string(); } if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { - auto p_gravity_compensation_frame_external = rclcpp::ParameterValue(gravity_compensation_.frame_.external_); + auto p_gravity_compensation_frame_external = rclcpp::ParameterValue( + params_.gravity_compensation_.frame_.external_); parameters_interface->declare_parameter("gravity_compensation.frame.external", p_gravity_compensation_frame_external); } else { - gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( + params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( "gravity_compensation.frame.external").as_bool(); } if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { - auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(gravity_compensation_.CoG_.pos_); + auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.pos_); parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); } else { - gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( + params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( "gravity_compensation.CoG.pos").as_double_array(); } if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { - auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(gravity_compensation_.CoG_.force_); + auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); } else { - gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( + params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( "gravity_compensation.CoG.force").as_double(); } if (!parameters_interface->has_parameter("admittance.selected_axes")) { - auto p_admittance_selected_axes = rclcpp::ParameterValue(admittance_.selected_axes_); + auto p_admittance_selected_axes = rclcpp::ParameterValue(params_.admittance_.selected_axes_); parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); } else { - admittance_.selected_axes_ = parameters_interface->get_parameter("admittance.selected_axes").as_bool_array(); + params_.admittance_.selected_axes_ = parameters_interface->get_parameter( + "admittance.selected_axes").as_bool_array(); } if (!parameters_interface->has_parameter("admittance.mass")) { - auto p_admittance_mass = rclcpp::ParameterValue(admittance_.mass_); + auto p_admittance_mass = rclcpp::ParameterValue(params_.admittance_.mass_); parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); } else { - admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); + params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); } if (!parameters_interface->has_parameter("admittance.damping_ratio")) { - auto p_admittance_damping_ratio = rclcpp::ParameterValue(admittance_.damping_ratio_); + auto p_admittance_damping_ratio = rclcpp::ParameterValue(params_.admittance_.damping_ratio_); parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); } else { - admittance_.damping_ratio_ = parameters_interface->get_parameter("admittance.damping_ratio").as_double_array(); + params_.admittance_.damping_ratio_ = parameters_interface->get_parameter( + "admittance.damping_ratio").as_double_array(); } if (!parameters_interface->has_parameter("admittance.stiffness")) { - auto p_admittance_stiffness = rclcpp::ParameterValue(admittance_.stiffness_); + auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); } else { - admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); + params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); } if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( - enable_parameter_update_without_reactivation_); + params_.enable_parameter_update_without_reactivation_); parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", p_enable_parameter_update_without_reactivation); } else { - enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( + params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( "enable_parameter_update_without_reactivation").as_bool(); } if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { - auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(use_feedforward_commanded_input_); + auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); } else { - use_feedforward_commanded_input_ = parameters_interface->get_parameter( + params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter( "use_feedforward_commanded_input").as_bool(); } if (!parameters_interface->has_parameter("joint_limiter_type")) { - auto p_joint_limiter_type = rclcpp::ParameterValue(joint_limiter_type_); + auto p_joint_limiter_type = rclcpp::ParameterValue(params_.joint_limiter_type_); parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); } else { - joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string(); + params_.joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string(); } if (!parameters_interface->has_parameter("state_publish_rate")) { - auto p_state_publish_rate = rclcpp::ParameterValue(state_publish_rate_); + auto p_state_publish_rate = rclcpp::ParameterValue(params_.state_publish_rate_); parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); } else { - state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double(); + params_.state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double(); } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6c4e2770a1..f0d084163c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -74,7 +74,8 @@ namespace admittance_controller { } try { - admittance_->parameters_ = std::make_shared(get_node()->get_node_parameters_interface()); + admittance_->parameter_handler = std::make_shared(get_node()->get_node_parameters_interface()); + admittance_->parameters_ = std::make_shared(admittance_->parameter_handler->params_); } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; From 9990dcadf97e81442c5886af0c75093d4c230244 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 27 Jun 2022 22:45:24 -0600 Subject: [PATCH 024/111] remove unused code --- admittance_controller/CMakeLists.txt | 165 ++++++++---------- .../admittance_controller.hpp | 5 - .../admittance_controller/admittance_rule.hpp | 7 - .../admittance_rule_impl.hpp | 117 ++++++------- admittance_controller/package.xml | 2 - .../src/admittance_controller.cpp | 123 ++++++------- 6 files changed, 175 insertions(+), 244 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 294dd0a839..117b760ad6 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -1,13 +1,13 @@ cmake_minimum_required(VERSION 3.5) project(admittance_controller) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif () -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () # find dependencies find_package(ament_cmake REQUIRED) @@ -32,52 +32,38 @@ find_package(trajectory_msgs REQUIRED) find_package(angles REQUIRED) find_package(rcutils REQUIRED) find_package(tf2_kdl REQUIRED) -find_package(gen_param_struct REQUIRED) - # The admittance controller add_library(admittance_controller SHARED src/admittance_controller.cpp -) - -# specify which yaml root name to convert to struct -set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config/admittance_controller.yaml) -# specify which yaml root name to convert to struct -set(YAML_TARGET admittance_struct) -# specify which yaml root name to convert to struct -set(OUT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config) - -generate_param_struct_header(admittance_controller ${OUT_DIR} ${YAML_FILE} ${YAML_TARGET}) - - + ) target_include_directories( - admittance_controller - PRIVATE - include + admittance_controller + PRIVATE + include ) ament_target_dependencies( - admittance_controller - backward_ros - control_msgs - control_toolbox - controller_interface + admittance_controller + backward_ros + control_msgs + control_toolbox + controller_interface kinematics_interface -# ${Eigen_LIBRARIES} Eigen3 - geometry_msgs - hardware_interface - joint_trajectory_controller - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_eigen + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen tf2_kdl - tf2_geometry_msgs - tf2_ros + tf2_geometry_msgs + tf2_ros angles ) @@ -88,70 +74,69 @@ pluginlib_export_plugin_description_file(controller_interface admittance_control install( - TARGETS admittance_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib + TARGETS admittance_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) install( - DIRECTORY include/ - DESTINATION include + DIRECTORY include/ + DESTINATION include ) -set(BUILD_TESTING 0) -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) - target_include_directories(test_load_admittance_controller PRIVATE include) - ament_target_dependencies( - test_load_admittance_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - - ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) - target_include_directories(test_admittance_controller PRIVATE include) - target_link_libraries(test_admittance_controller admittance_controller) - ament_target_dependencies( - test_admittance_controller - control_msgs - controller_interface - hardware_interface - ros2_control_test_assets - ) -endif() +set(BUILD_TESTING 0) # TODO enable testing one test are fixed +if (BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) + target_include_directories(test_load_admittance_controller PRIVATE include) + ament_target_dependencies( + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) + target_include_directories(test_admittance_controller PRIVATE include) + target_link_libraries(test_admittance_controller admittance_controller) + ament_target_dependencies( + test_admittance_controller + control_msgs + controller_interface + hardware_interface + ros2_control_test_assets + ) +endif () ament_export_include_directories( - include + include ) ament_export_libraries( - admittance_controller + admittance_controller ) - ament_export_dependencies( - backward_ros - control_msgs - control_toolbox - controller_interface + backward_ros + control_msgs + control_toolbox + controller_interface kinematics_interface - geometry_msgs - hardware_interface - joint_trajectory_controller - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_eigen - tf2_geometry_msgs tf2_ros + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs tf2_ros ) ament_package() diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index b0467f08ca..3015f06e7f 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -145,15 +145,10 @@ namespace admittance_controller { trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, state_error_; trajectory_msgs::msg::JointTrajectory pre_admittance_point; - std::vector open_loop_buffer; // helper methods void joint_command_callback(const std::shared_ptr msg); - void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); - -// void read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); - void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 3c291bd189..082e1240f1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -73,17 +73,10 @@ namespace admittance_controller { public: - // TODO(destogl): Add parameter for this -// bool use_feedforward_commanded_input_ = true; - // Dynamic admittance config std::shared_ptr parameter_handler; std::shared_ptr parameters_; - // Filter parameter for exponential smoothing -// const double alpha = 0.005; // TODO make a ros param - - protected: /** * All values are in the controller frame diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index c11f6ef6a0..328d8d6600 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -26,7 +26,7 @@ namespace admittance_controller { controller_interface::return_type - AdmittanceRule::configure(const std::shared_ptr& node, int num_joints) { + AdmittanceRule::configure(const std::shared_ptr &node, int num_joints) { // configure admittance rule using num_joints and load kinematics interface num_joints_ = num_joints; @@ -42,7 +42,7 @@ namespace admittance_controller { // Load the differential IK plugin if (!parameters_->kinematics_.plugin_name_.empty()) { try { - kinematics_loader_ = std::make_shared>( + kinematics_loader_ = std::make_shared < pluginlib::ClassLoader < kinematics_interface::KinematicsBaseClass >> ( "kdl_plugin", "kinematics_interface::KinematicsBaseClass"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_->kinematics_.plugin_name_)); @@ -103,11 +103,11 @@ namespace admittance_controller { wrench_.setZero(); // parameters - if (parameter_handler->params_.enable_parameter_update_without_reactivation_){ + if (parameter_handler->params_.enable_parameter_update_without_reactivation_) { // point to the dynamic data - std::shared_ptr tmp {¶meter_handler->params_}; + std::shared_ptr tmp{¶meter_handler->params_}; parameters_ = tmp; - } else{ + } else { // make a copy of the data parameters_ = std::make_shared(); *parameters_ = parameter_handler->params_; @@ -116,7 +116,7 @@ namespace admittance_controller { return controller_interface::return_type::OK; } -// Update from reference joint states + // Update from reference joint states controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, const geometry_msgs::msg::Wrench &measured_wrench, @@ -127,14 +127,13 @@ namespace admittance_controller { double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; // update param values - memcpy(cog_.data(), parameters_->gravity_compensation_.CoG_.pos_.data(), 3*sizeof(double )); + memcpy(cog_.data(), parameters_->gravity_compensation_.CoG_.pos_.data(), 3 * sizeof(double)); ee_weight.setZero(); ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; mass_ = parameters_->admittance_.mass_; - selected_axes_= parameters_->admittance_.selected_axes_; + selected_axes_ = parameters_->admittance_.selected_axes_; stiffness_ = parameters_->admittance_.stiffness_; - for (auto i = 0ul; i < damping_.size(); ++i) - { + for (auto i = 0ul; i < damping_.size(); ++i) { damping_[i] = parameters_->admittance_.damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); } @@ -142,12 +141,17 @@ namespace admittance_controller { bool success = true; // get all needed transforms - world_transform = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, parameters_->fixed_world_frame_.external_, success); - control_transform = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, parameters_->control_.frame_.external_, success); - reference_ee_transform = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, false, success); + world_transform = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, + parameters_->fixed_world_frame_.external_, success); + control_transform = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, + parameters_->control_.frame_.external_, success); + reference_ee_transform = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, false, + success); ee_transform = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); - sensor_transform = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); - cog_transform = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, parameters_->gravity_compensation_.frame_.external_, success); + sensor_transform = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, + parameters_->ft_sensor_.frame_.external_, success); + cog_transform = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, + parameters_->gravity_compensation_.frame_.external_, success); // get all needed rotations ee_rot = ee_transform.block<3, 3>(0, 0); @@ -190,12 +194,14 @@ namespace admittance_controller { for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { joint_pos[i] += joint_vel[i] * dt;//- .2 * joint_pos[i] * (1.0 / 1000.0); desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos[i]; - state_message_.error_joint_state.positions[i] = reference_joint_state.positions[i] - current_joint_state.positions[i]; + state_message_.error_joint_state.positions[i] = + reference_joint_state.positions[i] - current_joint_state.positions[i]; } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel[i]; - state_message_.error_joint_state.velocities[i] = reference_joint_state.velocities[i] - current_joint_state.velocities[i]; + state_message_.error_joint_state.velocities[i] = + reference_joint_state.velocities[i] - current_joint_state.velocities[i]; } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { @@ -206,35 +212,6 @@ namespace admittance_controller { state_message_.actual_joint_state = current_joint_state; state_message_.desired_joint_state = desired_joint_state; - - -// state_message_.input_wrench_control_frame.wrench = wrench_control; - -// geometry_msgs/WrenchStamped input_wrench_command #commanded input wrench_ for the controller -// geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller -// trajectory_msgs/JointTrajectory input_joint_command # commanded input JointTrajectory (used only first point) -// -// geometry_msgs/WrenchStamped input_wrench_control_frame # input wrench_ transformed into control frame -// geometry_msgs/PoseStamped input_pose_control_frame # input pose transformed into control frame -// -// geometry_msgs/WrenchStamped measured_wrench # measured wrench_ from the sensor (sensor frame) -// geometry_msgs/WrenchStamped measured_wrench_filtered # measured wrench_ after low-pass and gravity compensation filters in sensor frame -// geometry_msgs/WrenchStamped measured_wrench_control_frame # transformed measured wrench_ to control frame -// geometry_msgs/WrenchStamped measured_wrench_endeffector_frame # measured wrench_ transformed to endeffector -// -// trajectory_msgs/JointTrajectoryPoint admittance_rule_calculated_values # Values calculated by admittance rule (Cartesian space: [x, y, z, rx, ry, rz]) - position = pose_error; effort = measured_wrench -// -// geometry_msgs/PoseStamped current_pose # Current pose from FK -// geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot -// geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule -// -// string[] joint_names -// trajectory_msgs/JointTrajectoryPoint desired_joint_state # result of IK from goal_pose_command -// trajectory_msgs/JointTrajectoryPoint actual_joint_state # read from the hardware -// trajectory_msgs/JointTrajectoryPoint error_joint_state - - - return controller_interface::return_type::OK; } @@ -249,11 +226,11 @@ namespace admittance_controller { if (selected_axes_[axis]) { double pose_error = -admittance_position_(axis, 3); admittance_acceleration_(axis, 0) = (1.0 / mass_[axis]) * (wrench(axis, 0) + - (damping_[axis] * - (desired_vel(axis, 0) - - admittance_velocity_(axis, 0))) + - (stiffness_[axis] * - pose_error)); + (damping_[axis] * + (desired_vel(axis, 0) - + admittance_velocity_(axis, 0))) + + (stiffness_[axis] * + pose_error)); admittance_velocity_(axis, 0) += admittance_acceleration_(axis, 0) * dt; admittance_position_(axis, 3) += admittance_velocity_(axis, 0) * dt; @@ -264,7 +241,6 @@ namespace admittance_controller { } } -// auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); Eigen::Vector3d V = get_rotation_axis(R); double theta = acos((1.0 / 2.0) * (R.trace() - 1)); @@ -277,16 +253,16 @@ namespace admittance_controller { if (selected_axes_[axis + 3]) { double pose_error = sign * theta * V(axis); admittance_acceleration_(axis, 1) = (1.0 / mass_[axis + 3]) * (wrench(axis, 1) + - (damping_[axis + 3] * - (desired_vel(axis, 1) - - admittance_velocity_(axis, 1))) + - (stiffness_[axis + 3] * - pose_error)); + (damping_[axis + 3] * + (desired_vel(axis, 1) - + admittance_velocity_(axis, 1))) + + (stiffness_[axis + 3] * + pose_error)); admittance_velocity_(axis, 1) += admittance_acceleration_(axis, 1) * dt; // store calculated values - state_message_.admittance_rule_calculated_values.velocities[axis+3] = admittance_velocity_(axis, 1); - state_message_.admittance_rule_calculated_values.accelerations[axis+3] = admittance_acceleration_(axis, 1); + state_message_.admittance_rule_calculated_values.velocities[axis + 3] = admittance_velocity_(axis, 1); + state_message_.admittance_rule_calculated_values.accelerations[axis + 3] = admittance_acceleration_(axis, 1); } } @@ -300,15 +276,18 @@ namespace admittance_controller { normalize_rotation(R); admittance_position_.block<3, 3>(0, 0) = R; // store calculated values - state_message_.admittance_rule_calculated_values.positions[0] = atan2(admittance_position_(2,1), admittance_position_(2,2)); - state_message_.admittance_rule_calculated_values.positions[0] = asin(admittance_position_(2,0)); - state_message_.admittance_rule_calculated_values.positions[0] = -atan2(admittance_position_(1,0), admittance_position_(0,0)); + state_message_.admittance_rule_calculated_values.positions[0] = atan2(admittance_position_(2, 1), + admittance_position_(2, 2)); + state_message_.admittance_rule_calculated_values.positions[0] = asin(admittance_position_(2, 0)); + state_message_.admittance_rule_calculated_values.positions[0] = -atan2(admittance_position_(1, 0), + admittance_position_(0, 0)); } Eigen::Matrix - AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, bool &success) { + AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, + bool &success) { Eigen::Matrix transform; - if (external){ + if (external) { transform.setIdentity(); try { auto transform_msg = tf_buffer_->lookupTransform(parameters_->kinematics_.base_, link_name, tf2::TimePointZero); @@ -321,7 +300,7 @@ namespace admittance_controller { rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); success = false; } - } else{ + } else { success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec); memcpy(transform.data(), transform_buffer_vec.data(), 16 * sizeof(double)); } @@ -464,11 +443,13 @@ namespace admittance_controller { eigen_to_msg(wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench_filtered); eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench); - eigen_to_msg(control_rot.transpose()*world_rot.transpose()*measured_wrench_, parameters_->control_.frame_.id_, state_message.measured_wrench_control_frame); - eigen_to_msg(ee_rot.transpose()*world_rot.transpose()*measured_wrench_, parameters_->kinematics_.tip_, state_message.measured_wrench_endeffector_frame); + eigen_to_msg(control_rot.transpose() * world_rot.transpose() * measured_wrench_, parameters_->control_.frame_.id_, + state_message.measured_wrench_control_frame); + eigen_to_msg(ee_rot.transpose() * world_rot.transpose() * measured_wrench_, parameters_->kinematics_.tip_, + state_message.measured_wrench_endeffector_frame); state_message.joint_names = parameters_->joints_; - state_message.desired_joint_state = state_message_.desired_joint_state; + state_message.desired_joint_state = state_message_.desired_joint_state; state_message.actual_joint_state = state_message_.actual_joint_state; state_message.error_joint_state = state_message_.error_joint_state; diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index c001b65888..dd92c8d421 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -30,8 +30,6 @@ tf2_ros trajectory_msgs gen_param_struct - - ament_cmake_gmock control_msgs diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f0d084163c..84ec935cd8 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -29,7 +29,8 @@ #include "rcutils/logging_macros.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr size_t +ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops namespace admittance_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -38,24 +39,28 @@ namespace admittance_controller { AdmittanceController::AdmittanceController() = default; void - AdmittanceController::joint_command_callback(const std::shared_ptr msg) { + AdmittanceController::joint_command_callback(const std::shared_ptr msg) { if (controller_is_active_) { rtBuffers.input_joint_command_.writeFromNonRT(msg); } } CallbackReturn AdmittanceController::on_init() { - // initialize controller config + admittance_ = std::make_unique(); -// admittance_->parameters_.initialize(get_node()); - // TODO need error handling - params = { - get_node()->get_parameter("joints").as_string_array(), - get_node()->get_parameter("command_interfaces").as_string_array(), - get_node()->get_parameter("state_interfaces").as_string_array(), - get_node()->get_parameter("chainable_command_interfaces").as_string_array(), - }; + try { + params = { + get_node()->get_parameter("joints").as_string_array(), + get_node()->get_parameter("command_interfaces").as_string_array(), + get_node()->get_parameter("state_interfaces").as_string_array(), + get_node()->get_parameter("chainable_command_interfaces").as_string_array(), + }; + } catch (const std::exception &e) { + RCLCPP_ERROR(get_node()->get_logger(), + "Exception thrown during init stage while reading parameters with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } for (const auto &tmp: params.state_interface_types_) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); @@ -67,15 +72,17 @@ namespace admittance_controller { if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); } else { - RCLCPP_ERROR(get_node()->get_logger(), "interface type %s is not supported. Supported types are %s and %s", + RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } } try { - admittance_->parameter_handler = std::make_shared(get_node()->get_node_parameters_interface()); - admittance_->parameters_ = std::make_shared(admittance_->parameter_handler->params_); + admittance_->parameter_handler = std::make_shared( + get_node()->get_node_parameters_interface()); + admittance_->parameters_ = std::make_shared( + admittance_->parameter_handler->params_); } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; @@ -92,7 +99,7 @@ namespace admittance_controller { // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported - std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); + std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); for (const auto &interface: params.state_interface_types_) { for (const auto &joint: params.joint_names_) { @@ -113,7 +120,7 @@ namespace admittance_controller { // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported - std::vector command_interfaces_config_names;//(joint_names_.size() * command_interface_types_.size()); + std::vector command_interfaces_config_names; for (const auto &interface: params.command_interface_types_) { for (const auto &joint: params.joint_names_) { @@ -125,16 +132,17 @@ namespace admittance_controller { command_interfaces_config_names}; } - std::vector AdmittanceController::on_export_reference_interfaces() { + std::vector AdmittanceController::on_export_reference_interfaces() { + // create CommandInterface interfaces that other controllers will be able to chain to - std::vector chainable_command_interfaces; + std::vector chainable_command_interfaces; auto num_chainable_interfaces = params.chainable_command_interface_types_.size() * params.joint_names_.size(); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); chainable_command_interfaces.reserve(num_chainable_interfaces); position_reference_.clear(); velocity_reference_.clear(); - std::unordered_map *> chainable_interface_map = { + std::unordered_map < std::string, std::vector < double * > * > chainable_interface_map = { {hardware_interface::HW_IF_POSITION, &position_reference_}, {hardware_interface::HW_IF_VELOCITY, &velocity_reference_}}; @@ -173,12 +181,14 @@ namespace admittance_controller { bool AdmittanceController::on_set_chained_mode(bool chained_mode) { // this method sets the chained mode to value of argument chained_mode if true is returned. + return true; } CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { // Print output so users can be sure the interface setup is correct - auto get_interface_list = [](const std::vector &interface_types) { + + auto get_interface_list = [](const std::vector &interface_types) { std::stringstream ss_command_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { if (index != 0) { @@ -199,7 +209,8 @@ namespace admittance_controller { std::bind(&AdmittanceController::joint_command_callback, this, std::placeholders::_1)); s_publisher_ = get_node()->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); - rtBuffers.state_publisher_ = std::make_unique>(s_publisher_); + rtBuffers.state_publisher_ = + std::make_unique < realtime_tools::RealtimePublisher < ControllerStateMsg >> (s_publisher_); // Initialize state message rtBuffers.state_publisher_->lock(); @@ -215,9 +226,6 @@ namespace admittance_controller { // configure admittance rule admittance_->configure(get_node(), num_joints_); -// // HACK: This is workaround because it seems that updating config only in `on_activate` does -// // not work properly: why? -// admittance_->parameters_.update(); return LifecycleNodeInterface::on_configure(previous_state); } @@ -237,21 +245,25 @@ namespace admittance_controller { joint_velocity_state_interface_.clear(); // assign state interfaces - std::unordered_map> *> command_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} - }; + std::unordered_map < std::string, + std::vector < std::reference_wrapper < hardware_interface::LoanedCommandInterface >> * > + command_interface_map = { + {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} + }; for (auto i = 0ul; i < params.command_interface_types_.size(); i++) { for (auto j = 0ul; j < params.joint_names_.size(); j++) { command_interface_map[params.command_interface_types_[i]]->emplace_back( command_interfaces_[i * num_joints_ + j]); } } + // assign command interfaces - std::unordered_map> *> state_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} - }; + std::unordered_map < std::string, + std::vector < std::reference_wrapper < hardware_interface::LoanedStateInterface >> * > state_interface_map = { + {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} + }; for (auto i = 0ul; i < params.state_interface_types_.size(); i++) { for (auto j = 0ul; j < params.joint_names_.size(); j++) { state_interface_map[params.state_interface_types_[i]]->emplace_back(state_interfaces_[i * num_joints_ + j]); @@ -319,7 +331,7 @@ namespace admittance_controller { } // save state reference before applying admittance rule TODO why is this here? - //pre_admittance_point.points[0] = state_reference_; TODO fix this + // pre_admittance_point.points[0] = state_reference_; // Publish controller state rtBuffers.state_publisher_->lock(); rtBuffers.state_publisher_->msg_.input_joint_command = pre_admittance_point; @@ -351,9 +363,8 @@ namespace admittance_controller { void AdmittanceController::read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint &state_current) { - // Fill fields of state_current argument from hardware state interfaces. If the hardware does not exist, - // the values are nan, that corresponding state field will be set to empty. If running in open loop - // control, all states fields will be empty + // Fill fields of state_current argument from hardware state interfaces. If the hardware does not exist or + // the values are nan, the corresponding state field will be set to empty. state_current.positions.resize(joint_position_state_interface_.size()); state_current.velocities.resize(joint_velocity_state_interface_.size()); @@ -362,7 +373,7 @@ namespace admittance_controller { // fill state message with values from hardware state interfaces // if any interface has nan values, assume state_current is the last command state for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { - state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); + state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); if (std::isnan(state_current.positions[i])) { state_current.positions = last_commanded_state_.positions; break; @@ -385,43 +396,10 @@ namespace admittance_controller { } -// void AdmittanceController::read_state_from_command_interfaces( -// trajectory_msgs::msg::JointTrajectoryPoint &commanded_state) { -// // Fill fields of commanded_state argument from hardware command interfaces. If the interface does not exist or -// // the values are nan, that corresponding state field will be set to empty -// -// commanded_state.positions.resize(joint_position_command_interface_.size(), 0.0); -// commanded_state.velocities.resize(joint_velocity_command_interface_.size(), 0.0); -// commanded_state.accelerations.resize(joint_acceleration_command_interface_.size(), 0.0); -// -// // fill state message with values from hardware command interfaces -// for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { -// commanded_state.positions[i] = joint_position_command_interface_[i].get().get_value(); -// if (std::isnan(commanded_state.positions[i])) { -// commanded_state.positions.clear(); -// break; -// } -// } -// for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { -// commanded_state.velocities[i] = joint_velocity_command_interface_[i].get().get_value(); -// if (std::isnan(commanded_state.velocities[i])) { -// commanded_state.velocities.clear(); -// break; -// } -// } -// for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { -// commanded_state.accelerations[i] = joint_acceleration_command_interface_[i].get().get_value(); -// if (std::isnan(commanded_state.accelerations[i])) { -// commanded_state.accelerations.clear(); -// break; -// } -// } -// } - void AdmittanceController::read_state_reference_interfaces( trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { // Fill fields of state_reference argument from controller references. If the interface does not exist or - // the values are nan, that corresponding field will be set to empty + // the values are nan, the corresponding field will be set to empty state_reference.positions.resize(position_reference_.size(), 0.0); state_reference.velocities.resize(velocity_reference_.size(), 0.0); @@ -448,4 +426,5 @@ namespace admittance_controller { #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(admittance_controller::AdmittanceController, - controller_interface::ChainableControllerInterface) + controller_interface::ChainableControllerInterface +) From 690b89be41e797956fa8389235c54fa9f6810afe Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 28 Jun 2022 08:50:39 -0600 Subject: [PATCH 025/111] eliminate ik drift: broken --- .../admittance_controller/admittance_rule.hpp | 65 +++--- .../admittance_rule_impl.hpp | 190 ++++++++++-------- .../src/admittance_controller.cpp | 6 +- 3 files changed, 140 insertions(+), 121 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 3c291bd189..18086932ce 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -48,7 +48,9 @@ namespace admittance_controller { class AdmittanceRule { public: - AdmittanceRule() = default; + AdmittanceRule(){ + parameters_ = ¶meters_copy; + } controller_interface::return_type configure(const std::shared_ptr& node, int num_joint); controller_interface::return_type reset(); @@ -73,16 +75,10 @@ namespace admittance_controller { public: - // TODO(destogl): Add parameter for this -// bool use_feedforward_commanded_input_ = true; - - // Dynamic admittance config - std::shared_ptr parameter_handler; - std::shared_ptr parameters_; - - // Filter parameter for exponential smoothing -// const double alpha = 0.005; // TODO make a ros param - + // admittance config parameters + std::shared_ptr parameter_handler_; + admittance_struct_parameters::admittance_struct::params* parameters_; + admittance_struct_parameters::admittance_struct::params parameters_copy; protected: /** @@ -105,6 +101,7 @@ namespace admittance_controller { const std::vector &joint_delta, bool &success); void normalize_rotation(Eigen::Matrix& R); + Eigen::Matrix invert_transform(Eigen::Matrix T); Eigen::Matrix get_transform(const std::vector& positions, const std::string & link_name, bool external, bool & success); void eigen_to_msg(const Eigen::Matrix& wrench, const std::string& frame_id, geometry_msgs::msg::WrenchStamped& wrench_msg); @@ -116,42 +113,42 @@ namespace admittance_controller { int num_joints_; // buffers to pass data to kinematics interface - std::vector transform_buffer_vec; - std::vector joint_buffer_vec; - std::vector cart_buffer_vec; + std::vector transform_buffer_vec_; + std::vector joint_buffer_vec_; + std::vector cart_buffer_vec_; // admittance controller values - Eigen::Matrix admittance_acceleration_; - Eigen::Matrix admittance_velocity_; + Eigen::Matrix admittance_acceleration_; + Eigen::Matrix admittance_velocity_; Eigen::Matrix admittance_position_; // transforms - Eigen::Matrix ee_transform; - Eigen::Matrix reference_ee_transform; - Eigen::Matrix sensor_transform; - Eigen::Matrix control_transform; - Eigen::Matrix cog_transform; - Eigen::Matrix world_transform; + Eigen::Matrix ee_transform_; + Eigen::Matrix reference_ee_transform_; + Eigen::Matrix sensor_transform_; + Eigen::Matrix control_transform_; + Eigen::Matrix cog_transform_; + Eigen::Matrix world_transform_; // rotations - Eigen::Matrix ee_rot; - Eigen::Matrix control_rot; - Eigen::Matrix sensor_rot; - Eigen::Matrix cog_rot; - Eigen::Matrix world_rot; + Eigen::Matrix ee_rot_; + Eigen::Matrix control_rot_; + Eigen::Matrix sensor_rot_; + Eigen::Matrix cog_rot_; + Eigen::Matrix world_rot_; // external force - Eigen::Matrix wrench_; - Eigen::Matrix measured_wrench_; + Eigen::Matrix wrench_; + Eigen::Matrix measured_wrench_; // position of center of gravity in cog_frame - Eigen::Matrix cog_; + Eigen::Vector3d cog_; // force applied to sensor due to weight of end effector - Eigen::Matrix ee_weight; + Eigen::Vector3d ee_weight; // admittance controller values in joint space - std::vector joint_vel; - std::vector joint_acc; - std::vector joint_pos; + std::vector joint_pos_; + std::vector joint_vel_; + std::vector joint_acc_; std::vector damping_; std::vector mass_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index c11f6ef6a0..bfd95c51b2 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, PickNik, Inc. +// Copyright (c) 2022, PickNik, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -68,9 +68,9 @@ namespace admittance_controller { // reset all values back to zero //allocate dynamic buffers - joint_buffer_vec.assign(num_joints_, 0.0); - transform_buffer_vec.assign(16, 0.0); - cart_buffer_vec.assign(6, 0.0); + joint_buffer_vec_.assign(num_joints_, 0.0); + transform_buffer_vec_.assign(16, 0.0); + cart_buffer_vec_.assign(6, 0.0); state_message_.error_joint_state.positions.resize(num_joints_, 0.0); state_message_.error_joint_state.velocities.resize(num_joints_, 0.0); @@ -84,17 +84,17 @@ namespace admittance_controller { selected_axes_.assign(6, false); // admittance state vectors in joint space - joint_pos.resize(num_joints_, 0.0); - joint_vel.resize(num_joints_, 0.0); - joint_acc.resize(num_joints_, 0.0); + joint_pos_.resize(num_joints_, 0.0); + joint_vel_.resize(num_joints_, 0.0); + joint_acc_.resize(num_joints_, 0.0); // transforms - ee_transform.setIdentity(); - reference_ee_transform.setIdentity(); - sensor_transform.setIdentity(); - control_transform.setIdentity(); - cog_transform.setIdentity(); - world_transform.setIdentity(); + ee_transform_.setIdentity(); + reference_ee_transform_.setIdentity(); + sensor_transform_.setIdentity(); + control_transform_.setIdentity(); + cog_transform_.setIdentity(); + world_transform_.setIdentity(); // admittance values admittance_position_.setIdentity(); @@ -103,14 +103,12 @@ namespace admittance_controller { wrench_.setZero(); // parameters - if (parameter_handler->params_.enable_parameter_update_without_reactivation_){ + if (parameter_handler_->params_.enable_parameter_update_without_reactivation_){ // point to the dynamic data - std::shared_ptr tmp {¶meter_handler->params_}; - parameters_ = tmp; + parameters_ = ¶meter_handler_->params_; } else{ // make a copy of the data - parameters_ = std::make_shared(); - *parameters_ = parameter_handler->params_; + parameters_ = ¶meters_copy; } return controller_interface::return_type::OK; @@ -142,43 +140,90 @@ namespace admittance_controller { bool success = true; // get all needed transforms - world_transform = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, parameters_->fixed_world_frame_.external_, success); - control_transform = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, parameters_->control_.frame_.external_, success); - reference_ee_transform = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, false, success); - ee_transform = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); - sensor_transform = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); - cog_transform = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, parameters_->gravity_compensation_.frame_.external_, success); + world_transform_ = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, parameters_->fixed_world_frame_.external_, success); + control_transform_ = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, parameters_->control_.frame_.external_, success); + reference_ee_transform_ = get_transform(reference_joint_state.positions, parameters_->kinematics_.tip_, false, success); + ee_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); + sensor_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); + cog_transform_ = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, parameters_->gravity_compensation_.frame_.external_, success); // get all needed rotations - ee_rot = ee_transform.block<3, 3>(0, 0); - control_rot = control_transform.block<3, 3>(0, 0); - sensor_rot = sensor_transform.block<3, 3>(0, 0); - cog_rot = cog_transform.block<3, 3>(0, 0); - world_rot = world_transform.block<3, 3>(0, 0); + ee_rot_ = ee_transform_.block<3, 3>(0, 0); + control_rot_ = control_transform_.block<3, 3>(0, 0); + sensor_rot_ = sensor_transform_.block<3, 3>(0, 0); + cog_rot_ = cog_transform_.block<3, 3>(0, 0); + world_rot_ = world_transform_.block<3, 3>(0, 0); // apply filter and update wrench_ vector - process_wrench_measurements(measured_wrench, world_rot * sensor_rot, world_rot * cog_rot); + process_wrench_measurements(measured_wrench, world_rot_ * sensor_rot_, world_rot_ * cog_rot_); // transform wrench_ into control frame - Eigen::Matrix wrench_control = control_rot.transpose() * wrench_; + Eigen::Matrix wrench_control = control_rot_.transpose() * wrench_; // calculate desired cartesian velocity in control frame Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, current_joint_state.velocities, success); - desired_vel = control_rot.transpose() * desired_vel * parameters_->use_feedforward_commanded_input_; + desired_vel = control_rot_.transpose() * desired_vel * parameters_->use_feedforward_commanded_input_; // Compute admittance control law: F = M*a + D*v + S*(x - x_d) calculate_admittance_rule(wrench_control, desired_vel, dt); // transform admittance values back to base frame - auto admittance_velocity_base = control_rot * admittance_velocity_; + auto admittance_position_base = admittance_position_; + + + // update joint desired joint state + for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { + desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos_[i]; + } + //add correction + for (int i = 0; i < 10; i++){ + Eigen::Matrix val; +// Eigen::Matrix admittance_rot = admittance_position_base.block<3, 3>(0, 0); + + // position difference + Eigen::Matrix ee_transform_i = get_transform(desired_joint_state.positions, parameters_->kinematics_.tip_, false, success)*invert_transform(reference_ee_transform_); +// Eigen::Matrix ee_transform_i = invert_transform(reference_ee_transform_)*get_transform(desired_joint_state.positions, parameters_->kinematics_.tip_, false, success); + + Eigen::Vector3d ee_pos = control_rot_.transpose()*ee_transform_i.block<3,1>(0,3); + Eigen::Matrix ee_rot = ee_transform_i.block<3,3>(0,0); + + val(0,0) = admittance_position_base(0,3) - ee_pos(0); + val(1,0) = admittance_position_base(1,3) - ee_pos(1); + val(2,0) = admittance_position_base(2,3) - ee_pos(2); + // orientation difference +// Eigen::Matrix ee_rot = ee_transform_i.block<3, 3>(0, 0); + Eigen::Matrix admittance_rot = admittance_position_base.block<3, 3>(0, 0); + Eigen::Matrix R = control_rot_.transpose()*ee_rot*admittance_rot.transpose(); + Eigen::Vector3d V = get_rotation_axis(R); + double theta = acos(std::max((1.0 / 2.0) * (R.trace() - 1), 1.0)); + theta = 0; + auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) + + V[2] * (R(0, 1) - R(1, 0)); + double sign = (tmp >= 0) ? 1.0 : -1.0; + val.block<3,1>(0,1) = .1*sign*theta*V; + + val.block<3,1>(0,0) = control_rot_*val.block<3,1>(0,0); + val.block<3,1>(0,1) = control_rot_*val.block<3,1>(0,1); + + convert_cartesian_deltas_to_joint_deltas(desired_joint_state.positions, val, + joint_vel_, success); + + for (auto j = 0ul; j < reference_joint_state.positions.size(); j++) { + joint_pos_[j] += 0.1*joint_vel_[j]; + desired_joint_state.positions[j] = reference_joint_state.positions[j] + joint_pos_[j]; + } + } + + + auto admittance_velocity_base = control_rot_ * admittance_velocity_; convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_base, - joint_vel, success); - auto admittance_acceleration_base = control_rot * admittance_acceleration_; + joint_vel_, success); + auto admittance_acceleration_base = control_rot_ * admittance_acceleration_; convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_base, - joint_acc, success); + joint_acc_, success); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference if (!success) { @@ -188,53 +233,24 @@ namespace admittance_controller { // update joint desired joint state for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { - joint_pos[i] += joint_vel[i] * dt;//- .2 * joint_pos[i] * (1.0 / 1000.0); - desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos[i]; + joint_pos_[i] += joint_vel_[i] * dt;//- .2 * joint_pos_[i] * (1.0 / 1000.0); + desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos_[i]; state_message_.error_joint_state.positions[i] = reference_joint_state.positions[i] - current_joint_state.positions[i]; } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { - desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel[i]; + desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel_[i]; state_message_.error_joint_state.velocities[i] = reference_joint_state.velocities[i] - current_joint_state.velocities[i]; } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { - desired_joint_state.accelerations[i] = joint_acc[i]; + desired_joint_state.accelerations[i] = joint_acc_[i]; } // update admittance state message state_message_.actual_joint_state = current_joint_state; state_message_.desired_joint_state = desired_joint_state; - - -// state_message_.input_wrench_control_frame.wrench = wrench_control; - -// geometry_msgs/WrenchStamped input_wrench_command #commanded input wrench_ for the controller -// geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller -// trajectory_msgs/JointTrajectory input_joint_command # commanded input JointTrajectory (used only first point) -// -// geometry_msgs/WrenchStamped input_wrench_control_frame # input wrench_ transformed into control frame -// geometry_msgs/PoseStamped input_pose_control_frame # input pose transformed into control frame -// -// geometry_msgs/WrenchStamped measured_wrench # measured wrench_ from the sensor (sensor frame) -// geometry_msgs/WrenchStamped measured_wrench_filtered # measured wrench_ after low-pass and gravity compensation filters in sensor frame -// geometry_msgs/WrenchStamped measured_wrench_control_frame # transformed measured wrench_ to control frame -// geometry_msgs/WrenchStamped measured_wrench_endeffector_frame # measured wrench_ transformed to endeffector -// -// trajectory_msgs/JointTrajectoryPoint admittance_rule_calculated_values # Values calculated by admittance rule (Cartesian space: [x, y, z, rx, ry, rz]) - position = pose_error; effort = measured_wrench -// -// geometry_msgs/PoseStamped current_pose # Current pose from FK -// geometry_msgs/PoseStamped desired_pose # goal pose to send to the robot -// geometry_msgs/TransformStamped relative_desired_pose # relative pose from the actual pose as result of the admittance rule -// -// string[] joint_names -// trajectory_msgs/JointTrajectoryPoint desired_joint_state # result of IK from goal_pose_command -// trajectory_msgs/JointTrajectoryPoint actual_joint_state # read from the hardware -// trajectory_msgs/JointTrajectoryPoint error_joint_state - - - return controller_interface::return_type::OK; } @@ -267,7 +283,7 @@ namespace admittance_controller { // auto R = admittance_position_(Eigen::seq(0,2), Eigen::seq(0,2)); Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); Eigen::Vector3d V = get_rotation_axis(R); - double theta = acos((1.0 / 2.0) * (R.trace() - 1)); + double theta = acos(std::max((1.0 / 2.0) * (R.trace() - 1), 1.0)); // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) + V[2] * (R(0, 1) - R(1, 0)); @@ -322,16 +338,16 @@ namespace admittance_controller { success = false; } } else{ - success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec); - memcpy(transform.data(), transform_buffer_vec.data(), 16 * sizeof(double)); + success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); + memcpy(transform.data(), transform_buffer_vec_.data(), 16 * sizeof(double)); } return transform; } void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &cur_sensor_rot, - const Eigen::Matrix &cog_rot + const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &sensor_world_rot, + const Eigen::Matrix &cog_world_rot ) { Eigen::Matrix new_wrench; new_wrench(0, 0) = measured_wrench.force.x; @@ -342,13 +358,13 @@ namespace admittance_controller { new_wrench(2, 1) = measured_wrench.torque.z; // transform to base frame - Eigen::Matrix new_wrench_base = cur_sensor_rot * new_wrench; + Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; // store value for state message measured_wrench_ = new_wrench_base; // apply gravity compensation new_wrench_base(2, 0) -= ee_weight[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_rot * cog_).cross(ee_weight); + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight); for (auto i = 0; i < 6; i++) { wrench_(i) = filters::exponentialSmoothing( @@ -443,11 +459,21 @@ namespace admittance_controller { } + Eigen::Matrix + AdmittanceRule::invert_transform(Eigen::Matrix T) { + Eigen::Matrix3d R = T.block<3, 3>(0, 0); + Eigen::Matrix4d T_inv = T; + T_inv.block<3, 3>(0, 0) = R.transpose(); + T_inv.block<3, 1>(0, 3) = -R.transpose() * T_inv.block<3, 1>(0, 3); + return T_inv; + } + + void AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, const Eigen::Matrix &cartesian_delta, std::vector &joint_delta, bool &success) { - memcpy(cart_buffer_vec.data(), cartesian_delta.data(), 6 * sizeof(double)); - success &= kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec, joint_delta); + memcpy(cart_buffer_vec_.data(), cartesian_delta.data(), 6 * sizeof(double)); + success &= kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec_, joint_delta); } Eigen::Matrix @@ -455,8 +481,8 @@ namespace admittance_controller { const std::vector &joint_delta, bool &success) { Eigen::Matrix cartesian_delta; - success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, cart_buffer_vec); - memcpy(cartesian_delta.data(), cart_buffer_vec.data(), 6 * sizeof(double)); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, cart_buffer_vec_); + memcpy(cartesian_delta.data(), cart_buffer_vec_.data(), 6 * sizeof(double)); return cartesian_delta; } @@ -464,8 +490,8 @@ namespace admittance_controller { eigen_to_msg(wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench_filtered); eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench); - eigen_to_msg(control_rot.transpose()*world_rot.transpose()*measured_wrench_, parameters_->control_.frame_.id_, state_message.measured_wrench_control_frame); - eigen_to_msg(ee_rot.transpose()*world_rot.transpose()*measured_wrench_, parameters_->kinematics_.tip_, state_message.measured_wrench_endeffector_frame); + eigen_to_msg(control_rot_.transpose()*world_rot_.transpose()*measured_wrench_, parameters_->control_.frame_.id_, state_message.measured_wrench_control_frame); + eigen_to_msg(ee_rot_.transpose()*world_rot_.transpose()*measured_wrench_, parameters_->kinematics_.tip_, state_message.measured_wrench_endeffector_frame); state_message.joint_names = parameters_->joints_; state_message.desired_joint_state = state_message_.desired_joint_state; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f0d084163c..9232b23f1e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -74,8 +74,7 @@ namespace admittance_controller { } try { - admittance_->parameter_handler = std::make_shared(get_node()->get_node_parameters_interface()); - admittance_->parameters_ = std::make_shared(admittance_->parameter_handler->params_); + admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; @@ -215,9 +214,6 @@ namespace admittance_controller { // configure admittance rule admittance_->configure(get_node(), num_joints_); -// // HACK: This is workaround because it seems that updating config only in `on_activate` does -// // not work properly: why? -// admittance_->parameters_.update(); return LifecycleNodeInterface::on_configure(previous_state); } From 79a54dce779a80ae9dcc3362ed9210b195e5c3f8 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 29 Jun 2022 16:43:06 -0600 Subject: [PATCH 026/111] working drift correction for position and rotation --- .../admittance_controller/admittance_rule.hpp | 25 +-- .../admittance_rule_impl.hpp | 208 +++++++++--------- 2 files changed, 111 insertions(+), 122 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f24639b2f..4d3a58c76d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -104,14 +104,6 @@ namespace admittance_controller { const std::string &link_name, Eigen::Matrix &joint_delta, bool &success); - void convert_cartesian_deltas_to_joint_deltas_with_target(const std::vector &positions, - const Eigen::Matrix &joint_target, - double weight, - Eigen::Matrix &cartesian_delta, - const std::string &link_name, - Eigen::Matrix &joint_delta, - bool &success); - Eigen::Matrix convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, const std::vector &joint_delta, const std::string &link_name, @@ -119,14 +111,15 @@ namespace admittance_controller { void normalize_rotation(Eigen::Matrix &R); - Eigen::Matrix invert_transform(Eigen::Matrix T); - Eigen::Matrix get_transform(const std::vector &positions, const std::string &link_name, bool external, bool &success); void eigen_to_msg(const Eigen::Matrix &wrench, const std::string &frame_id, geometry_msgs::msg::WrenchStamped &wrench_msg); + template + void vec_to_eigen(const std::vector data, T2 &matrix); + // Kinematics interface plugin loader std::shared_ptr> kinematics_loader_; std::unique_ptr kinematics_; @@ -162,7 +155,7 @@ namespace admittance_controller { Eigen::Matrix world_rot_; // external force - Eigen::Matrix wrench_; + Eigen::Matrix wrench_world_; Eigen::Matrix measured_wrench_; // position of center of gravity in cog_frame Eigen::Vector3d cog_; @@ -173,12 +166,12 @@ namespace admittance_controller { Eigen::Matrix joint_pos_; Eigen::Matrix joint_vel_; Eigen::Matrix joint_acc_; - Eigen::Matrix joint_target_; - std::vector damping_; - std::vector mass_; - std::vector selected_axes_; - std::vector stiffness_; + Eigen::Matrix damping_; + Eigen::Matrix mass_; + Eigen::Matrix mass_inv_; + Eigen::Matrix selected_axes_; + Eigen::Matrix stiffness_; // jacobian Eigen::Matrix jacobian_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 815b879b28..dc0bca0dce 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -79,16 +79,11 @@ namespace admittance_controller { state_message_.admittance_rule_calculated_values.velocities.resize(6, 0.0); state_message_.admittance_rule_calculated_values.accelerations.resize(6, 0.0); - damping_.assign(6, 0.0); - mass_.assign(6, 0.0); - stiffness_.assign(6, 0.0); - selected_axes_.assign(6, false); // admittance state vectors in joint space joint_pos_ = Eigen::Matrix(num_joints_); joint_vel_ = Eigen::Matrix(num_joints_); joint_acc_ = Eigen::Matrix(num_joints_); - joint_target_ = Eigen::Matrix(num_joints_); // transforms ee_transform_.setIdentity(); @@ -103,11 +98,16 @@ namespace admittance_controller { admittance_position_.setIdentity(); admittance_velocity_.setZero(); admittance_acceleration_.setZero(); - wrench_.setZero(); + wrench_world_.setZero(); jacobian_ = Eigen::Matrix(6, num_joints_); jacobian_.setZero(); identity = Eigen::Matrix(num_joints_, num_joints_); identity.setIdentity(); + damping_.setZero(); + mass_.setOnes(); + mass_inv_.setZero(); + stiffness_.setZero(); + selected_axes_.setZero(); // parameters if (parameter_handler_->params_.enable_parameter_update_without_reactivation_) { @@ -132,13 +132,14 @@ namespace admittance_controller { double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; // update param values - memcpy(cog_.data(), parameters_->gravity_compensation_.CoG_.pos_.data(), 3 * sizeof(double)); ee_weight.setZero(); ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; - mass_ = parameters_->admittance_.mass_; - selected_axes_ = parameters_->admittance_.selected_axes_; - stiffness_ = parameters_->admittance_.stiffness_; - for (auto i = 0ul; i < damping_.size(); ++i) { + vec_to_eigen(parameters_->admittance_.mass_, mass_); + vec_to_eigen(parameters_->admittance_.stiffness_, stiffness_); + vec_to_eigen(parameters_->admittance_.selected_axes_, selected_axes_); + + for (auto i = 0ul; i < 6; ++i) { + mass_inv_[i] = 1.0 / parameters_->admittance_.mass_[i]; damping_[i] = parameters_->admittance_.damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); } @@ -151,10 +152,12 @@ namespace admittance_controller { reference_ft_transform_ = get_transform(reference_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); + ft_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, + parameters_->ft_sensor_.frame_.external_, success); + ee_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); - ft_transform_ = get_transform(current_joint_state.positions, parameters_->control_.frame_.id_, - parameters_->control_.frame_.external_, success); + world_transform_ = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, parameters_->fixed_world_frame_.external_, success); sensor_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, @@ -169,45 +172,47 @@ namespace admittance_controller { cog_rot_ = cog_transform_.block<3, 3>(0, 0); world_rot_ = world_transform_.block<3, 3>(0, 0); - // apply filter and update wrench_ vector + // apply filter and update wrench_world_ vector process_wrench_measurements(measured_wrench, world_rot_ * sensor_rot_, world_rot_ * cog_rot_); - // transform wrench_ into control frame - Eigen::Matrix wrench_control = control_rot_.transpose() * wrench_; + // transform wrench_world_ into control frame + Eigen::Matrix wrench_base = world_rot_.transpose() * wrench_world_; // calculate desired cartesian velocity in control frame Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, current_joint_state.velocities, parameters_->ft_sensor_.frame_.id_, success); + // set desired_vel to zero if not using feed forward input + desired_vel = desired_vel * parameters_->use_feedforward_commanded_input_; - desired_vel = control_rot_.transpose() * desired_vel * parameters_->use_feedforward_commanded_input_; + // Compute admittance control law + calculate_admittance_rule(wrench_base, desired_vel, dt); - // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - calculate_admittance_rule(wrench_control, desired_vel, dt); + // calculate drift due to integrating the joint positions + Eigen::Matrix admittance_position_base = reference_ft_transform_.block<3,1>(0, 3) + admittance_position_.block<3,1>(0, 3); + Eigen::Matrix admittance_actually_position_base = ft_transform_.block<3,1>(0,3); - // convert to base frame - Eigen::Matrix admittance_velocity_base = control_rot_ * admittance_velocity_; - Eigen::Matrix admittance_acceleration_base = control_rot_ * admittance_acceleration_; + Eigen::Matrix ft_rot = ft_transform_.block<3,3>(0,0); + Eigen::Matrix reference_ft_rot = reference_ft_transform_.block<3,3>(0,0); + Eigen::Matrix admittance_rot = admittance_position_.block<3,3>(0,0); + Eigen::Matrix R = admittance_rot.transpose()*(reference_ft_rot.transpose()*reference_ft_rot); - // calculate drift and correction term due to integrating the joint positions + // calculate correction term Eigen::Matrix correction_velocity; - correction_velocity.setZero(); - Eigen::Matrix admittance_position_base = reference_ft_transform_.block<3,1>(0, 3) + control_rot_ * admittance_position_.block<3,1>(0, 3); - Eigen::Matrix admittance_actually_position_base = ft_transform_.block<3,1>(0,3); - - correction_velocity.block<3,1>(0,0) = 2*(admittance_position_base - admittance_actually_position_base); - convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, correction_velocity, - parameters_->ft_sensor_.frame_.id_, - joint_target_, success); + correction_velocity.block<3, 1>(0, 0) = (admittance_position_base - admittance_actually_position_base); + Eigen::Vector3d V = get_rotation_axis(R); + double theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); + auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) + + V[2] * (R(0, 1) - R(1, 0)); + double sign = (tmp >= 0) ? 1.0 : -1.0; + correction_velocity.block<3, 1>(0, 1) = .1*sign*theta*V; - // weight the correction term by the error in position - double weight = correction_velocity.block<3,1>(0,0).norm(); - convert_cartesian_deltas_to_joint_deltas_with_target(current_joint_state.positions, joint_target_, weight, - admittance_velocity_base, parameters_->ft_sensor_.frame_.id_, - joint_vel_, success); + convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_ + correction_velocity, + parameters_->ft_sensor_.frame_.id_, + joint_vel_, success); - convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_base, + convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_, parameters_->ft_sensor_.frame_.id_, joint_acc_, success); @@ -246,51 +251,57 @@ namespace admittance_controller { const Eigen::Matrix &desired_vel, const double dt ) { - // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - - for (size_t axis = 0; axis < 3; ++axis) { - if (selected_axes_[axis]) { - double pose_error = -admittance_position_(axis, 3); - admittance_acceleration_(axis, 0) = (1.0 / mass_[axis]) * (wrench(axis, 0) + - (damping_[axis] * - (desired_vel(axis, 0) - - admittance_velocity_(axis, 0))) + - (stiffness_[axis] * - pose_error)); - admittance_velocity_(axis, 0) += - admittance_acceleration_(axis, 0) * dt; - admittance_position_(axis, 3) += admittance_velocity_(axis, 0) * dt; - // store calculated values - state_message_.admittance_rule_calculated_values.positions[axis] = admittance_position_(axis, 3); - state_message_.admittance_rule_calculated_values.velocities[axis] = admittance_velocity_(axis, 0); - state_message_.admittance_rule_calculated_values.accelerations[axis] = admittance_acceleration_(axis, 0); - } - } + // reshape inputs + auto admittance_velocity_flat = Eigen::Matrix(admittance_velocity_.data()); + auto wrench_flat = Eigen::Matrix(wrench.data()); + const auto desired_vel_flat = Eigen::Matrix(desired_vel.data()); + + // create stiffness matrix + Eigen::Matrix K = Eigen::Matrix::Zero(); + Eigen::Matrix K_pos = Eigen::Matrix::Zero(); + Eigen::Matrix K_rot = Eigen::Matrix::Zero(); + K_pos.diagonal() = stiffness_.block<3,1>(0,0); + K_rot.diagonal() = stiffness_.block<3,1>(3,0); + K_pos = control_rot_.transpose()*K_pos*control_rot_; + K_rot = control_rot_.transpose()*K_rot*control_rot_; + K.block<3,3>(0,0) = K_pos; + K.block<3,3>(3,3) = K_rot; + + // create damping matrix + Eigen::Matrix D = Eigen::Matrix::Zero(); + Eigen::Matrix D_pos = Eigen::Matrix::Zero(); + Eigen::Matrix D_rot = Eigen::Matrix::Zero(); + D_pos.diagonal() = damping_.block<3,1>(0,0); + D_rot.diagonal() = damping_.block<3,1>(3,0); + D_pos = control_rot_.transpose()*D_pos*control_rot_; + D_rot = control_rot_.transpose()*D_rot*control_rot_; + D.block<3,3>(0,0) = D_pos; + D.block<3,3>(3,3) = D_rot; + + // calculate pose error + Eigen::Matrix pose_error; + pose_error.block<3, 1>(0, 0) = -admittance_position_.block<3, 1>(0, 3); + + // calculate rotation error Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); Eigen::Vector3d V = get_rotation_axis(R); - double theta = acos(std::max((1.0 / 2.0) * (R.trace() - 1), 1.0)); + double theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) + V[2] * (R(0, 1) - R(1, 0)); double sign = (tmp >= 0) ? 1.0 : -1.0; + pose_error.block<3, 1>(3, 0) = sign * theta * V; - for (size_t axis = 0; axis < 3; ++axis) { - if (selected_axes_[axis + 3]) { - double pose_error = sign * theta * V(axis); - admittance_acceleration_(axis, 1) = (1.0 / mass_[axis + 3]) * (wrench(axis, 1) + - (damping_[axis + 3] * - (desired_vel(axis, 1) - - admittance_velocity_(axis, 1))) + - (stiffness_[axis + 3] * - pose_error)); - admittance_velocity_(axis, 1) += - admittance_acceleration_(axis, 1) * dt; - // store calculated values - state_message_.admittance_rule_calculated_values.velocities[axis + 3] = admittance_velocity_(axis, 1); - state_message_.admittance_rule_calculated_values.accelerations[axis + 3] = admittance_acceleration_(axis, 1); - } - } + // Compute admittance control law: F = M*a + D*v + S*(x - x_d) + Eigen::Matrix admittance_acceleration_flat = mass_inv_.cwiseProduct(wrench_flat + + D*(desired_vel_flat-admittance_velocity_flat) + + K*pose_error); + // reshape admittance outputs + admittance_acceleration_ = Eigen::Matrix(admittance_acceleration_flat.data()); + admittance_velocity_ += admittance_acceleration_*dt; + + admittance_position_.block<3, 1>(0, 3) += admittance_velocity_.block<3,1>(0,0)*dt; Eigen::Matrix3d skew_symmetric; skew_symmetric << 0, -admittance_velocity_(2, 1), admittance_velocity_(1, 1), @@ -301,12 +312,14 @@ namespace admittance_controller { R += R_dot * dt; normalize_rotation(R); admittance_position_.block<3, 3>(0, 0) = R; + // store calculated values state_message_.admittance_rule_calculated_values.positions[0] = atan2(admittance_position_(2, 1), admittance_position_(2, 2)); state_message_.admittance_rule_calculated_values.positions[0] = asin(admittance_position_(2, 0)); state_message_.admittance_rule_calculated_values.positions[0] = -atan2(admittance_position_(1, 0), admittance_position_(0, 0)); + } void AdmittanceRule::process_wrench_measurements( @@ -331,8 +344,8 @@ namespace admittance_controller { new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight); for (auto i = 0; i < 6; i++) { - wrench_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_(i), parameters_->ft_sensor_.filter_coefficient_); + wrench_world_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_world_(i), parameters_->ft_sensor_.filter_coefficient_); } } @@ -423,15 +436,6 @@ namespace admittance_controller { } - Eigen::Matrix - AdmittanceRule::invert_transform(Eigen::Matrix T) { - Eigen::Matrix3d R = T.block<3, 3>(0, 0); - Eigen::Matrix4d T_inv = T; - T_inv.block<3, 3>(0, 0) = R.transpose(); - T_inv.block<3, 1>(0, 3) = -R.transpose() * T_inv.block<3, 1>(0, 3); - return T_inv; - } - Eigen::Matrix AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, bool &success) { @@ -451,7 +455,7 @@ namespace admittance_controller { } } else { success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); - memcpy(transform.data(), transform_buffer_vec_.data(), 16 * sizeof(double)); + vec_to_eigen(transform_buffer_vec_, transform); } return transform; @@ -468,24 +472,6 @@ namespace admittance_controller { joint_delta = Eigen::Map>(joint_buffer_vec_.data(), 6, 1); } - - void AdmittanceRule::convert_cartesian_deltas_to_joint_deltas_with_target(const std::vector &positions, - const Eigen::Matrix &joint_target, - double weight, - Eigen::Matrix &cartesian_delta, - const std::string &link_name, - Eigen::Matrix &joint_delta, - bool &success) { - success &= kinematics_->calculate_jacobian(positions, link_name, jacobian_buffer_vec_); - jacobian_ = Eigen::Map>(jacobian_buffer_vec_.data(), num_joints_, 6); - - Eigen::Matrix J_inverse = - (jacobian_.transpose() * jacobian_ + weight * identity).inverse(); - joint_delta = J_inverse * (jacobian_.transpose()*Eigen::Map>(cartesian_delta.data(), 6, 1) - + weight*identity*joint_target); - - } - Eigen::Matrix AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, const std::vector &joint_delta, @@ -494,13 +480,13 @@ namespace admittance_controller { Eigen::Matrix cartesian_delta; success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, link_name, cart_buffer_vec_); - memcpy(cartesian_delta.data(), cart_buffer_vec_.data(), 6 * sizeof(double)); + vec_to_eigen(cart_buffer_vec_, cartesian_delta); return cartesian_delta; } void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { - eigen_to_msg(wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench_filtered); + eigen_to_msg(wrench_world_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench_filtered); eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench); eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_->control_.frame_.id_, state_message.measured_wrench_control_frame); @@ -528,6 +514,16 @@ namespace admittance_controller { wrench_msg.header.stamp = clock_->now(); } + template + void AdmittanceRule::vec_to_eigen(const std::vector data, T2 &matrix) { + for (auto col = 0; col < matrix.cols(); col++) { + for (auto row = 0; row < matrix.rows(); row++) { + matrix(row, col) = data[row + col*matrix.rows()]; + } + } + + } + } // namespace admittance_controller #endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ From c54d46cf6f316dc04e786eeffffc1618a97d01f8 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 30 Jun 2022 16:57:21 -0600 Subject: [PATCH 027/111] fix pparameter copying behaviour --- .../include/admittance_controller/admittance_rule_impl.hpp | 3 ++- admittance_controller/src/admittance_controller.cpp | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index dc0bca0dce..75e1bf6fea 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -43,7 +43,7 @@ namespace admittance_controller { if (!parameters_->kinematics_.plugin_name_.empty()) { try { kinematics_loader_ = std::make_shared>( - "kdl_plugin", "kinematics_interface::KinematicsBaseClass"); + "kinematics_interface_kdl", "kinematics_interface::KinematicsBaseClass"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_->kinematics_.plugin_name_)); if (!kinematics_->initialize(node, parameters_->kinematics_.tip_)) { @@ -115,6 +115,7 @@ namespace admittance_controller { parameters_ = ¶meter_handler_->params_; } else { // make a copy of the data + parameters_copy = parameter_handler_->params_; parameters_ = ¶meters_copy; } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 8eef4106ff..9ac4365510 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -80,6 +80,7 @@ namespace admittance_controller { try { admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); + admittance_->parameters_ = &admittance_->parameter_handler_->params_; } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; From 906c60234cafd8db60e60a8c6686da293976068e Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 1 Jul 2022 13:50:49 -0600 Subject: [PATCH 028/111] add parameter field for IK package --- .../admittance_rule_impl.hpp | 2 +- .../config/admittance_controller.yaml | 1 + .../config/admittance_struct.h | 658 +++++++++--------- 3 files changed, 312 insertions(+), 349 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 75e1bf6fea..115b542d4d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -43,7 +43,7 @@ namespace admittance_controller { if (!parameters_->kinematics_.plugin_name_.empty()) { try { kinematics_loader_ = std::make_shared>( - "kinematics_interface_kdl", "kinematics_interface::KinematicsBaseClass"); + parameters_->kinematics_.plugin_package_, "kinematics_interface::KinematicsBaseClass"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_->kinematics_.plugin_name_)); if (!kinematics_->initialize(node, parameters_->kinematics_.tip_)) { diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml index 8b1d82565d..fddd499ce6 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml +++ b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml @@ -21,6 +21,7 @@ admittance_struct: kinematics: plugin_name: kdl_plugin/KDLKinematics + plugin_package: kinematics_interface_kdl base: base_link # Assumed to be stationary tip: ee_link group_name: ur5e_manipulator diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 944349368d..7fc9eab807 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -7,367 +7,329 @@ namespace admittance_struct_parameters { - struct admittance_struct { + struct admittance_struct { // if true, prevent parameters from updating bool lock_params_ = false; std::shared_ptr handle_; - admittance_struct(const std::shared_ptr ¶meters_interface) { - declare_params(parameters_interface); - auto update_param_cb = [this](const std::vector ¶meters) { - return this->update(parameters); - }; - handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); + admittance_struct(const std::shared_ptr& parameters_interface){ + declare_params(parameters_interface); + auto update_param_cb = [this](const std::vector ¶meters){return this->update(parameters);}; + handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); } struct params { - std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", - "wrist_2_joint", "wrist_3_joint"}; - std::vector command_interfaces_ = {"position"}; - std::vector state_interfaces_ = {"position", "velocity"}; - std::vector chainable_command_interfaces_ = {"position", "velocity"}; - struct kinematics { - std::string plugin_name_ = "kdl_plugin/KDLKinematics"; - std::string base_ = "base_link"; - std::string tip_ = "ee_link"; - std::string group_name_ = "ur5e_manipulator"; - double alpha_ = 5e-06; - } kinematics_; - struct ft_sensor { - std::string name_ = "tcp_fts_sensor"; - struct frame { - std::string id_ = "ee_link"; - bool external_ = false; - } frame_; - double filter_coefficient_ = 0.005; - } ft_sensor_; - struct control { - struct frame { - std::string id_ = "ee_link"; - bool external_ = false; - } frame_; - } control_; - struct fixed_world_frame { - std::string id_ = "base_link"; - bool external_ = false; - } fixed_world_frame_; - struct gravity_compensation { - struct frame { - std::string id_ = "ee_link"; - bool external_ = false; - } frame_; - struct CoG { - std::vector pos_ = {0.1, 0.0, 0.0}; - double force_ = 23.0; - } CoG_; - } gravity_compensation_; - struct admittance { - std::vector selected_axes_ = {true, true, true, true, true, true}; - std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; - std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; - std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; - } admittance_; - bool enable_parameter_update_without_reactivation_ = true; - bool use_feedforward_commanded_input_ = true; - std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; - double state_publish_rate_ = 200.0; + std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"}; +std::vector command_interfaces_ = {"position"}; +std::vector state_interfaces_ = {"position", "velocity"}; +std::vector chainable_command_interfaces_ = {"position", "velocity"}; +struct kinematics { +std::string plugin_name_ = "kdl_plugin/KDLKinematics"; +std::string plugin_package_ = "kinematics_interface_kdl"; +std::string base_ = "base_link"; +std::string tip_ = "ee_link"; +std::string group_name_ = "ur5e_manipulator"; +double alpha_ = 0.0005; +} kinematics_; +struct ft_sensor { +std::string name_ = "tcp_fts_sensor"; +struct frame { +std::string id_ = "ee_link"; +bool external_ = false; +} frame_; +double filter_coefficient_ = 0.005; +} ft_sensor_; +struct control { +struct frame { +std::string id_ = "ee_link"; +bool external_ = false; +} frame_; +} control_; +struct fixed_world_frame { +std::string id_ = "base_link"; +bool external_ = false; +} fixed_world_frame_; +struct gravity_compensation { +struct frame { +std::string id_ = "ee_link"; +bool external_ = false; +} frame_; +struct CoG { +std::vector pos_ = {0.1, 0.0, 0.0}; +double force_ = 23.0; +} CoG_; +} gravity_compensation_; +struct admittance { +std::vector selected_axes_ = {true, true, true, true, true, true}; +std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; +std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; +std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; +} admittance_; +bool enable_parameter_update_without_reactivation_ = true; +bool use_feedforward_commanded_input_ = true; +std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; +double state_publish_rate_ = 200.0; } params_; - rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = !lock_params_; - if (lock_params_) { - result.reason = "The parameters can not be updated because they are currently locked."; - return result; - } + rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = !lock_params_; + if (lock_params_){ + result.reason = "The parameters can not be updated because they are currently locked."; + return result; + } - result.reason = "success"; - for (const auto ¶m: parameters) { - if (param.get_name() == "joints") { - params_.joints_ = param.as_string_array(); - } - if (param.get_name() == "command_interfaces") { - params_.command_interfaces_ = param.as_string_array(); - } - if (param.get_name() == "state_interfaces") { - params_.state_interfaces_ = param.as_string_array(); - } - if (param.get_name() == "chainable_command_interfaces") { - params_.chainable_command_interfaces_ = param.as_string_array(); - } - if (param.get_name() == "kinematics.plugin_name") { - params_.kinematics_.plugin_name_ = param.as_string(); - } - if (param.get_name() == "kinematics.base") { - params_.kinematics_.base_ = param.as_string(); - } - if (param.get_name() == "kinematics.tip") { - params_.kinematics_.tip_ = param.as_string(); - } - if (param.get_name() == "kinematics.group_name") { - params_.kinematics_.group_name_ = param.as_string(); - } - if (param.get_name() == "kinematics.alpha") { - params_.kinematics_.alpha_ = param.as_double(); - } - if (param.get_name() == "ft_sensor.name") { - params_.ft_sensor_.name_ = param.as_string(); - } - if (param.get_name() == "ft_sensor.frame.id") { - params_.ft_sensor_.frame_.id_ = param.as_string(); - } - if (param.get_name() == "ft_sensor.frame.external") { - params_.ft_sensor_.frame_.external_ = param.as_bool(); - } - if (param.get_name() == "ft_sensor.filter_coefficient") { - params_.ft_sensor_.filter_coefficient_ = param.as_double(); - } - if (param.get_name() == "control.frame.id") { - params_.control_.frame_.id_ = param.as_string(); - } - if (param.get_name() == "control.frame.external") { - params_.control_.frame_.external_ = param.as_bool(); - } - if (param.get_name() == "fixed_world_frame.id") { - params_.fixed_world_frame_.id_ = param.as_string(); - } - if (param.get_name() == "fixed_world_frame.external") { - params_.fixed_world_frame_.external_ = param.as_bool(); - } - if (param.get_name() == "gravity_compensation.frame.id") { - params_.gravity_compensation_.frame_.id_ = param.as_string(); - } - if (param.get_name() == "gravity_compensation.frame.external") { - params_.gravity_compensation_.frame_.external_ = param.as_bool(); - } - if (param.get_name() == "gravity_compensation.CoG.pos") { - params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); - } - if (param.get_name() == "gravity_compensation.CoG.force") { - params_.gravity_compensation_.CoG_.force_ = param.as_double(); - } - if (param.get_name() == "admittance.selected_axes") { - params_.admittance_.selected_axes_ = param.as_bool_array(); - } - if (param.get_name() == "admittance.mass") { - params_.admittance_.mass_ = param.as_double_array(); - } - if (param.get_name() == "admittance.damping_ratio") { - params_.admittance_.damping_ratio_ = param.as_double_array(); - } - if (param.get_name() == "admittance.stiffness") { - params_.admittance_.stiffness_ = param.as_double_array(); - } - if (param.get_name() == "enable_parameter_update_without_reactivation") { - params_.enable_parameter_update_without_reactivation_ = param.as_bool(); - } - if (param.get_name() == "use_feedforward_commanded_input") { - params_.use_feedforward_commanded_input_ = param.as_bool(); - } - if (param.get_name() == "joint_limiter_type") { - params_.joint_limiter_type_ = param.as_string(); - } - if (param.get_name() == "state_publish_rate") { - params_.state_publish_rate_ = param.as_double(); - } + result.reason = "success"; + for (const auto ¶m: parameters) { + if (param.get_name() == "joints") { +params_.joints_ = param.as_string_array(); +} +if (param.get_name() == "command_interfaces") { +params_.command_interfaces_ = param.as_string_array(); +} +if (param.get_name() == "state_interfaces") { +params_.state_interfaces_ = param.as_string_array(); +} +if (param.get_name() == "chainable_command_interfaces") { +params_.chainable_command_interfaces_ = param.as_string_array(); +} +if (param.get_name() == "kinematics.plugin_name") { +params_.kinematics_.plugin_name_ = param.as_string(); +} +if (param.get_name() == "kinematics.plugin_package") { +params_.kinematics_.plugin_package_ = param.as_string(); +} +if (param.get_name() == "kinematics.base") { +params_.kinematics_.base_ = param.as_string(); +} +if (param.get_name() == "kinematics.tip") { +params_.kinematics_.tip_ = param.as_string(); +} +if (param.get_name() == "kinematics.group_name") { +params_.kinematics_.group_name_ = param.as_string(); +} +if (param.get_name() == "kinematics.alpha") { +params_.kinematics_.alpha_ = param.as_double(); +} +if (param.get_name() == "ft_sensor.name") { +params_.ft_sensor_.name_ = param.as_string(); +} +if (param.get_name() == "ft_sensor.frame.id") { +params_.ft_sensor_.frame_.id_ = param.as_string(); +} +if (param.get_name() == "ft_sensor.frame.external") { +params_.ft_sensor_.frame_.external_ = param.as_bool(); +} +if (param.get_name() == "ft_sensor.filter_coefficient") { +params_.ft_sensor_.filter_coefficient_ = param.as_double(); +} +if (param.get_name() == "control.frame.id") { +params_.control_.frame_.id_ = param.as_string(); +} +if (param.get_name() == "control.frame.external") { +params_.control_.frame_.external_ = param.as_bool(); +} +if (param.get_name() == "fixed_world_frame.id") { +params_.fixed_world_frame_.id_ = param.as_string(); +} +if (param.get_name() == "fixed_world_frame.external") { +params_.fixed_world_frame_.external_ = param.as_bool(); +} +if (param.get_name() == "gravity_compensation.frame.id") { +params_.gravity_compensation_.frame_.id_ = param.as_string(); +} +if (param.get_name() == "gravity_compensation.frame.external") { +params_.gravity_compensation_.frame_.external_ = param.as_bool(); +} +if (param.get_name() == "gravity_compensation.CoG.pos") { +params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); +} +if (param.get_name() == "gravity_compensation.CoG.force") { +params_.gravity_compensation_.CoG_.force_ = param.as_double(); +} +if (param.get_name() == "admittance.selected_axes") { +params_.admittance_.selected_axes_ = param.as_bool_array(); +} +if (param.get_name() == "admittance.mass") { +params_.admittance_.mass_ = param.as_double_array(); +} +if (param.get_name() == "admittance.damping_ratio") { +params_.admittance_.damping_ratio_ = param.as_double_array(); +} +if (param.get_name() == "admittance.stiffness") { +params_.admittance_.stiffness_ = param.as_double_array(); +} +if (param.get_name() == "enable_parameter_update_without_reactivation") { +params_.enable_parameter_update_without_reactivation_ = param.as_bool(); +} +if (param.get_name() == "use_feedforward_commanded_input") { +params_.use_feedforward_commanded_input_ = param.as_bool(); +} +if (param.get_name() == "joint_limiter_type") { +params_.joint_limiter_type_ = param.as_string(); +} +if (param.get_name() == "state_publish_rate") { +params_.state_publish_rate_ = param.as_double(); +} - } - return result; - } + } + return result; + } - void declare_params(const std::shared_ptr ¶meters_interface) { - if (!parameters_interface->has_parameter("joints")) { - auto p_joints = rclcpp::ParameterValue(params_.joints_); - parameters_interface->declare_parameter("joints", p_joints); - } else { - params_.joints_ = parameters_interface->get_parameter("joints").as_string_array(); - } - if (!parameters_interface->has_parameter("command_interfaces")) { - auto p_command_interfaces = rclcpp::ParameterValue(params_.command_interfaces_); - parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); - } else { - params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); - } - if (!parameters_interface->has_parameter("state_interfaces")) { - auto p_state_interfaces = rclcpp::ParameterValue(params_.state_interfaces_); - parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); - } else { - params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); - } - if (!parameters_interface->has_parameter("chainable_command_interfaces")) { - auto p_chainable_command_interfaces = rclcpp::ParameterValue(params_.chainable_command_interfaces_); - parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); - } else { - params_.chainable_command_interfaces_ = parameters_interface->get_parameter( - "chainable_command_interfaces").as_string_array(); - } - if (!parameters_interface->has_parameter("kinematics.plugin_name")) { - auto p_kinematics_plugin_name = rclcpp::ParameterValue(params_.kinematics_.plugin_name_); - parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); - } else { - params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.base")) { - auto p_kinematics_base = rclcpp::ParameterValue(params_.kinematics_.base_); - parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); - } else { - params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.tip")) { - auto p_kinematics_tip = rclcpp::ParameterValue(params_.kinematics_.tip_); - parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); - } else { - params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.group_name")) { - auto p_kinematics_group_name = rclcpp::ParameterValue(params_.kinematics_.group_name_); - parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); - } else { - params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.alpha")) { - auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); - parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); - } else { - params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); - } - if (!parameters_interface->has_parameter("ft_sensor.name")) { - auto p_ft_sensor_name = rclcpp::ParameterValue(params_.ft_sensor_.name_); - parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); - } else { - params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); - } - if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { - auto p_ft_sensor_frame_id = rclcpp::ParameterValue(params_.ft_sensor_.frame_.id_); - parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); - } else { - params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); - } - if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { - auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); - parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); - } else { - params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { - auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); - parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); - } else { - params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( - "ft_sensor.filter_coefficient").as_double(); - } - if (!parameters_interface->has_parameter("control.frame.id")) { - auto p_control_frame_id = rclcpp::ParameterValue(params_.control_.frame_.id_); - parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); - } else { - params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); - } - if (!parameters_interface->has_parameter("control.frame.external")) { - auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); - parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); - } else { - params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("fixed_world_frame.id")) { - auto p_fixed_world_frame_id = rclcpp::ParameterValue(params_.fixed_world_frame_.id_); - parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); - } else { - params_.fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string(); - } - if (!parameters_interface->has_parameter("fixed_world_frame.external")) { - auto p_fixed_world_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.external_); - parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); - } else { - params_.fixed_world_frame_.external_ = parameters_interface->get_parameter( - "fixed_world_frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { - auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.id_); - parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); - } else { - params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( - "gravity_compensation.frame.id").as_string(); - } - if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { - auto p_gravity_compensation_frame_external = rclcpp::ParameterValue( - params_.gravity_compensation_.frame_.external_); - parameters_interface->declare_parameter("gravity_compensation.frame.external", - p_gravity_compensation_frame_external); - } else { - params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( - "gravity_compensation.frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { - auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.pos_); - parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); - } else { - params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( - "gravity_compensation.CoG.pos").as_double_array(); - } - if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { - auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); - parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); - } else { - params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( - "gravity_compensation.CoG.force").as_double(); - } - if (!parameters_interface->has_parameter("admittance.selected_axes")) { - auto p_admittance_selected_axes = rclcpp::ParameterValue(params_.admittance_.selected_axes_); - parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); - } else { - params_.admittance_.selected_axes_ = parameters_interface->get_parameter( - "admittance.selected_axes").as_bool_array(); - } - if (!parameters_interface->has_parameter("admittance.mass")) { - auto p_admittance_mass = rclcpp::ParameterValue(params_.admittance_.mass_); - parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); - } else { - params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); - } - if (!parameters_interface->has_parameter("admittance.damping_ratio")) { - auto p_admittance_damping_ratio = rclcpp::ParameterValue(params_.admittance_.damping_ratio_); - parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); - } else { - params_.admittance_.damping_ratio_ = parameters_interface->get_parameter( - "admittance.damping_ratio").as_double_array(); - } - if (!parameters_interface->has_parameter("admittance.stiffness")) { - auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); - parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); - } else { - params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); - } - if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { - auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( - params_.enable_parameter_update_without_reactivation_); - parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", - p_enable_parameter_update_without_reactivation); - } else { - params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( - "enable_parameter_update_without_reactivation").as_bool(); - } - if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { - auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); - parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); - } else { - params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter( - "use_feedforward_commanded_input").as_bool(); - } - if (!parameters_interface->has_parameter("joint_limiter_type")) { - auto p_joint_limiter_type = rclcpp::ParameterValue(params_.joint_limiter_type_); - parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); - } else { - params_.joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string(); - } - if (!parameters_interface->has_parameter("state_publish_rate")) { - auto p_state_publish_rate = rclcpp::ParameterValue(params_.state_publish_rate_); - parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); - } else { - params_.state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double(); - } + void declare_params(const std::shared_ptr& parameters_interface){ + if (!parameters_interface->has_parameter("joints")){ +auto p_joints = rclcpp::ParameterValue(params_.joints_); +parameters_interface->declare_parameter("joints", p_joints); +} else { +params_.joints_ = parameters_interface->get_parameter("joints").as_string_array();} +if (!parameters_interface->has_parameter("command_interfaces")){ +auto p_command_interfaces = rclcpp::ParameterValue(params_.command_interfaces_); +parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); +} else { +params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array();} +if (!parameters_interface->has_parameter("state_interfaces")){ +auto p_state_interfaces = rclcpp::ParameterValue(params_.state_interfaces_); +parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); +} else { +params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array();} +if (!parameters_interface->has_parameter("chainable_command_interfaces")){ +auto p_chainable_command_interfaces = rclcpp::ParameterValue(params_.chainable_command_interfaces_); +parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); +} else { +params_.chainable_command_interfaces_ = parameters_interface->get_parameter("chainable_command_interfaces").as_string_array();} +if (!parameters_interface->has_parameter("kinematics.plugin_name")){ +auto p_kinematics_plugin_name = rclcpp::ParameterValue(params_.kinematics_.plugin_name_); +parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); +} else { +params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string();} +if (!parameters_interface->has_parameter("kinematics.plugin_package")){ +auto p_kinematics_plugin_package = rclcpp::ParameterValue(params_.kinematics_.plugin_package_); +parameters_interface->declare_parameter("kinematics.plugin_package", p_kinematics_plugin_package); +} else { +params_.kinematics_.plugin_package_ = parameters_interface->get_parameter("kinematics.plugin_package").as_string();} +if (!parameters_interface->has_parameter("kinematics.base")){ +auto p_kinematics_base = rclcpp::ParameterValue(params_.kinematics_.base_); +parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); +} else { +params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string();} +if (!parameters_interface->has_parameter("kinematics.tip")){ +auto p_kinematics_tip = rclcpp::ParameterValue(params_.kinematics_.tip_); +parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); +} else { +params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string();} +if (!parameters_interface->has_parameter("kinematics.group_name")){ +auto p_kinematics_group_name = rclcpp::ParameterValue(params_.kinematics_.group_name_); +parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); +} else { +params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string();} +if (!parameters_interface->has_parameter("kinematics.alpha")){ +auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); +parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); +} else { +params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double();} +if (!parameters_interface->has_parameter("ft_sensor.name")){ +auto p_ft_sensor_name = rclcpp::ParameterValue(params_.ft_sensor_.name_); +parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); +} else { +params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string();} +if (!parameters_interface->has_parameter("ft_sensor.frame.id")){ +auto p_ft_sensor_frame_id = rclcpp::ParameterValue(params_.ft_sensor_.frame_.id_); +parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); +} else { +params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string();} +if (!parameters_interface->has_parameter("ft_sensor.frame.external")){ +auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); +parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); +} else { +params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool();} +if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")){ +auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); +parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); +} else { +params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter("ft_sensor.filter_coefficient").as_double();} +if (!parameters_interface->has_parameter("control.frame.id")){ +auto p_control_frame_id = rclcpp::ParameterValue(params_.control_.frame_.id_); +parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); +} else { +params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string();} +if (!parameters_interface->has_parameter("control.frame.external")){ +auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); +parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); +} else { +params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool();} +if (!parameters_interface->has_parameter("fixed_world_frame.id")){ +auto p_fixed_world_frame_id = rclcpp::ParameterValue(params_.fixed_world_frame_.id_); +parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); +} else { +params_.fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string();} +if (!parameters_interface->has_parameter("fixed_world_frame.external")){ +auto p_fixed_world_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.external_); +parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); +} else { +params_.fixed_world_frame_.external_ = parameters_interface->get_parameter("fixed_world_frame.external").as_bool();} +if (!parameters_interface->has_parameter("gravity_compensation.frame.id")){ +auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.id_); +parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); +} else { +params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter("gravity_compensation.frame.id").as_string();} +if (!parameters_interface->has_parameter("gravity_compensation.frame.external")){ +auto p_gravity_compensation_frame_external = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.external_); +parameters_interface->declare_parameter("gravity_compensation.frame.external", p_gravity_compensation_frame_external); +} else { +params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter("gravity_compensation.frame.external").as_bool();} +if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")){ +auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.pos_); +parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); +} else { +params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter("gravity_compensation.CoG.pos").as_double_array();} +if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")){ +auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); +parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); +} else { +params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter("gravity_compensation.CoG.force").as_double();} +if (!parameters_interface->has_parameter("admittance.selected_axes")){ +auto p_admittance_selected_axes = rclcpp::ParameterValue(params_.admittance_.selected_axes_); +parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); +} else { +params_.admittance_.selected_axes_ = parameters_interface->get_parameter("admittance.selected_axes").as_bool_array();} +if (!parameters_interface->has_parameter("admittance.mass")){ +auto p_admittance_mass = rclcpp::ParameterValue(params_.admittance_.mass_); +parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); +} else { +params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array();} +if (!parameters_interface->has_parameter("admittance.damping_ratio")){ +auto p_admittance_damping_ratio = rclcpp::ParameterValue(params_.admittance_.damping_ratio_); +parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); +} else { +params_.admittance_.damping_ratio_ = parameters_interface->get_parameter("admittance.damping_ratio").as_double_array();} +if (!parameters_interface->has_parameter("admittance.stiffness")){ +auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); +parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); +} else { +params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array();} +if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")){ +auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue(params_.enable_parameter_update_without_reactivation_); +parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", p_enable_parameter_update_without_reactivation); +} else { +params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter("enable_parameter_update_without_reactivation").as_bool();} +if (!parameters_interface->has_parameter("use_feedforward_commanded_input")){ +auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); +parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); +} else { +params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter("use_feedforward_commanded_input").as_bool();} +if (!parameters_interface->has_parameter("joint_limiter_type")){ +auto p_joint_limiter_type = rclcpp::ParameterValue(params_.joint_limiter_type_); +parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); +} else { +params_.joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string();} +if (!parameters_interface->has_parameter("state_publish_rate")){ +auto p_state_publish_rate = rclcpp::ParameterValue(params_.state_publish_rate_); +parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); +} else { +params_.state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double();} - } - }; + } + }; } // namespace admittance_struct_parameters From 4a6f77900c330408112a1d9175a07b24dcd1a3f4 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 1 Jul 2022 13:52:58 -0600 Subject: [PATCH 029/111] fix struct formatting --- .../config/admittance_struct.h | 669 ++++++++++-------- 1 file changed, 359 insertions(+), 310 deletions(-) diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 7fc9eab807..2b102ea0d0 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -7,329 +7,378 @@ namespace admittance_struct_parameters { - struct admittance_struct { + struct admittance_struct { // if true, prevent parameters from updating bool lock_params_ = false; std::shared_ptr handle_; - admittance_struct(const std::shared_ptr& parameters_interface){ - declare_params(parameters_interface); - auto update_param_cb = [this](const std::vector ¶meters){return this->update(parameters);}; - handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); + admittance_struct(const std::shared_ptr ¶meters_interface) { + declare_params(parameters_interface); + auto update_param_cb = [this](const std::vector ¶meters) { + return this->update(parameters); + }; + handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); } struct params { - std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"}; -std::vector command_interfaces_ = {"position"}; -std::vector state_interfaces_ = {"position", "velocity"}; -std::vector chainable_command_interfaces_ = {"position", "velocity"}; -struct kinematics { -std::string plugin_name_ = "kdl_plugin/KDLKinematics"; -std::string plugin_package_ = "kinematics_interface_kdl"; -std::string base_ = "base_link"; -std::string tip_ = "ee_link"; -std::string group_name_ = "ur5e_manipulator"; -double alpha_ = 0.0005; -} kinematics_; -struct ft_sensor { -std::string name_ = "tcp_fts_sensor"; -struct frame { -std::string id_ = "ee_link"; -bool external_ = false; -} frame_; -double filter_coefficient_ = 0.005; -} ft_sensor_; -struct control { -struct frame { -std::string id_ = "ee_link"; -bool external_ = false; -} frame_; -} control_; -struct fixed_world_frame { -std::string id_ = "base_link"; -bool external_ = false; -} fixed_world_frame_; -struct gravity_compensation { -struct frame { -std::string id_ = "ee_link"; -bool external_ = false; -} frame_; -struct CoG { -std::vector pos_ = {0.1, 0.0, 0.0}; -double force_ = 23.0; -} CoG_; -} gravity_compensation_; -struct admittance { -std::vector selected_axes_ = {true, true, true, true, true, true}; -std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; -std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; -std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; -} admittance_; -bool enable_parameter_update_without_reactivation_ = true; -bool use_feedforward_commanded_input_ = true; -std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; -double state_publish_rate_ = 200.0; + std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", + "wrist_2_joint", "wrist_3_joint"}; + std::vector command_interfaces_ = {"position"}; + std::vector state_interfaces_ = {"position", "velocity"}; + std::vector chainable_command_interfaces_ = {"position", "velocity"}; + struct kinematics { + std::string plugin_name_ = "kdl_plugin/KDLKinematics"; + std::string plugin_package_ = "kinematics_interface_kdl"; + std::string base_ = "base_link"; + std::string tip_ = "ee_link"; + std::string group_name_ = "ur5e_manipulator"; + double alpha_ = 0.0005; + } kinematics_; + struct ft_sensor { + std::string name_ = "tcp_fts_sensor"; + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + double filter_coefficient_ = 0.005; + } ft_sensor_; + struct control { + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + } control_; + struct fixed_world_frame { + std::string id_ = "base_link"; + bool external_ = false; + } fixed_world_frame_; + struct gravity_compensation { + struct frame { + std::string id_ = "ee_link"; + bool external_ = false; + } frame_; + struct CoG { + std::vector pos_ = {0.1, 0.0, 0.0}; + double force_ = 23.0; + } CoG_; + } gravity_compensation_; + struct admittance { + std::vector selected_axes_ = {true, true, true, true, true, true}; + std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; + std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; + std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; + } admittance_; + bool enable_parameter_update_without_reactivation_ = true; + bool use_feedforward_commanded_input_ = true; + std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; + double state_publish_rate_ = 200.0; } params_; - rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = !lock_params_; - if (lock_params_){ - result.reason = "The parameters can not be updated because they are currently locked."; - return result; - } + rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = !lock_params_; + if (lock_params_) { + result.reason = "The parameters can not be updated because they are currently locked."; + return result; + } - result.reason = "success"; - for (const auto ¶m: parameters) { - if (param.get_name() == "joints") { -params_.joints_ = param.as_string_array(); -} -if (param.get_name() == "command_interfaces") { -params_.command_interfaces_ = param.as_string_array(); -} -if (param.get_name() == "state_interfaces") { -params_.state_interfaces_ = param.as_string_array(); -} -if (param.get_name() == "chainable_command_interfaces") { -params_.chainable_command_interfaces_ = param.as_string_array(); -} -if (param.get_name() == "kinematics.plugin_name") { -params_.kinematics_.plugin_name_ = param.as_string(); -} -if (param.get_name() == "kinematics.plugin_package") { -params_.kinematics_.plugin_package_ = param.as_string(); -} -if (param.get_name() == "kinematics.base") { -params_.kinematics_.base_ = param.as_string(); -} -if (param.get_name() == "kinematics.tip") { -params_.kinematics_.tip_ = param.as_string(); -} -if (param.get_name() == "kinematics.group_name") { -params_.kinematics_.group_name_ = param.as_string(); -} -if (param.get_name() == "kinematics.alpha") { -params_.kinematics_.alpha_ = param.as_double(); -} -if (param.get_name() == "ft_sensor.name") { -params_.ft_sensor_.name_ = param.as_string(); -} -if (param.get_name() == "ft_sensor.frame.id") { -params_.ft_sensor_.frame_.id_ = param.as_string(); -} -if (param.get_name() == "ft_sensor.frame.external") { -params_.ft_sensor_.frame_.external_ = param.as_bool(); -} -if (param.get_name() == "ft_sensor.filter_coefficient") { -params_.ft_sensor_.filter_coefficient_ = param.as_double(); -} -if (param.get_name() == "control.frame.id") { -params_.control_.frame_.id_ = param.as_string(); -} -if (param.get_name() == "control.frame.external") { -params_.control_.frame_.external_ = param.as_bool(); -} -if (param.get_name() == "fixed_world_frame.id") { -params_.fixed_world_frame_.id_ = param.as_string(); -} -if (param.get_name() == "fixed_world_frame.external") { -params_.fixed_world_frame_.external_ = param.as_bool(); -} -if (param.get_name() == "gravity_compensation.frame.id") { -params_.gravity_compensation_.frame_.id_ = param.as_string(); -} -if (param.get_name() == "gravity_compensation.frame.external") { -params_.gravity_compensation_.frame_.external_ = param.as_bool(); -} -if (param.get_name() == "gravity_compensation.CoG.pos") { -params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); -} -if (param.get_name() == "gravity_compensation.CoG.force") { -params_.gravity_compensation_.CoG_.force_ = param.as_double(); -} -if (param.get_name() == "admittance.selected_axes") { -params_.admittance_.selected_axes_ = param.as_bool_array(); -} -if (param.get_name() == "admittance.mass") { -params_.admittance_.mass_ = param.as_double_array(); -} -if (param.get_name() == "admittance.damping_ratio") { -params_.admittance_.damping_ratio_ = param.as_double_array(); -} -if (param.get_name() == "admittance.stiffness") { -params_.admittance_.stiffness_ = param.as_double_array(); -} -if (param.get_name() == "enable_parameter_update_without_reactivation") { -params_.enable_parameter_update_without_reactivation_ = param.as_bool(); -} -if (param.get_name() == "use_feedforward_commanded_input") { -params_.use_feedforward_commanded_input_ = param.as_bool(); -} -if (param.get_name() == "joint_limiter_type") { -params_.joint_limiter_type_ = param.as_string(); -} -if (param.get_name() == "state_publish_rate") { -params_.state_publish_rate_ = param.as_double(); -} - - } - return result; + result.reason = "success"; + for (const auto ¶m: parameters) { + if (param.get_name() == "joints") { + params_.joints_ = param.as_string_array(); + } + if (param.get_name() == "command_interfaces") { + params_.command_interfaces_ = param.as_string_array(); + } + if (param.get_name() == "state_interfaces") { + params_.state_interfaces_ = param.as_string_array(); + } + if (param.get_name() == "chainable_command_interfaces") { + params_.chainable_command_interfaces_ = param.as_string_array(); + } + if (param.get_name() == "kinematics.plugin_name") { + params_.kinematics_.plugin_name_ = param.as_string(); + } + if (param.get_name() == "kinematics.plugin_package") { + params_.kinematics_.plugin_package_ = param.as_string(); + } + if (param.get_name() == "kinematics.base") { + params_.kinematics_.base_ = param.as_string(); + } + if (param.get_name() == "kinematics.tip") { + params_.kinematics_.tip_ = param.as_string(); + } + if (param.get_name() == "kinematics.group_name") { + params_.kinematics_.group_name_ = param.as_string(); + } + if (param.get_name() == "kinematics.alpha") { + params_.kinematics_.alpha_ = param.as_double(); + } + if (param.get_name() == "ft_sensor.name") { + params_.ft_sensor_.name_ = param.as_string(); + } + if (param.get_name() == "ft_sensor.frame.id") { + params_.ft_sensor_.frame_.id_ = param.as_string(); + } + if (param.get_name() == "ft_sensor.frame.external") { + params_.ft_sensor_.frame_.external_ = param.as_bool(); + } + if (param.get_name() == "ft_sensor.filter_coefficient") { + params_.ft_sensor_.filter_coefficient_ = param.as_double(); + } + if (param.get_name() == "control.frame.id") { + params_.control_.frame_.id_ = param.as_string(); + } + if (param.get_name() == "control.frame.external") { + params_.control_.frame_.external_ = param.as_bool(); + } + if (param.get_name() == "fixed_world_frame.id") { + params_.fixed_world_frame_.id_ = param.as_string(); + } + if (param.get_name() == "fixed_world_frame.external") { + params_.fixed_world_frame_.external_ = param.as_bool(); + } + if (param.get_name() == "gravity_compensation.frame.id") { + params_.gravity_compensation_.frame_.id_ = param.as_string(); + } + if (param.get_name() == "gravity_compensation.frame.external") { + params_.gravity_compensation_.frame_.external_ = param.as_bool(); + } + if (param.get_name() == "gravity_compensation.CoG.pos") { + params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); + } + if (param.get_name() == "gravity_compensation.CoG.force") { + params_.gravity_compensation_.CoG_.force_ = param.as_double(); + } + if (param.get_name() == "admittance.selected_axes") { + params_.admittance_.selected_axes_ = param.as_bool_array(); + } + if (param.get_name() == "admittance.mass") { + params_.admittance_.mass_ = param.as_double_array(); + } + if (param.get_name() == "admittance.damping_ratio") { + params_.admittance_.damping_ratio_ = param.as_double_array(); + } + if (param.get_name() == "admittance.stiffness") { + params_.admittance_.stiffness_ = param.as_double_array(); + } + if (param.get_name() == "enable_parameter_update_without_reactivation") { + params_.enable_parameter_update_without_reactivation_ = param.as_bool(); + } + if (param.get_name() == "use_feedforward_commanded_input") { + params_.use_feedforward_commanded_input_ = param.as_bool(); + } + if (param.get_name() == "joint_limiter_type") { + params_.joint_limiter_type_ = param.as_string(); + } + if (param.get_name() == "state_publish_rate") { + params_.state_publish_rate_ = param.as_double(); } - void declare_params(const std::shared_ptr& parameters_interface){ - if (!parameters_interface->has_parameter("joints")){ -auto p_joints = rclcpp::ParameterValue(params_.joints_); -parameters_interface->declare_parameter("joints", p_joints); -} else { -params_.joints_ = parameters_interface->get_parameter("joints").as_string_array();} -if (!parameters_interface->has_parameter("command_interfaces")){ -auto p_command_interfaces = rclcpp::ParameterValue(params_.command_interfaces_); -parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); -} else { -params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array();} -if (!parameters_interface->has_parameter("state_interfaces")){ -auto p_state_interfaces = rclcpp::ParameterValue(params_.state_interfaces_); -parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); -} else { -params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array();} -if (!parameters_interface->has_parameter("chainable_command_interfaces")){ -auto p_chainable_command_interfaces = rclcpp::ParameterValue(params_.chainable_command_interfaces_); -parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); -} else { -params_.chainable_command_interfaces_ = parameters_interface->get_parameter("chainable_command_interfaces").as_string_array();} -if (!parameters_interface->has_parameter("kinematics.plugin_name")){ -auto p_kinematics_plugin_name = rclcpp::ParameterValue(params_.kinematics_.plugin_name_); -parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); -} else { -params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string();} -if (!parameters_interface->has_parameter("kinematics.plugin_package")){ -auto p_kinematics_plugin_package = rclcpp::ParameterValue(params_.kinematics_.plugin_package_); -parameters_interface->declare_parameter("kinematics.plugin_package", p_kinematics_plugin_package); -} else { -params_.kinematics_.plugin_package_ = parameters_interface->get_parameter("kinematics.plugin_package").as_string();} -if (!parameters_interface->has_parameter("kinematics.base")){ -auto p_kinematics_base = rclcpp::ParameterValue(params_.kinematics_.base_); -parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); -} else { -params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string();} -if (!parameters_interface->has_parameter("kinematics.tip")){ -auto p_kinematics_tip = rclcpp::ParameterValue(params_.kinematics_.tip_); -parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); -} else { -params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string();} -if (!parameters_interface->has_parameter("kinematics.group_name")){ -auto p_kinematics_group_name = rclcpp::ParameterValue(params_.kinematics_.group_name_); -parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); -} else { -params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string();} -if (!parameters_interface->has_parameter("kinematics.alpha")){ -auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); -parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); -} else { -params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double();} -if (!parameters_interface->has_parameter("ft_sensor.name")){ -auto p_ft_sensor_name = rclcpp::ParameterValue(params_.ft_sensor_.name_); -parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); -} else { -params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string();} -if (!parameters_interface->has_parameter("ft_sensor.frame.id")){ -auto p_ft_sensor_frame_id = rclcpp::ParameterValue(params_.ft_sensor_.frame_.id_); -parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); -} else { -params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string();} -if (!parameters_interface->has_parameter("ft_sensor.frame.external")){ -auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); -parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); -} else { -params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool();} -if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")){ -auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); -parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); -} else { -params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter("ft_sensor.filter_coefficient").as_double();} -if (!parameters_interface->has_parameter("control.frame.id")){ -auto p_control_frame_id = rclcpp::ParameterValue(params_.control_.frame_.id_); -parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); -} else { -params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string();} -if (!parameters_interface->has_parameter("control.frame.external")){ -auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); -parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); -} else { -params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool();} -if (!parameters_interface->has_parameter("fixed_world_frame.id")){ -auto p_fixed_world_frame_id = rclcpp::ParameterValue(params_.fixed_world_frame_.id_); -parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); -} else { -params_.fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string();} -if (!parameters_interface->has_parameter("fixed_world_frame.external")){ -auto p_fixed_world_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.external_); -parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); -} else { -params_.fixed_world_frame_.external_ = parameters_interface->get_parameter("fixed_world_frame.external").as_bool();} -if (!parameters_interface->has_parameter("gravity_compensation.frame.id")){ -auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.id_); -parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); -} else { -params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter("gravity_compensation.frame.id").as_string();} -if (!parameters_interface->has_parameter("gravity_compensation.frame.external")){ -auto p_gravity_compensation_frame_external = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.external_); -parameters_interface->declare_parameter("gravity_compensation.frame.external", p_gravity_compensation_frame_external); -} else { -params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter("gravity_compensation.frame.external").as_bool();} -if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")){ -auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.pos_); -parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); -} else { -params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter("gravity_compensation.CoG.pos").as_double_array();} -if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")){ -auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); -parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); -} else { -params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter("gravity_compensation.CoG.force").as_double();} -if (!parameters_interface->has_parameter("admittance.selected_axes")){ -auto p_admittance_selected_axes = rclcpp::ParameterValue(params_.admittance_.selected_axes_); -parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); -} else { -params_.admittance_.selected_axes_ = parameters_interface->get_parameter("admittance.selected_axes").as_bool_array();} -if (!parameters_interface->has_parameter("admittance.mass")){ -auto p_admittance_mass = rclcpp::ParameterValue(params_.admittance_.mass_); -parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); -} else { -params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array();} -if (!parameters_interface->has_parameter("admittance.damping_ratio")){ -auto p_admittance_damping_ratio = rclcpp::ParameterValue(params_.admittance_.damping_ratio_); -parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); -} else { -params_.admittance_.damping_ratio_ = parameters_interface->get_parameter("admittance.damping_ratio").as_double_array();} -if (!parameters_interface->has_parameter("admittance.stiffness")){ -auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); -parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); -} else { -params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array();} -if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")){ -auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue(params_.enable_parameter_update_without_reactivation_); -parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", p_enable_parameter_update_without_reactivation); -} else { -params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter("enable_parameter_update_without_reactivation").as_bool();} -if (!parameters_interface->has_parameter("use_feedforward_commanded_input")){ -auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); -parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); -} else { -params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter("use_feedforward_commanded_input").as_bool();} -if (!parameters_interface->has_parameter("joint_limiter_type")){ -auto p_joint_limiter_type = rclcpp::ParameterValue(params_.joint_limiter_type_); -parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); -} else { -params_.joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string();} -if (!parameters_interface->has_parameter("state_publish_rate")){ -auto p_state_publish_rate = rclcpp::ParameterValue(params_.state_publish_rate_); -parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); -} else { -params_.state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double();} + } + return result; + } - } - }; + void declare_params(const std::shared_ptr ¶meters_interface) { + if (!parameters_interface->has_parameter("joints")) { + auto p_joints = rclcpp::ParameterValue(params_.joints_); + parameters_interface->declare_parameter("joints", p_joints); + } else { + params_.joints_ = parameters_interface->get_parameter("joints").as_string_array(); + } + if (!parameters_interface->has_parameter("command_interfaces")) { + auto p_command_interfaces = rclcpp::ParameterValue(params_.command_interfaces_); + parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); + } else { + params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); + } + if (!parameters_interface->has_parameter("state_interfaces")) { + auto p_state_interfaces = rclcpp::ParameterValue(params_.state_interfaces_); + parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); + } else { + params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); + } + if (!parameters_interface->has_parameter("chainable_command_interfaces")) { + auto p_chainable_command_interfaces = rclcpp::ParameterValue(params_.chainable_command_interfaces_); + parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); + } else { + params_.chainable_command_interfaces_ = parameters_interface->get_parameter( + "chainable_command_interfaces").as_string_array(); + } + if (!parameters_interface->has_parameter("kinematics.plugin_name")) { + auto p_kinematics_plugin_name = rclcpp::ParameterValue(params_.kinematics_.plugin_name_); + parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); + } else { + params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); + } + if (!parameters_interface->has_parameter("kinematics.plugin_package")) { + auto p_kinematics_plugin_package = rclcpp::ParameterValue(params_.kinematics_.plugin_package_); + parameters_interface->declare_parameter("kinematics.plugin_package", p_kinematics_plugin_package); + } else { + params_.kinematics_.plugin_package_ = parameters_interface->get_parameter( + "kinematics.plugin_package").as_string(); + } + if (!parameters_interface->has_parameter("kinematics.base")) { + auto p_kinematics_base = rclcpp::ParameterValue(params_.kinematics_.base_); + parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); + } else { + params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); + } + if (!parameters_interface->has_parameter("kinematics.tip")) { + auto p_kinematics_tip = rclcpp::ParameterValue(params_.kinematics_.tip_); + parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); + } else { + params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); + } + if (!parameters_interface->has_parameter("kinematics.group_name")) { + auto p_kinematics_group_name = rclcpp::ParameterValue(params_.kinematics_.group_name_); + parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); + } else { + params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); + } + if (!parameters_interface->has_parameter("kinematics.alpha")) { + auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); + parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); + } else { + params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); + } + if (!parameters_interface->has_parameter("ft_sensor.name")) { + auto p_ft_sensor_name = rclcpp::ParameterValue(params_.ft_sensor_.name_); + parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); + } else { + params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); + } + if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { + auto p_ft_sensor_frame_id = rclcpp::ParameterValue(params_.ft_sensor_.frame_.id_); + parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); + } else { + params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); + } + if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { + auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); + parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); + } else { + params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); + } + if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { + auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); + parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); + } else { + params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( + "ft_sensor.filter_coefficient").as_double(); + } + if (!parameters_interface->has_parameter("control.frame.id")) { + auto p_control_frame_id = rclcpp::ParameterValue(params_.control_.frame_.id_); + parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); + } else { + params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); + } + if (!parameters_interface->has_parameter("control.frame.external")) { + auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); + parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); + } else { + params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); + } + if (!parameters_interface->has_parameter("fixed_world_frame.id")) { + auto p_fixed_world_frame_id = rclcpp::ParameterValue(params_.fixed_world_frame_.id_); + parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); + } else { + params_.fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string(); + } + if (!parameters_interface->has_parameter("fixed_world_frame.external")) { + auto p_fixed_world_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.external_); + parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); + } else { + params_.fixed_world_frame_.external_ = parameters_interface->get_parameter( + "fixed_world_frame.external").as_bool(); + } + if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { + auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.id_); + parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); + } else { + params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( + "gravity_compensation.frame.id").as_string(); + } + if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { + auto p_gravity_compensation_frame_external = rclcpp::ParameterValue( + params_.gravity_compensation_.frame_.external_); + parameters_interface->declare_parameter("gravity_compensation.frame.external", + p_gravity_compensation_frame_external); + } else { + params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( + "gravity_compensation.frame.external").as_bool(); + } + if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { + auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.pos_); + parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); + } else { + params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( + "gravity_compensation.CoG.pos").as_double_array(); + } + if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { + auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); + parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); + } else { + params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( + "gravity_compensation.CoG.force").as_double(); + } + if (!parameters_interface->has_parameter("admittance.selected_axes")) { + auto p_admittance_selected_axes = rclcpp::ParameterValue(params_.admittance_.selected_axes_); + parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); + } else { + params_.admittance_.selected_axes_ = parameters_interface->get_parameter( + "admittance.selected_axes").as_bool_array(); + } + if (!parameters_interface->has_parameter("admittance.mass")) { + auto p_admittance_mass = rclcpp::ParameterValue(params_.admittance_.mass_); + parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); + } else { + params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); + } + if (!parameters_interface->has_parameter("admittance.damping_ratio")) { + auto p_admittance_damping_ratio = rclcpp::ParameterValue(params_.admittance_.damping_ratio_); + parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); + } else { + params_.admittance_.damping_ratio_ = parameters_interface->get_parameter( + "admittance.damping_ratio").as_double_array(); + } + if (!parameters_interface->has_parameter("admittance.stiffness")) { + auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); + parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); + } else { + params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); + } + if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { + auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( + params_.enable_parameter_update_without_reactivation_); + parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", + p_enable_parameter_update_without_reactivation); + } else { + params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( + "enable_parameter_update_without_reactivation").as_bool(); + } + if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { + auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); + parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); + } else { + params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter( + "use_feedforward_commanded_input").as_bool(); + } + if (!parameters_interface->has_parameter("joint_limiter_type")) { + auto p_joint_limiter_type = rclcpp::ParameterValue(params_.joint_limiter_type_); + parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); + } else { + params_.joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string(); + } + if (!parameters_interface->has_parameter("state_publish_rate")) { + auto p_state_publish_rate = rclcpp::ParameterValue(params_.state_publish_rate_); + parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); + } else { + params_.state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double(); + } + + } + }; } // namespace admittance_struct_parameters From 84ccddeccedd5841bbed376318a3a65b5927c8ea Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 3 Jul 2022 21:00:24 -0600 Subject: [PATCH 030/111] enable tests: currently failing --- admittance_controller/CMakeLists.txt | 41 +++-- .../config/admittance_controller.yaml | 1 + .../src/admittance_controller.cpp | 126 ++++++++++------ .../test/test_admittance_controller.cpp | 120 +++++++++------ .../test/test_admittance_controller.hpp | 140 +++++++++--------- .../test/test_load_admittance_controller.cpp | 13 +- admittance_controller/test/test_params.yaml | 100 +++++++++++++ 7 files changed, 372 insertions(+), 169 deletions(-) create mode 100644 admittance_controller/test/test_params.yaml diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index f6966b7b57..46e29baf7b 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -98,23 +98,43 @@ install( DESTINATION include ) -set(BUILD_TESTING 0) # TODO enable testing one test are fixed if (BUILD_TESTING) + # create custom test function to pass yaml file into test main + function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) + add_executable(${TARGET} ${SOURCES}) + _ament_cmake_gmock_find_gmock() + target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") + target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) + set(executable "$") + set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") + ament_add_test( + ${TARGET} + COMMAND ${executable} --ros-args --params-file ${YAML_FILE} + --gtest_output=xml:${result_file} + OUTPUT_FILE ${CMAKE_BINARY_DIR}/ament_cmake_gmock/test_load_admittance_controller.txt + RESULT_FILE ${result_file} + ) + endfunction() + # find testing packages find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - - ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) - target_include_directories(test_load_admittance_controller PRIVATE include) + # test loading admittance controller + add_test_with_yaml_input(test_load_admittance_controller "test/test_load_admittance_controller.cpp" + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_load_admittance_controller PUBLIC "${GMOCK_INCLUDE_DIRS}") + target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) ament_target_dependencies( - test_load_admittance_controller - controller_manager - hardware_interface - ros2_control_test_assets + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets ) - - ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) + # test admittance controller function +# ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) + add_test_with_yaml_input(test_admittance_controller "test/test_admittance_controller.cpp" + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) target_include_directories(test_admittance_controller PRIVATE include) target_link_libraries(test_admittance_controller admittance_controller) ament_target_dependencies( @@ -124,6 +144,7 @@ if (BUILD_TESTING) hardware_interface ros2_control_test_assets ) + endif () ament_export_include_directories( diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml index fddd499ce6..7a33208e60 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml +++ b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml @@ -95,3 +95,4 @@ admittance_struct: use_feedforward_commanded_input: true joint_limiter_type: "joint_limits/SimpleJointLimiter" state_publish_rate: 200.0 # Defaults to 50 + diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 9ac4365510..6a39754a49 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -49,44 +49,44 @@ namespace admittance_controller { // initialize controller config admittance_ = std::make_unique(); - try { - params = { - get_node()->get_parameter("joints").as_string_array(), - get_node()->get_parameter("command_interfaces").as_string_array(), - get_node()->get_parameter("state_interfaces").as_string_array(), - get_node()->get_parameter("chainable_command_interfaces").as_string_array(), - }; - } catch (const std::exception &e) { - RCLCPP_ERROR(get_node()->get_logger(), - "Exception thrown during init stage while reading parameters with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - for (const auto &tmp: params.state_interface_types_) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); - } - for (const auto &tmp: params.command_interface_types_) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); - } - for (const auto &tmp: params.chainable_command_interface_types_) { - if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); - } else { - RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", - tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); - return CallbackReturn::ERROR; - } - } - - try { - admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); - admittance_->parameters_ = &admittance_->parameter_handler_->params_; - } catch (const std::exception &e) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - num_joints_ = params.joint_names_.size(); +// try { +// params = { +// get_node()->get_parameter("joints").as_string_array(), +// get_node()->get_parameter("command_interfaces").as_string_array(), +// get_node()->get_parameter("state_interfaces").as_string_array(), +// get_node()->get_parameter("chainable_command_interfaces").as_string_array(), +// }; +// } catch (const std::exception &e) { +// RCLCPP_ERROR(get_node()->get_logger(), +// "Exception thrown during init stage while reading parameters with message: %s \n", e.what()); +// return CallbackReturn::ERROR; +// } +// +// for (const auto &tmp: params.state_interface_types_) { +// RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); +// } +// for (const auto &tmp: params.command_interface_types_) { +// RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); +// } +// for (const auto &tmp: params.chainable_command_interface_types_) { +// if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { +// RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); +// } else { +// RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", +// tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); +// return CallbackReturn::ERROR; +// } +// } +// +// try { +// admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); +// admittance_->parameters_ = &admittance_->parameter_handler_->params_; +// } catch (const std::exception &e) { +// RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); +// return CallbackReturn::ERROR; +// } +// +// num_joints_ = params.joint_names_.size(); return CallbackReturn::SUCCESS; } @@ -186,6 +186,46 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { // Print output so users can be sure the interface setup is correct + try { + params = { + get_node()->get_parameter("joints").as_string_array(), + get_node()->get_parameter("command_interfaces").as_string_array(), + get_node()->get_parameter("state_interfaces").as_string_array(), + get_node()->get_parameter("chainable_command_interfaces").as_string_array(), + }; + } catch (const std::exception &e) { + RCLCPP_ERROR(get_node()->get_logger(), + "Exception thrown during init stage while reading parameters with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + for (const auto &tmp: params.state_interface_types_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto &tmp: params.command_interface_types_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + for (const auto &tmp: params.chainable_command_interface_types_) { + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", + tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } + } + + try { + admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); + admittance_->parameters_ = &admittance_->parameter_handler_->params_; + } catch (const std::exception &e) { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + num_joints_ = params.joint_names_.size(); + + auto get_interface_list = [](const std::vector &interface_types) { std::stringstream ss_command_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { @@ -203,7 +243,7 @@ namespace admittance_controller { // setup subscribers and publishers input_joint_command_subscriber_ = get_node()->create_subscription( - "~/pose_commands", rclcpp::SystemDefaultsQoS(), + "~/joint_commands", rclcpp::SystemDefaultsQoS(), std::bind(&AdmittanceController::joint_command_callback, this, std::placeholders::_1)); s_publisher_ = get_node()->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); @@ -223,7 +263,9 @@ namespace admittance_controller { semantic_components::ForceTorqueSensor(admittance_->parameters_->ft_sensor_.name_)); // configure admittance rule - admittance_->configure(get_node(), num_joints_); + if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR){ + return CallbackReturn::ERROR; + } return LifecycleNodeInterface::on_configure(previous_state); } @@ -242,7 +284,7 @@ namespace admittance_controller { joint_position_state_interface_.clear(); joint_velocity_state_interface_.clear(); - // assign state interfaces + // assign command interfaces std::unordered_map < std::string, std::vector < std::reference_wrapper < hardware_interface::LoanedCommandInterface >> * > command_interface_map = { @@ -256,7 +298,7 @@ namespace admittance_controller { } } - // assign command interfaces + // assign state interfaces std::unordered_map < std::string, std::vector < std::reference_wrapper < hardware_interface::LoanedStateInterface >> * > state_interface_map = { {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 36999d764e..317e763e4f 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -21,11 +21,21 @@ #include #include + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_invalid) +{ + SetUpController(true); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + // When there are many mandatory parameters, set all by default and remove one by one in a // parameterized test TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_missing) { - SetUpController(); + SetUpController(false); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } @@ -60,11 +70,11 @@ INSTANTIATE_TEST_SUITE_P( // rclcpp::ParameterValue(false) // ), std::make_tuple( - std::string("IK.base"), + std::string("kinematics.base"), rclcpp::ParameterValue("") ), std::make_tuple( - std::string("IK.group_name"), + std::string("kinematics.group_name"), rclcpp::ParameterValue("") ), std::make_tuple( @@ -177,57 +187,61 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { - SetUpController(); - ASSERT_TRUE(controller_->joint_names_.empty()); - ASSERT_TRUE(controller_->command_interface_types_.empty()); + ASSERT_TRUE(controller_->params.joint_names_.empty()); + ASSERT_TRUE(controller_->params.command_interface_types_.empty()); + + SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(!controller_->joint_names_.empty()); - ASSERT_TRUE(controller_->joint_names_.size() == joint_names_.size()); - ASSERT_TRUE(std::equal(controller_->joint_names_.begin(), controller_->joint_names_.end(), + ASSERT_TRUE(!controller_->params.joint_names_.empty()); + ASSERT_TRUE(controller_->params.joint_names_.size() == joint_names_.size()); + ASSERT_TRUE(std::equal(controller_->params.joint_names_.begin(), controller_->params.joint_names_.end(), joint_names_.begin(), joint_names_.end())); - ASSERT_TRUE(!controller_->command_interface_types_.empty()); - ASSERT_TRUE(controller_->command_interface_types_.size() == command_interface_types_.size()); + ASSERT_TRUE(!controller_->params.command_interface_types_.empty()); + ASSERT_TRUE(controller_->params.command_interface_types_.size() == command_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->command_interface_types_.begin(), controller_->command_interface_types_.end(), + controller_->params.command_interface_types_.begin(), controller_->params.command_interface_types_.end(), command_interface_types_.begin(), command_interface_types_.end())); - ASSERT_TRUE(!controller_->state_interface_types_.empty()); - ASSERT_TRUE(controller_->state_interface_types_.size() == state_interface_types_.size()); + ASSERT_TRUE(!controller_->params.state_interface_types_.empty()); + ASSERT_TRUE(controller_->params.state_interface_types_.size() == state_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->state_interface_types_.begin(), controller_->state_interface_types_.end(), + controller_->params.state_interface_types_.begin(), controller_->params.state_interface_types_.end(), state_interface_types_.begin(), state_interface_types_.end())); - ASSERT_EQ(controller_->ft_sensor_name_, ft_sensor_name_); - ASSERT_EQ(controller_->admittance_->parameters_.ik_base_frame_, ik_base_frame_); - ASSERT_EQ(controller_->admittance_->parameters_.ik_group_name_, ik_group_name_); - ASSERT_EQ(controller_->admittance_->parameters_.sensor_frame_, sensor_frame_); + ASSERT_EQ(controller_->admittance_->parameters_->ft_sensor_.name_, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->parameters_->kinematics_.base_, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->parameters_->kinematics_.group_name_, ik_group_name_); + ASSERT_EQ(controller_->admittance_->parameters_->ft_sensor_.frame_.id_, sensor_frame_); - ASSERT_TRUE(!controller_->admittance_->parameters_.selected_axes_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.selected_axes_.size() == admittance_selected_axes_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.selected_axes_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.selected_axes_.size() == admittance_selected_axes_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.selected_axes_.begin(), controller_->admittance_->parameters_.selected_axes_.end(), + controller_->admittance_->parameters_->admittance_.selected_axes_.begin(), + controller_->admittance_->parameters_->admittance_.selected_axes_.end(), admittance_selected_axes_.begin(), admittance_selected_axes_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_.mass_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.mass_.size() == admittance_mass_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.mass_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.mass_.size() == admittance_mass_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.mass_.begin(), controller_->admittance_->parameters_.mass_.end(), + controller_->admittance_->parameters_->admittance_.mass_.begin(), controller_->admittance_->parameters_->admittance_.mass_.end(), admittance_mass_.begin(), admittance_mass_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_.damping_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.damping_.size() == admittance_damping_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.damping_ratio_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.damping_ratio_.size() == admittance_damping_ratio_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.damping_.begin(), controller_->admittance_->parameters_.damping_.end(), - admittance_damping_.begin(), admittance_damping_.end())); + controller_->admittance_->parameters_->admittance_.damping_ratio_.begin(), + controller_->admittance_->parameters_->admittance_.damping_ratio_.end(), + admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_.stiffness_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.stiffness_.size() == admittance_stiffness_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.stiffness_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.stiffness_.size() == admittance_stiffness_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.stiffness_.begin(), controller_->admittance_->parameters_.stiffness_.end(), + controller_->admittance_->parameters_->admittance_.stiffness_.begin(), + controller_->admittance_->parameters_->admittance_.stiffness_.end(), admittance_stiffness_.begin(), admittance_stiffness_.end())); } @@ -254,29 +268,30 @@ TEST_F(AdmittanceControllerTest, activate_success) SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - // Check ordered interfaces - ASSERT_TRUE(!controller_->joint_command_interface_.empty()); + // Check command interfaces size + std::unordered_map < std::string, + std::vector < std::reference_wrapper < hardware_interface::LoanedCommandInterface >> * > + command_interface_map = { + {hardware_interface::HW_IF_POSITION, &controller_->joint_position_command_interface_}, + {hardware_interface::HW_IF_VELOCITY, &controller_->joint_velocity_command_interface_} + }; for (const auto & interface : command_interface_types_) { - auto it = std::find( - controller_->allowed_interface_types_.begin(), controller_->allowed_interface_types_.end(), interface); - auto index = std::distance(controller_->allowed_interface_types_.begin(), it); - - ASSERT_TRUE(controller_->joint_command_interface_[index].size() == joint_names_.size()); + ASSERT_TRUE(command_interface_map[interface]->size() == joint_names_.size()); } - - ASSERT_TRUE(!controller_->joint_state_interface_.empty()); - for (const auto & interface : state_interface_types_) { - auto it = std::find( - controller_->allowed_interface_types_.begin(), controller_->allowed_interface_types_.end(), interface); - auto index = std::distance(controller_->allowed_interface_types_.begin(), it); - - ASSERT_TRUE(controller_->joint_state_interface_[index].size() == joint_names_.size()); + // Check state interfaces size + // assign state interfaces + std::unordered_map < std::string, + std::vector < std::reference_wrapper < hardware_interface::LoanedStateInterface >> * > state_interface_map = { + {hardware_interface::HW_IF_POSITION, &controller_->joint_position_state_interface_}, + {hardware_interface::HW_IF_VELOCITY, &controller_->joint_velocity_state_interface_} + }; + for (const auto & interface : command_interface_types_) { + ASSERT_TRUE(state_interface_map[interface]->size() == joint_names_.size()); } + } TEST_F(AdmittanceControllerTest, update_success) @@ -442,5 +457,12 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); } +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} // Add test, wrong interfaces diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 6476956d91..d0f7e19ffa 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -40,7 +40,7 @@ // TODO(anyone): replace the state and command message types using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; -using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectoryPoint; using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; namespace @@ -80,10 +80,7 @@ class TestableAdmittanceController admittance_controller::AdmittanceController::on_configure(previous_state); // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - if (admittance_->unified_mode_) { - input_wrench_command_subscriber_wait_set_.add_subscription(input_wrench_command_subscriber_); - } - input_pose_command_subscriber_wait_set_.add_subscription(input_pose_command_subscriber_); + input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); } return ret; } @@ -101,9 +98,6 @@ class TestableAdmittanceController { bool success = input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (admittance_->unified_mode_) { - success = success && input_wrench_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - } if (success) { executor.spin_some(); } @@ -120,7 +114,7 @@ class AdmittanceControllerTest : public ::testing::Test public: static void SetUpTestCase() { - rclcpp::init(0, nullptr); +// rclcpp::init(0, nullptr); } void SetUp() @@ -142,7 +136,7 @@ class AdmittanceControllerTest : public ::testing::Test static void TearDownTestCase() { - rclcpp::shutdown(); +// rclcpp::shutdown(); } void TearDown() @@ -157,58 +151,60 @@ class AdmittanceControllerTest : public ::testing::Test ASSERT_EQ(result, controller_interface::return_type::OK); assign_interfaces(); + controller_->get_node()->set_parameter({"robot_description", robot_description_}); + controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); - if (set_parameters) { - controller_->get_node()->set_parameter({"use_joint_commands_as_input", use_joint_commands_as_input}); - - controller_->get_node()->set_parameter({"joints", joint_names_}); - controller_->get_node()->set_parameter({"command_interfaces", command_interface_types_}); - controller_->get_node()->set_parameter({"state_interfaces", state_interface_types_}); - controller_->get_node()->set_parameter({"ft_sensor_name", ft_sensor_name_}); - controller_->get_node()->set_parameter({"hardware_state_has_offset", hardware_state_has_offset_}); - - controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); - controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); - // TODO(destogl): enable when IK support is added -// controller_->get_node()->set_parameter({"IK.plugin", ik_group_name_}); - controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_}); - controller_->get_node()->set_parameter({"robot_description", robot_description_}); - controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); - - controller_->get_node()->set_parameter({"control_frame", control_frame_}); - controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_}); - controller_->get_node()->set_parameter({"fixed_world_frame", fixed_world_frame_}); - controller_->get_node()->set_parameter({"sensor_frame", sensor_frame_}); - - controller_->get_node()->set_parameter({"admittance.selected_axes.x", admittance_selected_axes_[0]}); - controller_->get_node()->set_parameter({"admittance.selected_axes.y", admittance_selected_axes_[1]}); - controller_->get_node()->set_parameter({"admittance.selected_axes.z", admittance_selected_axes_[2]}); - controller_->get_node()->set_parameter({"admittance.selected_axes.rx", admittance_selected_axes_[3]}); - controller_->get_node()->set_parameter({"admittance.selected_axes.ry", admittance_selected_axes_[4]}); - controller_->get_node()->set_parameter({"admittance.selected_axes.rz", admittance_selected_axes_[5]}); - - controller_->get_node()->set_parameter({"admittance.mass.x", admittance_mass_[0]}); - controller_->get_node()->set_parameter({"admittance.mass.y", admittance_mass_[1]}); - controller_->get_node()->set_parameter({"admittance.mass.z", admittance_mass_[2]}); - controller_->get_node()->set_parameter({"admittance.mass.rx", admittance_mass_[3]}); - controller_->get_node()->set_parameter({"admittance.mass.ry", admittance_mass_[4]}); - controller_->get_node()->set_parameter({"admittance.mass.rz", admittance_mass_[5]}); - - controller_->get_node()->set_parameter({"admittance.damping.x", admittance_damping_[0]}); - controller_->get_node()->set_parameter({"admittance.damping.y", admittance_damping_[1]}); - controller_->get_node()->set_parameter({"admittance.damping.z", admittance_damping_[2]}); - controller_->get_node()->set_parameter({"admittance.damping.rx", admittance_damping_[3]}); - controller_->get_node()->set_parameter({"admittance.damping.ry", admittance_damping_[4]}); - controller_->get_node()->set_parameter({"admittance.damping.rz", admittance_damping_[5]}); - - controller_->get_node()->set_parameter({"admittance.stiffness.x", admittance_stiffness_[0]}); - controller_->get_node()->set_parameter({"admittance.stiffness.y", admittance_stiffness_[1]}); - controller_->get_node()->set_parameter({"admittance.stiffness.z", admittance_stiffness_[2]}); - controller_->get_node()->set_parameter({"admittance.stiffness.rx", admittance_stiffness_[3]}); - controller_->get_node()->set_parameter({"admittance.stiffness.ry", admittance_stiffness_[4]}); - controller_->get_node()->set_parameter({"admittance.stiffness.rz", admittance_stiffness_[5]}); - - } +// if (set_parameters) { +// controller_->get_node()->set_parameter({"use_joint_commands_as_input", use_joint_commands_as_input}); +// +// controller_->get_node()->set_parameter({"joints", joint_names_}); +// controller_->get_node()->set_parameter({"command_interfaces", command_interface_types_}); +// controller_->get_node()->set_parameter({"state_interfaces", state_interface_types_}); +// controller_->get_node()->set_parameter({"ft_sensor_name", ft_sensor_name_}); +// controller_->get_node()->set_parameter({"hardware_state_has_offset", hardware_state_has_offset_}); +// +// controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); +// controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); +// // TODO(destogl): enable when IK support is added +//// controller_->get_node()->set_parameter({"IK.plugin", ik_group_name_}); +// controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_}); +// controller_->get_node()->set_parameter({"robot_description", robot_description_}); +// controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); +// +// controller_->get_node()->set_parameter({"control_frame", control_frame_}); +// controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_}); +// controller_->get_node()->set_parameter({"fixed_world_frame", fixed_world_frame_}); +// controller_->get_node()->set_parameter({"sensor_frame", sensor_frame_}); +// +// controller_->get_node()->set_parameter({"admittance.selected_axes.x", admittance_selected_axes_[0]}); +// controller_->get_node()->set_parameter({"admittance.selected_axes.y", admittance_selected_axes_[1]}); +// controller_->get_node()->set_parameter({"admittance.selected_axes.z", admittance_selected_axes_[2]}); +// controller_->get_node()->set_parameter({"admittance.selected_axes.rx", admittance_selected_axes_[3]}); +// controller_->get_node()->set_parameter({"admittance.selected_axes.ry", admittance_selected_axes_[4]}); +// controller_->get_node()->set_parameter({"admittance.selected_axes.rz", admittance_selected_axes_[5]}); +// +// controller_->get_node()->set_parameter({"admittance.mass.x", admittance_mass_[0]}); +// controller_->get_node()->set_parameter({"admittance.mass.y", admittance_mass_[1]}); +// controller_->get_node()->set_parameter({"admittance.mass.z", admittance_mass_[2]}); +// controller_->get_node()->set_parameter({"admittance.mass.rx", admittance_mass_[3]}); +// controller_->get_node()->set_parameter({"admittance.mass.ry", admittance_mass_[4]}); +// controller_->get_node()->set_parameter({"admittance.mass.rz", admittance_mass_[5]}); +// +// controller_->get_node()->set_parameter({"admittance.damping.x", admittance_damping_ratio_[0]}); +// controller_->get_node()->set_parameter({"admittance.damping.y", admittance_damping_ratio_[1]}); +// controller_->get_node()->set_parameter({"admittance.damping.z", admittance_damping_ratio_[2]}); +// controller_->get_node()->set_parameter({"admittance.damping.rx", admittance_damping_ratio_[3]}); +// controller_->get_node()->set_parameter({"admittance.damping.ry", admittance_damping_ratio_[4]}); +// controller_->get_node()->set_parameter({"admittance.damping.rz", admittance_damping_ratio_[5]}); +// +// controller_->get_node()->set_parameter({"admittance.stiffness.x", admittance_stiffness_[0]}); +// controller_->get_node()->set_parameter({"admittance.stiffness.y", admittance_stiffness_[1]}); +// controller_->get_node()->set_parameter({"admittance.stiffness.z", admittance_stiffness_[2]}); +// controller_->get_node()->set_parameter({"admittance.stiffness.rx", admittance_stiffness_[3]}); +// controller_->get_node()->set_parameter({"admittance.stiffness.ry", admittance_stiffness_[4]}); +// controller_->get_node()->set_parameter({"admittance.stiffness.rz", admittance_stiffness_[5]}); +// +// } } void assign_interfaces() @@ -362,7 +358,7 @@ class AdmittanceControllerTest : public ::testing::Test wait_for_topic(joint_command_publisher_->get_topic_name()); ControllerCommandJointMsg joint_msg; - joint_msg.joint_names = joint_names_; +// joint_msg.joint_names = joint_names_; trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; auto num_joints = joint_names_.size(); trajectory_point.positions.reserve(num_joints); @@ -370,7 +366,7 @@ class AdmittanceControllerTest : public ::testing::Test for (auto index = 0u; index < num_joints; ++index) { trajectory_point.positions.emplace_back(joint_state_values_[index]); } - joint_msg.points.emplace_back(trajectory_point); + joint_msg = trajectory_point; joint_command_publisher_->publish(joint_msg); } @@ -399,7 +395,7 @@ class AdmittanceControllerTest : public ::testing::Test std::array admittance_selected_axes_ = {true, true, true, true, true, true}; std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - std::array admittance_damping_ = {100.1, 100.2, 100.3, 100.4, 100.5, 100.6}; + std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427 , 2.828427, 2.828427, 2.828427 }; std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; @@ -440,8 +436,20 @@ public ::testing::WithParamInterfaceget_node()->undeclare_parameter(std::get<0>(GetParam())); - controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); +// controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); +// controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); +// rclcpp::Parameter parameter("joints"); +// controller_->get_node()->set_parameter(parameter); +// controller_->get_node()->set_parameter({"joints", ""}); + if (set_parameters){ + auto tmp = std::get<1>(GetParam()); + rclcpp::Parameter parameter(std::get<0>(GetParam()),std::get<1>(GetParam())); + } else{ + controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); + } + + +// controller_->get_node()->set_parameter(parameter); } }; diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index acbb3365a5..ce535a1df7 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -25,7 +25,7 @@ TEST(TestLoadAdmittanceController, load_controller) { - rclcpp::init(0, nullptr); + std::shared_ptr executor = std::make_shared(); @@ -36,6 +36,15 @@ TEST(TestLoadAdmittanceController, load_controller) ASSERT_NO_THROW( cm.load_controller( - "test_admittance_controller", + "load_admittance_controller", "admittance_controller/AdmittanceController")); + +} + +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/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml new file mode 100644 index 0000000000..37550447aa --- /dev/null +++ b/admittance_controller/test/test_params.yaml @@ -0,0 +1,100 @@ +load_admittance_controller: + # contains minimal parameters that need to be set to load controller + ros__parameters: + joints: + - joint1 + - joint2 + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + + chainable_command_interfaces: + - position + - velocity + + +test_admittance_controller: + # contains minimal needed parameters for kuka_kr6 + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + + chainable_command_interfaces: + - position + - velocity + + kinematics: + plugin_name: kinematics_interface_kdl/KDLKinematics + plugin_package: kinematics_interface_kdl + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: sensor_frame # tool0 Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 + + From 976d286b9cf8e293459fd54c530e5e8cb5e672d3 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 4 Jul 2022 21:19:47 -0600 Subject: [PATCH 031/111] updated auto generated struct with parameter validation --- admittance_controller/CMakeLists.txt | 6 +- .../admittance_rule_impl.hpp | 8 +- .../config/admittance_controller.yaml | 98 --- .../config/admittance_struct.h | 670 +++++++++++------- .../config/definition.yaml | 202 ++++++ 5 files changed, 630 insertions(+), 354 deletions(-) delete mode 100644 admittance_controller/include/admittance_controller/config/admittance_controller.yaml create mode 100644 admittance_controller/include/admittance_controller/config/definition.yaml diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 46e29baf7b..089def0aa9 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -48,13 +48,11 @@ target_include_directories( # uncomment to generate struct ## specify which yaml root name to convert to struct -#set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config/admittance_controller.yaml) -## specify which yaml root name to convert to struct -#set(YAML_TARGET admittance_struct) +#set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config/definition.yaml) ## specify which yaml root name to convert to struct #set(OUT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config) # -#generate_param_struct_header(admittance_controller ${OUT_DIR} ${YAML_FILE} ${YAML_TARGET}) +#generate_param_struct_header(admittance_controller ${OUT_DIR} ${YAML_FILE}) ament_target_dependencies( diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 115b542d4d..abbf80c54a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -159,8 +159,8 @@ namespace admittance_controller { ee_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); - world_transform_ = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.id_, - parameters_->fixed_world_frame_.external_, success); + world_transform_ = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.frame_.id_, + parameters_->fixed_world_frame_.frame_.external_, success); sensor_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); cog_transform_ = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, @@ -487,8 +487,8 @@ namespace admittance_controller { void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { - eigen_to_msg(wrench_world_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench_filtered); - eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.id_, state_message.measured_wrench); + eigen_to_msg(wrench_world_, parameters_->fixed_world_frame_.frame_.id_, state_message.measured_wrench_filtered); + eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.frame_.id_, state_message.measured_wrench); eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_->control_.frame_.id_, state_message.measured_wrench_control_frame); eigen_to_msg(ee_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_->kinematics_.tip_, diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml b/admittance_controller/include/admittance_controller/config/admittance_controller.yaml deleted file mode 100644 index 7a33208e60..0000000000 --- a/admittance_controller/include/admittance_controller/config/admittance_controller.yaml +++ /dev/null @@ -1,98 +0,0 @@ -admittance_struct: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - - command_interfaces: - - position - - state_interfaces: - - position - - velocity - - chainable_command_interfaces: - - position - - velocity - - kinematics: - plugin_name: kdl_plugin/KDLKinematics - plugin_package: kinematics_interface_kdl - base: base_link # Assumed to be stationary - tip: ee_link - group_name: ur5e_manipulator - alpha: 0.0005 - - ft_sensor: - name: tcp_fts_sensor - frame: - id: ee_link # ee_link Wrench measurements are in this frame - external: false # force torque frame exists within URDF kinematic chain - filter_coefficient: 0.005 - - control: - frame: - id: ee_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - external: false # control frame exists within URDF kinematic chain - - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - external: false # control frame exists within URDF kinematic chain - - gravity_compensation: - frame: - id: ee_link - external: false - - CoG: # specifies the center of gravity of the end effector - pos: - - 0.1 # x - - 0.0 # y - - 0.0 # z - force: 23.0 # mass * 9.81 - - admittance: - selected_axes: - - true # x - - true # y - - true # z - - true # rx - - true # ry - - true # rz - - # Having ".0" at the end is MUST, otherwise there is a loading error - # F = M*a + D*v + S*(x - x_d) - mass: - - 3.0 # x - - 3.0 # y - - 3.0 # z - - 0.05 # rx - - 0.05 # ry - - 0.05 # rz - - damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) - - 2.828427 # x - - 2.828427 # y - - 2.828427 # z - - 2.828427 # rx - - 2.828427 # ry - - 2.828427 # rz - - stiffness: - - 50.0 # x - - 50.0 # y - - 50.0 # z - - 1.0 # rx - - 1.0 # ry - - 1.0 # rz - - # general settings - enable_parameter_update_without_reactivation: true - use_feedforward_commanded_input: true - joint_limiter_type: "joint_limits/SimpleJointLimiter" - state_publish_rate: 200.0 # Defaults to 50 - diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 2b102ea0d0..8f918e6f25 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -8,10 +8,9 @@ namespace admittance_struct_parameters { struct admittance_struct { - // if true, prevent parameters from updating - bool lock_params_ = false; std::shared_ptr handle_; + // throws rclcpp::exceptions::InvalidParameterValueException on initialization if invalid parameter are loaded admittance_struct(const std::shared_ptr ¶meters_interface) { declare_params(parameters_interface); auto update_param_cb = [this](const std::vector ¶meters) { @@ -20,81 +19,163 @@ namespace admittance_struct_parameters { handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); } + void + declare_overrides(const std::shared_ptr ¶meters_interface, + const std::map &overrides, + std::unordered_map &desc_map) { + + for (auto &key: overrides) { + if (!parameters_interface->has_parameter(key.first)) { + parameters_interface->declare_parameter(key.first, key.second, desc_map[key.first]); + } + } + } + + template + bool validate_length(const std::vector &values, size_t len) { + return values.size() == len; + } + + template + bool validate_length(const std::string &name, const std::vector &values, size_t len, + rcl_interfaces::msg::SetParametersResult &result) { + if (!validate_length(values, len)) { + result.reason = std::string("Invalid size for vector parameter ") + name + + ". Expected " + std::to_string(len) + " got " + std::to_string(values.size()); + return false; + } + return true; + } + + + template + bool validate_bounds(std::vector values, const T &lower_bound, const T &upper_bound) { + for (const auto &val: values) { + if (!validate_bounds(val, lower_bound, upper_bound)) { + return false; + } + } + return true; + } + + template + bool validate_bounds(const std::string &name, std::vector values, const T &lower_bound, const T &upper_bound, + rcl_interfaces::msg::SetParametersResult &result) { + for (const auto &val: values) { + if (!validate_bounds(name, val, lower_bound, upper_bound, result)) { + return false; + } + } + return true; + } + + + template + bool validate_bounds(T value, const T &lower_bound, const T &upper_bound) { + if (value > upper_bound || value < lower_bound) { + return false; + } + return true; + } + + template + bool validate_bounds(const std::string &name, T value, const T &lower_bound, const T &upper_bound, + rcl_interfaces::msg::SetParametersResult &result) { + if (!validate_bounds(value, lower_bound, upper_bound)) { + result.reason = std::string("Invalid value for parameter ") + name + ". Value not within the required bounds."; + return false; + } + return true; + } + + struct params { - std::vector joints_ = {"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", - "wrist_2_joint", "wrist_3_joint"}; - std::vector command_interfaces_ = {"position"}; - std::vector state_interfaces_ = {"position", "velocity"}; - std::vector chainable_command_interfaces_ = {"position", "velocity"}; + std::vector joints_ = {"UNDEFINED"}; + std::vector command_interfaces_ = {"UNDEFINED"}; + std::vector state_interfaces_ = {"UNDEFINED"}; + std::vector chainable_command_interfaces_ = {"UNDEFINED"}; struct kinematics { - std::string plugin_name_ = "kdl_plugin/KDLKinematics"; - std::string plugin_package_ = "kinematics_interface_kdl"; - std::string base_ = "base_link"; - std::string tip_ = "ee_link"; - std::string group_name_ = "ur5e_manipulator"; + std::string plugin_name_ = "UNDEFINED"; + std::string plugin_package_ = "UNDEFINED"; + std::string base_ = "UNDEFINED"; + std::string tip_ = "UNDEFINED"; double alpha_ = 0.0005; + std::string group_name_ = "UNDEFINED"; } kinematics_; struct ft_sensor { - std::string name_ = "tcp_fts_sensor"; + std::string name_ = "UNDEFINED"; struct frame { - std::string id_ = "ee_link"; + std::string id_ = "UNDEFINED"; bool external_ = false; } frame_; double filter_coefficient_ = 0.005; } ft_sensor_; struct control { struct frame { - std::string id_ = "ee_link"; + std::string id_ = "UNDEFINED"; bool external_ = false; } frame_; } control_; struct fixed_world_frame { - std::string id_ = "base_link"; - bool external_ = false; + struct frame { + std::string id_ = "UNDEFINED"; + bool external_ = false; + } frame_; } fixed_world_frame_; struct gravity_compensation { struct frame { - std::string id_ = "ee_link"; + std::string id_ = "UNDEFINED"; bool external_ = false; } frame_; struct CoG { - std::vector pos_ = {0.1, 0.0, 0.0}; - double force_ = 23.0; + std::vector pos_ = {std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + double force_ = std::numeric_limits::quiet_NaN(); } CoG_; } gravity_compensation_; struct admittance { - std::vector selected_axes_ = {true, true, true, true, true, true}; - std::vector mass_ = {3.0, 3.0, 3.0, 0.05, 0.05, 0.05}; - std::vector damping_ratio_ = {2.828427, 2.828427, 2.828427, 2.828427, 2.828427, 2.828427}; - std::vector stiffness_ = {50.0, 50.0, 50.0, 1.0, 1.0, 1.0}; + std::vector selected_axes_ = {false, false, false, false, false, false}; + std::vector mass_ = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::vector damping_ratio_ = {std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::vector stiffness_ = {std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; } admittance_; bool enable_parameter_update_without_reactivation_ = true; bool use_feedforward_commanded_input_ = true; - std::string joint_limiter_type_ = "joint_limits/SimpleJointLimiter"; - double state_publish_rate_ = 200.0; } params_; rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; - result.successful = !lock_params_; - if (lock_params_) { - result.reason = "The parameters can not be updated because they are currently locked."; - return result; - } result.reason = "success"; for (const auto ¶m: parameters) { - if (param.get_name() == "joints") { + if (param.get_name() == "joints" && validate_length("joints", param.as_string_array(), 1, result)) { params_.joints_ = param.as_string_array(); } - if (param.get_name() == "command_interfaces") { + if (param.get_name() == "command_interfaces" && + validate_length("command_interfaces", param.as_string_array(), 1, result)) { params_.command_interfaces_ = param.as_string_array(); } - if (param.get_name() == "state_interfaces") { + if (param.get_name() == "state_interfaces" && + validate_length("state_interfaces", param.as_string_array(), 1, result)) { params_.state_interfaces_ = param.as_string_array(); } - if (param.get_name() == "chainable_command_interfaces") { + if (param.get_name() == "chainable_command_interfaces" && + validate_length("chainable_command_interfaces", param.as_string_array(), 1, result)) { params_.chainable_command_interfaces_ = param.as_string_array(); } if (param.get_name() == "kinematics.plugin_name") { @@ -109,12 +190,12 @@ namespace admittance_struct_parameters { if (param.get_name() == "kinematics.tip") { params_.kinematics_.tip_ = param.as_string(); } - if (param.get_name() == "kinematics.group_name") { - params_.kinematics_.group_name_ = param.as_string(); - } if (param.get_name() == "kinematics.alpha") { params_.kinematics_.alpha_ = param.as_double(); } + if (param.get_name() == "kinematics.group_name") { + params_.kinematics_.group_name_ = param.as_string(); + } if (param.get_name() == "ft_sensor.name") { params_.ft_sensor_.name_ = param.as_string(); } @@ -133,11 +214,11 @@ namespace admittance_struct_parameters { if (param.get_name() == "control.frame.external") { params_.control_.frame_.external_ = param.as_bool(); } - if (param.get_name() == "fixed_world_frame.id") { - params_.fixed_world_frame_.id_ = param.as_string(); + if (param.get_name() == "fixed_world_frame.frame.id") { + params_.fixed_world_frame_.frame_.id_ = param.as_string(); } - if (param.get_name() == "fixed_world_frame.external") { - params_.fixed_world_frame_.external_ = param.as_bool(); + if (param.get_name() == "fixed_world_frame.frame.external") { + params_.fixed_world_frame_.frame_.external_ = param.as_bool(); } if (param.get_name() == "gravity_compensation.frame.id") { params_.gravity_compensation_.frame_.id_ = param.as_string(); @@ -145,22 +226,29 @@ namespace admittance_struct_parameters { if (param.get_name() == "gravity_compensation.frame.external") { params_.gravity_compensation_.frame_.external_ = param.as_bool(); } - if (param.get_name() == "gravity_compensation.CoG.pos") { + if (param.get_name() == "gravity_compensation.CoG.pos" && + validate_length("gravity_compensation.CoG.pos", param.as_double_array(), 3, result)) { params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); } if (param.get_name() == "gravity_compensation.CoG.force") { params_.gravity_compensation_.CoG_.force_ = param.as_double(); } - if (param.get_name() == "admittance.selected_axes") { + if (param.get_name() == "admittance.selected_axes" && + validate_length("admittance.selected_axes", param.as_bool_array(), 6, result)) { params_.admittance_.selected_axes_ = param.as_bool_array(); } - if (param.get_name() == "admittance.mass") { + if (param.get_name() == "admittance.mass" && + validate_length("admittance.mass", param.as_double_array(), 6, result) && + validate_bounds("admittance.mass", param.as_double_array(), 0.0001, 100000000.0, result)) { params_.admittance_.mass_ = param.as_double_array(); } - if (param.get_name() == "admittance.damping_ratio") { + if (param.get_name() == "admittance.damping_ratio" && + validate_length("admittance.damping_ratio", param.as_double_array(), 6, result) && + validate_bounds("admittance.damping_ratio", param.as_double_array(), 0.1, 10.0, result)) { params_.admittance_.damping_ratio_ = param.as_double_array(); } - if (param.get_name() == "admittance.stiffness") { + if (param.get_name() == "admittance.stiffness" && + validate_length("admittance.stiffness", param.as_double_array(), 6, result)) { params_.admittance_.stiffness_ = param.as_double_array(); } if (param.get_name() == "enable_parameter_update_without_reactivation") { @@ -169,214 +257,300 @@ namespace admittance_struct_parameters { if (param.get_name() == "use_feedforward_commanded_input") { params_.use_feedforward_commanded_input_ = param.as_bool(); } - if (param.get_name() == "joint_limiter_type") { - params_.joint_limiter_type_ = param.as_string(); - } - if (param.get_name() == "state_publish_rate") { - params_.state_publish_rate_ = param.as_double(); - } } return result; } void declare_params(const std::shared_ptr ¶meters_interface) { - if (!parameters_interface->has_parameter("joints")) { - auto p_joints = rclcpp::ParameterValue(params_.joints_); - parameters_interface->declare_parameter("joints", p_joints); - } else { - params_.joints_ = parameters_interface->get_parameter("joints").as_string_array(); - } - if (!parameters_interface->has_parameter("command_interfaces")) { - auto p_command_interfaces = rclcpp::ParameterValue(params_.command_interfaces_); - parameters_interface->declare_parameter("command_interfaces", p_command_interfaces); - } else { - params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); - } - if (!parameters_interface->has_parameter("state_interfaces")) { - auto p_state_interfaces = rclcpp::ParameterValue(params_.state_interfaces_); - parameters_interface->declare_parameter("state_interfaces", p_state_interfaces); - } else { - params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); - } - if (!parameters_interface->has_parameter("chainable_command_interfaces")) { - auto p_chainable_command_interfaces = rclcpp::ParameterValue(params_.chainable_command_interfaces_); - parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces); - } else { - params_.chainable_command_interfaces_ = parameters_interface->get_parameter( - "chainable_command_interfaces").as_string_array(); - } - if (!parameters_interface->has_parameter("kinematics.plugin_name")) { - auto p_kinematics_plugin_name = rclcpp::ParameterValue(params_.kinematics_.plugin_name_); - parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name); - } else { - params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.plugin_package")) { - auto p_kinematics_plugin_package = rclcpp::ParameterValue(params_.kinematics_.plugin_package_); - parameters_interface->declare_parameter("kinematics.plugin_package", p_kinematics_plugin_package); - } else { - params_.kinematics_.plugin_package_ = parameters_interface->get_parameter( - "kinematics.plugin_package").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.base")) { - auto p_kinematics_base = rclcpp::ParameterValue(params_.kinematics_.base_); - parameters_interface->declare_parameter("kinematics.base", p_kinematics_base); - } else { - params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.tip")) { - auto p_kinematics_tip = rclcpp::ParameterValue(params_.kinematics_.tip_); - parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip); - } else { - params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.group_name")) { - auto p_kinematics_group_name = rclcpp::ParameterValue(params_.kinematics_.group_name_); - parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name); - } else { - params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); - } - if (!parameters_interface->has_parameter("kinematics.alpha")) { - auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); - parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha); - } else { - params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); - } - if (!parameters_interface->has_parameter("ft_sensor.name")) { - auto p_ft_sensor_name = rclcpp::ParameterValue(params_.ft_sensor_.name_); - parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name); - } else { - params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); - } - if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { - auto p_ft_sensor_frame_id = rclcpp::ParameterValue(params_.ft_sensor_.frame_.id_); - parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id); - } else { - params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); - } - if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { - auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); - parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external); - } else { - params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { - auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); - parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient); - } else { - params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( - "ft_sensor.filter_coefficient").as_double(); - } - if (!parameters_interface->has_parameter("control.frame.id")) { - auto p_control_frame_id = rclcpp::ParameterValue(params_.control_.frame_.id_); - parameters_interface->declare_parameter("control.frame.id", p_control_frame_id); - } else { - params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); - } - if (!parameters_interface->has_parameter("control.frame.external")) { - auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); - parameters_interface->declare_parameter("control.frame.external", p_control_frame_external); - } else { - params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("fixed_world_frame.id")) { - auto p_fixed_world_frame_id = rclcpp::ParameterValue(params_.fixed_world_frame_.id_); - parameters_interface->declare_parameter("fixed_world_frame.id", p_fixed_world_frame_id); - } else { - params_.fixed_world_frame_.id_ = parameters_interface->get_parameter("fixed_world_frame.id").as_string(); - } - if (!parameters_interface->has_parameter("fixed_world_frame.external")) { - auto p_fixed_world_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.external_); - parameters_interface->declare_parameter("fixed_world_frame.external", p_fixed_world_frame_external); - } else { - params_.fixed_world_frame_.external_ = parameters_interface->get_parameter( - "fixed_world_frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { - auto p_gravity_compensation_frame_id = rclcpp::ParameterValue(params_.gravity_compensation_.frame_.id_); - parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id); - } else { - params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( - "gravity_compensation.frame.id").as_string(); - } - if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { - auto p_gravity_compensation_frame_external = rclcpp::ParameterValue( - params_.gravity_compensation_.frame_.external_); - parameters_interface->declare_parameter("gravity_compensation.frame.external", - p_gravity_compensation_frame_external); - } else { - params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( - "gravity_compensation.frame.external").as_bool(); - } - if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { - auto p_gravity_compensation_CoG_pos = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.pos_); - parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos); - } else { - params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( - "gravity_compensation.CoG.pos").as_double_array(); - } - if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { - auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); - parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force); - } else { - params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( - "gravity_compensation.CoG.force").as_double(); - } - if (!parameters_interface->has_parameter("admittance.selected_axes")) { - auto p_admittance_selected_axes = rclcpp::ParameterValue(params_.admittance_.selected_axes_); - parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes); - } else { - params_.admittance_.selected_axes_ = parameters_interface->get_parameter( - "admittance.selected_axes").as_bool_array(); - } - if (!parameters_interface->has_parameter("admittance.mass")) { - auto p_admittance_mass = rclcpp::ParameterValue(params_.admittance_.mass_); - parameters_interface->declare_parameter("admittance.mass", p_admittance_mass); - } else { - params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); - } - if (!parameters_interface->has_parameter("admittance.damping_ratio")) { - auto p_admittance_damping_ratio = rclcpp::ParameterValue(params_.admittance_.damping_ratio_); - parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio); - } else { - params_.admittance_.damping_ratio_ = parameters_interface->get_parameter( - "admittance.damping_ratio").as_double_array(); - } - if (!parameters_interface->has_parameter("admittance.stiffness")) { - auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); - parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness); - } else { - params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); - } - if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { - auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( - params_.enable_parameter_update_without_reactivation_); - parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", - p_enable_parameter_update_without_reactivation); - } else { - params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( - "enable_parameter_update_without_reactivation").as_bool(); - } - if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { - auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); - parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input); - } else { - params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter( - "use_feedforward_commanded_input").as_bool(); - } - if (!parameters_interface->has_parameter("joint_limiter_type")) { - auto p_joint_limiter_type = rclcpp::ParameterValue(params_.joint_limiter_type_); - parameters_interface->declare_parameter("joint_limiter_type", p_joint_limiter_type); - } else { - params_.joint_limiter_type_ = parameters_interface->get_parameter("joint_limiter_type").as_string(); - } - if (!parameters_interface->has_parameter("state_publish_rate")) { - auto p_state_publish_rate = rclcpp::ParameterValue(params_.state_publish_rate_); - parameters_interface->declare_parameter("state_publish_rate", p_state_publish_rate); - } else { - params_.state_publish_rate_ = parameters_interface->get_parameter("state_publish_rate").as_double(); + + std::unordered_map desc_map; + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which joints will be used by the controller"; + descriptor.read_only = true; + desc_map["joints"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which command interfaces to claim"; + descriptor.read_only = true; + desc_map["command_interfaces"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which state interfaces to claim"; + descriptor.read_only = true; + desc_map["state_interfaces"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which chainable interfaces to claim"; + descriptor.read_only = true; + desc_map["chainable_command_interfaces"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which kinematics plugin to load"; + descriptor.read_only = false; + desc_map["kinematics.plugin_name"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the package to load the kinematics plugin from"; + descriptor.read_only = false; + desc_map["kinematics.plugin_package"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the base link of the robot description used by the kinematics plugin"; + descriptor.read_only = false; + desc_map["kinematics.base"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the end effector link of the robot description used by the kinematics plugin"; + descriptor.read_only = false; + desc_map["kinematics.tip"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the damping coefficient for the Jacobian pseudo inverse"; + descriptor.read_only = false; + desc_map["kinematics.alpha"] = descriptor; + if (!parameters_interface->has_parameter("kinematics.alpha")) { + auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); + parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha, descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the group name for planning with Moveit"; + descriptor.read_only = false; + desc_map["kinematics.group_name"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "name of the force torque sensor in the robot description"; + descriptor.read_only = false; + desc_map["ft_sensor.name"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "frame of the force torque sensor"; + descriptor.read_only = false; + desc_map["ft_sensor.frame.id"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + desc_map["ft_sensor.frame.external"] = descriptor; + if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { + auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); + parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external, descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the coefficient for the sensor's exponential filter"; + descriptor.read_only = false; + desc_map["ft_sensor.filter_coefficient"] = descriptor; + if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { + auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); + parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient, + descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "control frame used for admittance control"; + descriptor.read_only = false; + desc_map["control.frame.id"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the control frame is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + desc_map["control.frame.external"] = descriptor; + if (!parameters_interface->has_parameter("control.frame.external")) { + auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); + parameters_interface->declare_parameter("control.frame.external", p_control_frame_external, descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "world frame, gravity points down (neg. Z) in this frame"; + descriptor.read_only = false; + desc_map["fixed_world_frame.frame.id"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the world frame is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + desc_map["fixed_world_frame.frame.external"] = descriptor; + if (!parameters_interface->has_parameter("fixed_world_frame.frame.external")) { + auto p_fixed_world_frame_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.frame_.external_); + parameters_interface->declare_parameter("fixed_world_frame.frame.external", + p_fixed_world_frame_frame_external, descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "frame which center of gravity (CoG) is defined in"; + descriptor.read_only = false; + desc_map["gravity_compensation.frame.id"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + desc_map["gravity_compensation.frame.external"] = descriptor; + if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { + auto p_gravity_compensation_frame_external = rclcpp::ParameterValue( + params_.gravity_compensation_.frame_.external_); + parameters_interface->declare_parameter("gravity_compensation.frame.external", + p_gravity_compensation_frame_external, descriptor); + } } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "position of the center of gravity (CoG) in its frame"; + descriptor.read_only = false; + desc_map["gravity_compensation.CoG.pos"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "weight of the end effector, e.g mass * 9.81"; + descriptor.read_only = false; + desc_map["gravity_compensation.CoG.force"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the axes x, y, z, rx, ry, and rz are enabled"; + descriptor.read_only = false; + desc_map["admittance.selected_axes"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0001; + range.to_value = 100000000.0; + descriptor.floating_point_range.push_back(range); + descriptor.read_only = false; + desc_map["admittance.mass"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.1; + range.to_value = 10.0; + descriptor.floating_point_range.push_back(range); + descriptor.read_only = false; + desc_map["admittance.damping_ratio"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation"; + descriptor.read_only = false; + desc_map["admittance.stiffness"] = descriptor; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "if enabled, configurable parameters will be dynamically updated in the control loop"; + descriptor.read_only = false; + desc_map["enable_parameter_update_without_reactivation"] = descriptor; + if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { + auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( + params_.enable_parameter_update_without_reactivation_); + parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", + p_enable_parameter_update_without_reactivation, descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity"; + descriptor.read_only = false; + desc_map["use_feedforward_commanded_input"] = descriptor; + if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { + auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); + parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input, + descriptor); + } + } + + + std::map overrides = parameters_interface->get_parameter_overrides(); + declare_overrides(parameters_interface, overrides, desc_map); + + params_.joints_ = parameters_interface->get_parameter("joints").as_string_array(); + params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); + params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); + params_.chainable_command_interfaces_ = parameters_interface->get_parameter( + "chainable_command_interfaces").as_string_array(); + params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); + params_.kinematics_.plugin_package_ = parameters_interface->get_parameter( + "kinematics.plugin_package").as_string(); + params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); + params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); + params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); + params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); + params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); + params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); + params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); + params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( + "ft_sensor.filter_coefficient").as_double(); + params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); + params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); + params_.fixed_world_frame_.frame_.id_ = parameters_interface->get_parameter( + "fixed_world_frame.frame.id").as_string(); + params_.fixed_world_frame_.frame_.external_ = parameters_interface->get_parameter( + "fixed_world_frame.frame.external").as_bool(); + params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( + "gravity_compensation.frame.id").as_string(); + params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( + "gravity_compensation.frame.external").as_bool(); + if (!validate_length(parameters_interface->get_parameter("gravity_compensation.CoG.pos").as_double_array(), 3)) { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter gravity_compensation.CoG.pos "); + } + params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( + "gravity_compensation.CoG.pos").as_double_array(); + params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( + "gravity_compensation.CoG.force").as_double(); + if (!validate_length(parameters_interface->get_parameter("admittance.selected_axes").as_bool_array(), 6)) { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.selected_axes "); + } + params_.admittance_.selected_axes_ = parameters_interface->get_parameter( + "admittance.selected_axes").as_bool_array(); + if (!validate_length(parameters_interface->get_parameter("admittance.mass").as_double_array(), 6) || + !validate_bounds(parameters_interface->get_parameter("admittance.mass").as_double_array(), 0.0001, + 100000000.0)) { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.mass "); + } + params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); + if (!validate_length(parameters_interface->get_parameter("admittance.damping_ratio").as_double_array(), 6) || + !validate_bounds(parameters_interface->get_parameter("admittance.damping_ratio").as_double_array(), 0.1, + 10.0)) { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.damping_ratio "); + } + params_.admittance_.damping_ratio_ = parameters_interface->get_parameter( + "admittance.damping_ratio").as_double_array(); + if (!validate_length(parameters_interface->get_parameter("admittance.stiffness").as_double_array(), 6)) { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.stiffness "); + } + params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); + params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( + "enable_parameter_update_without_reactivation").as_bool(); + params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter( + "use_feedforward_commanded_input").as_bool(); } }; diff --git a/admittance_controller/include/admittance_controller/config/definition.yaml b/admittance_controller/include/admittance_controller/config/definition.yaml new file mode 100644 index 0000000000..af04312478 --- /dev/null +++ b/admittance_controller/include/admittance_controller/config/definition.yaml @@ -0,0 +1,202 @@ +admittance_struct: + joints: { + default_value: ["UNDEFINED"], + description: "specifies which joints will be used by the controller", + configurable: false, + optional: false + } + command_interfaces: + { + default_value: ["UNDEFINED"], + description: "specifies which command interfaces to claim", + configurable: false, + optional: false + } + + state_interfaces: + { + default_value: ["UNDEFINED"], + description: "specifies which state interfaces to claim", + configurable: false, + optional: false + } + + chainable_command_interfaces: + { + default_value: ["UNDEFINED"], + description: "specifies which chainable interfaces to claim", + configurable: false, + optional: false + } + + kinematics: + plugin_name: { + default_value: "UNDEFINED", + description: "specifies which kinematics plugin to load", + configurable: true, # requires restart + optional: false + } + plugin_package: { + default_value: "UNDEFINED", + description: "specifies the package to load the kinematics plugin from", + configurable: true, # requires restart + optional: false + } + base: { + default_value: "UNDEFINED", + description: "specifies the base link of the robot description used by the kinematics plugin", + configurable: true, # requires restart + optional: false + } + tip: { + default_value: "UNDEFINED", + description: "specifies the end effector link of the robot description used by the kinematics plugin", + configurable: true, # requires restart + optional: false + } + alpha: { + default_value: 0.0005, + description: "specifies the damping coefficient for the Jacobian pseudo inverse", + configurable: true, # requires restart + optional: true + } + group_name: { + default_value: "UNDEFINED", + description: "specifies the group name for planning with Moveit", + configurable: true, # requires restart + optional: false + } + + ft_sensor: + name: { + default_value: "UNDEFINED", + description: "name of the force torque sensor in the robot description", + configurable: true, # requires restart + optional: false + } + frame: + id: { + default_value: "UNDEFINED", + description: "frame of the force torque sensor", + configurable: true, # requires restart + optional: false + } + external: { + default_value: No, + description: "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip", + configurable: true, # requires restart + optional: true + } + filter_coefficient: { + default_value: 0.005, + description: "specifies the coefficient for the sensor's exponential filter", + configurable: true, # requires restartF + optional: true + } + + control: + frame: + id: { + default_value: "UNDEFINED", + description: "control frame used for admittance control", + configurable: true, + optional: false + } + external: { + default_value: No, + description: "specifies if the control frame is contained in the kinematics chain from the base to the tip", + configurable: true, # requires restart + optional: true + } + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: { + default_value: "UNDEFINED", + description: "world frame, gravity points down (neg. Z) in this frame", + configurable: true, + optional: false + } + external: { + default_value: No, + description: "specifies if the world frame is contained in the kinematics chain from the base to the tip", + configurable: true, # requires restart + optional: true + } + + gravity_compensation: + frame: + id: { + default_value: "UNDEFINED", + description: "frame which center of gravity (CoG) is defined in", + configurable: true, + optional: false + } + external: { + default_value: No, + description: "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip", + configurable: true, # requires restart + optional: true + } + CoG: # specifies the center of gravity of the end effector + pos: { + default_value: [ .NAN, .NAN, .NAN ], + description: "position of the center of gravity (CoG) in its frame", + configurable: true, + optional: false + } + force: { + default_value: .NAN, + description: "weight of the end effector, e.g mass * 9.81", + configurable: true, + optional: false + } + + admittance: + selected_axes: + { + default_value: [ false, false, false, false, false, false ], + description: "specifies if the axes x, y, z, rx, ry, and rz are enabled", + configurable: true, + optional: false + } + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: { + default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN ], + description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation", + configurable: true, + optional: false, + bounds: [0.0001, 100000000.0] + } + + damping_ratio: { + default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN], + description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. + The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))", + configurable: true, + optional: false, + bounds: [0.1, 10] + } + + stiffness: { + default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN ], + description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", + configurable: true, + optional: false, + } + + # general settings + enable_parameter_update_without_reactivation: { + default_value: true, + description: "if enabled, configurable parameters will be dynamically updated in the control loop", + configurable: true, + optional: true + } + use_feedforward_commanded_input: { + default_value: true, + description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity", + configurable: true, + optional: true + } From 965c226e3fa0fea9a02dee60477c5c0d28125b62 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 6 Jul 2022 16:03:40 -0600 Subject: [PATCH 032/111] added parameter validation and descriptions --- .../admittance_controller.hpp | 10 - .../admittance_rule_impl.hpp | 1 + .../config/admittance_struct.h | 545 ++++++++++-------- .../config/definition.yaml | 171 +++--- .../src/admittance_controller.cpp | 175 +++--- .../test/test_admittance_controller.cpp | 22 +- admittance_controller/test/test_params.yaml | 94 ++- 7 files changed, 517 insertions(+), 501 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 3015f06e7f..5e22660017 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -55,14 +55,6 @@ namespace admittance_controller { std::unique_ptr> state_publisher_; }; - struct ParameterStruct { - std::vector joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - std::vector chainable_command_interface_types_; - }; - - class AdmittanceController : public controller_interface::ChainableControllerInterface { public: ADMITTANCE_CONTROLLER_PUBLIC @@ -121,8 +113,6 @@ namespace admittance_controller { std::unique_ptr admittance_; // joint limiter TODO std::unique_ptr force_torque_sensor_; - // controller config filled by ROS - ParameterStruct params; // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; rclcpp::Publisher::SharedPtr s_publisher_ = nullptr; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index abbf80c54a..5b76955a94 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -135,6 +135,7 @@ namespace admittance_controller { // update param values ee_weight.setZero(); ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; + vec_to_eigen(parameters_->gravity_compensation_.CoG_.pos_, cog_); vec_to_eigen(parameters_->admittance_.mass_, mass_); vec_to_eigen(parameters_->admittance_.stiffness_, stiffness_); vec_to_eigen(parameters_->admittance_.selected_axes_, selected_axes_); diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 8f918e6f25..2b62e1278e 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -1,8 +1,11 @@ // this is auto-generated code +#pragma once + #include #include #include +#include namespace admittance_struct_parameters { @@ -19,140 +22,60 @@ namespace admittance_struct_parameters { handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); } - void - declare_overrides(const std::shared_ptr ¶meters_interface, - const std::map &overrides, - std::unordered_map &desc_map) { - - for (auto &key: overrides) { - if (!parameters_interface->has_parameter(key.first)) { - parameters_interface->declare_parameter(key.first, key.second, desc_map[key.first]); - } - } - } - - template - bool validate_length(const std::vector &values, size_t len) { - return values.size() == len; - } - - template - bool validate_length(const std::string &name, const std::vector &values, size_t len, - rcl_interfaces::msg::SetParametersResult &result) { - if (!validate_length(values, len)) { - result.reason = std::string("Invalid size for vector parameter ") + name + - ". Expected " + std::to_string(len) + " got " + std::to_string(values.size()); - return false; - } - return true; - } - - - template - bool validate_bounds(std::vector values, const T &lower_bound, const T &upper_bound) { - for (const auto &val: values) { - if (!validate_bounds(val, lower_bound, upper_bound)) { - return false; - } - } - return true; - } - - template - bool validate_bounds(const std::string &name, std::vector values, const T &lower_bound, const T &upper_bound, - rcl_interfaces::msg::SetParametersResult &result) { - for (const auto &val: values) { - if (!validate_bounds(name, val, lower_bound, upper_bound, result)) { - return false; - } - } - return true; - } - - - template - bool validate_bounds(T value, const T &lower_bound, const T &upper_bound) { - if (value > upper_bound || value < lower_bound) { - return false; - } - return true; - } - - template - bool validate_bounds(const std::string &name, T value, const T &lower_bound, const T &upper_bound, - rcl_interfaces::msg::SetParametersResult &result) { - if (!validate_bounds(value, lower_bound, upper_bound)) { - result.reason = std::string("Invalid value for parameter ") + name + ". Value not within the required bounds."; - return false; - } - return true; - } - - struct params { - std::vector joints_ = {"UNDEFINED"}; - std::vector command_interfaces_ = {"UNDEFINED"}; - std::vector state_interfaces_ = {"UNDEFINED"}; - std::vector chainable_command_interfaces_ = {"UNDEFINED"}; + std::vector joints_; + std::vector command_interfaces_; + std::vector state_interfaces_; + std::vector chainable_command_interfaces_; struct kinematics { - std::string plugin_name_ = "UNDEFINED"; - std::string plugin_package_ = "UNDEFINED"; - std::string base_ = "UNDEFINED"; - std::string tip_ = "UNDEFINED"; + std::string plugin_name_; + std::string plugin_package_; + std::string base_; + std::string tip_; double alpha_ = 0.0005; - std::string group_name_ = "UNDEFINED"; + std::string group_name_; } kinematics_; struct ft_sensor { - std::string name_ = "UNDEFINED"; + std::string name_; struct frame { - std::string id_ = "UNDEFINED"; - bool external_ = false; + std::string id_; + bool external_; } frame_; double filter_coefficient_ = 0.005; } ft_sensor_; struct control { struct frame { - std::string id_ = "UNDEFINED"; - bool external_ = false; + std::string id_; + bool external_; } frame_; } control_; struct fixed_world_frame { struct frame { - std::string id_ = "UNDEFINED"; - bool external_ = false; + std::string id_; + bool external_; } frame_; } fixed_world_frame_; struct gravity_compensation { struct frame { - std::string id_ = "UNDEFINED"; - bool external_ = false; + std::string id_; + bool external_; } frame_; struct CoG { - std::vector pos_ = {std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; + std::vector pos_; double force_ = std::numeric_limits::quiet_NaN(); } CoG_; } gravity_compensation_; struct admittance { - std::vector selected_axes_ = {false, false, false, false, false, false}; - std::vector mass_ = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; - std::vector damping_ratio_ = {std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; + std::vector selected_axes_; + std::vector mass_; + std::vector damping_ratio_; std::vector stiffness_ = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; + std::numeric_limits::quiet_NaN(),}; } admittance_; + std::string robot_description_; bool enable_parameter_update_without_reactivation_ = true; bool use_feedforward_commanded_input_ = true; @@ -160,102 +83,162 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + gen_param_struct_validators::Result validation_result; result.reason = "success"; for (const auto ¶m: parameters) { - if (param.get_name() == "joints" && validate_length("joints", param.as_string_array(), 1, result)) { + if (param.get_name() == "joints") { params_.joints_ = param.as_string_array(); + result.successful = true; } - if (param.get_name() == "command_interfaces" && - validate_length("command_interfaces", param.as_string_array(), 1, result)) { + if (param.get_name() == "command_interfaces") { params_.command_interfaces_ = param.as_string_array(); + result.successful = true; } - if (param.get_name() == "state_interfaces" && - validate_length("state_interfaces", param.as_string_array(), 1, result)) { + if (param.get_name() == "state_interfaces") { params_.state_interfaces_ = param.as_string_array(); + result.successful = true; } - if (param.get_name() == "chainable_command_interfaces" && - validate_length("chainable_command_interfaces", param.as_string_array(), 1, result)) { + if (param.get_name() == "chainable_command_interfaces") { params_.chainable_command_interfaces_ = param.as_string_array(); + result.successful = true; } if (param.get_name() == "kinematics.plugin_name") { params_.kinematics_.plugin_name_ = param.as_string(); + result.successful = true; } if (param.get_name() == "kinematics.plugin_package") { params_.kinematics_.plugin_package_ = param.as_string(); + result.successful = true; } if (param.get_name() == "kinematics.base") { params_.kinematics_.base_ = param.as_string(); + result.successful = true; } if (param.get_name() == "kinematics.tip") { params_.kinematics_.tip_ = param.as_string(); + result.successful = true; } if (param.get_name() == "kinematics.alpha") { params_.kinematics_.alpha_ = param.as_double(); + result.successful = true; } if (param.get_name() == "kinematics.group_name") { params_.kinematics_.group_name_ = param.as_string(); + result.successful = true; } if (param.get_name() == "ft_sensor.name") { params_.ft_sensor_.name_ = param.as_string(); + result.successful = true; } if (param.get_name() == "ft_sensor.frame.id") { params_.ft_sensor_.frame_.id_ = param.as_string(); + result.successful = true; } if (param.get_name() == "ft_sensor.frame.external") { params_.ft_sensor_.frame_.external_ = param.as_bool(); + result.successful = true; } if (param.get_name() == "ft_sensor.filter_coefficient") { params_.ft_sensor_.filter_coefficient_ = param.as_double(); + result.successful = true; } if (param.get_name() == "control.frame.id") { params_.control_.frame_.id_ = param.as_string(); + result.successful = true; } if (param.get_name() == "control.frame.external") { params_.control_.frame_.external_ = param.as_bool(); + result.successful = true; } if (param.get_name() == "fixed_world_frame.frame.id") { params_.fixed_world_frame_.frame_.id_ = param.as_string(); + result.successful = true; } if (param.get_name() == "fixed_world_frame.frame.external") { params_.fixed_world_frame_.frame_.external_ = param.as_bool(); + result.successful = true; } if (param.get_name() == "gravity_compensation.frame.id") { params_.gravity_compensation_.frame_.id_ = param.as_string(); + result.successful = true; } if (param.get_name() == "gravity_compensation.frame.external") { params_.gravity_compensation_.frame_.external_ = param.as_bool(); - } - if (param.get_name() == "gravity_compensation.CoG.pos" && - validate_length("gravity_compensation.CoG.pos", param.as_double_array(), 3, result)) { - params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); + result.successful = true; + } + if (param.get_name() == "gravity_compensation.CoG.pos") { + validation_result = gen_param_struct_validators::validate_double_array_len(param, 3); + if (validation_result.success()) { + params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); + result.successful = true; + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } } if (param.get_name() == "gravity_compensation.CoG.force") { params_.gravity_compensation_.CoG_.force_ = param.as_double(); - } - if (param.get_name() == "admittance.selected_axes" && - validate_length("admittance.selected_axes", param.as_bool_array(), 6, result)) { - params_.admittance_.selected_axes_ = param.as_bool_array(); - } - if (param.get_name() == "admittance.mass" && - validate_length("admittance.mass", param.as_double_array(), 6, result) && - validate_bounds("admittance.mass", param.as_double_array(), 0.0001, 100000000.0, result)) { - params_.admittance_.mass_ = param.as_double_array(); - } - if (param.get_name() == "admittance.damping_ratio" && - validate_length("admittance.damping_ratio", param.as_double_array(), 6, result) && - validate_bounds("admittance.damping_ratio", param.as_double_array(), 0.1, 10.0, result)) { - params_.admittance_.damping_ratio_ = param.as_double_array(); - } - if (param.get_name() == "admittance.stiffness" && - validate_length("admittance.stiffness", param.as_double_array(), 6, result)) { + result.successful = true; + } + if (param.get_name() == "admittance.selected_axes") { + validation_result = gen_param_struct_validators::validate_bool_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.selected_axes_ = param.as_bool_array(); + result.successful = true; + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } + } + if (param.get_name() == "admittance.mass") { + validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0001, 1000000.0); + if (validation_result.success()) { + validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.mass_ = param.as_double_array(); + result.successful = true; + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } + } + if (param.get_name() == "admittance.damping_ratio") { + validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.1, 10.0); + if (validation_result.success()) { + validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.damping_ratio_ = param.as_double_array(); + result.successful = true; + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } + } + if (param.get_name() == "admittance.stiffness") { params_.admittance_.stiffness_ = param.as_double_array(); + result.successful = true; + } + if (param.get_name() == "robot_description") { + params_.robot_description_ = param.as_string(); + result.successful = true; } if (param.get_name() == "enable_parameter_update_without_reactivation") { params_.enable_parameter_update_without_reactivation_ = param.as_bool(); + result.successful = true; } if (param.get_name() == "use_feedforward_commanded_input") { params_.use_feedforward_commanded_input_ = param.as_bool(); + result.successful = true; } } @@ -263,62 +246,85 @@ namespace admittance_struct_parameters { } void declare_params(const std::shared_ptr ¶meters_interface) { - - std::unordered_map desc_map; + // declare all parameters and give default values to non-required ones { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies which joints will be used by the controller"; descriptor.read_only = true; - desc_map["joints"] = descriptor; + if (!parameters_interface->has_parameter("joints")) { + auto p_joints = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface->declare_parameter("joints", p_joints, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies which command interfaces to claim"; descriptor.read_only = true; - desc_map["command_interfaces"] = descriptor; + if (!parameters_interface->has_parameter("command_interfaces")) { + auto p_command_interfaces = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface->declare_parameter("command_interfaces", p_command_interfaces, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies which state interfaces to claim"; descriptor.read_only = true; - desc_map["state_interfaces"] = descriptor; + if (!parameters_interface->has_parameter("state_interfaces")) { + auto p_state_interfaces = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface->declare_parameter("state_interfaces", p_state_interfaces, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies which chainable interfaces to claim"; descriptor.read_only = true; - desc_map["chainable_command_interfaces"] = descriptor; + if (!parameters_interface->has_parameter("chainable_command_interfaces")) { + auto p_chainable_command_interfaces = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces, + descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies which kinematics plugin to load"; descriptor.read_only = false; - desc_map["kinematics.plugin_name"] = descriptor; + if (!parameters_interface->has_parameter("kinematics.plugin_name")) { + auto p_kinematics_plugin_name = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies the package to load the kinematics plugin from"; descriptor.read_only = false; - desc_map["kinematics.plugin_package"] = descriptor; + if (!parameters_interface->has_parameter("kinematics.plugin_package")) { + auto p_kinematics_plugin_package = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("kinematics.plugin_package", p_kinematics_plugin_package, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies the base link of the robot description used by the kinematics plugin"; descriptor.read_only = false; - desc_map["kinematics.base"] = descriptor; + if (!parameters_interface->has_parameter("kinematics.base")) { + auto p_kinematics_base = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("kinematics.base", p_kinematics_base, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies the end effector link of the robot description used by the kinematics plugin"; descriptor.read_only = false; - desc_map["kinematics.tip"] = descriptor; + if (!parameters_interface->has_parameter("kinematics.tip")) { + auto p_kinematics_tip = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies the damping coefficient for the Jacobian pseudo inverse"; descriptor.read_only = false; - desc_map["kinematics.alpha"] = descriptor; if (!parameters_interface->has_parameter("kinematics.alpha")) { auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha, descriptor); @@ -328,27 +334,35 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies the group name for planning with Moveit"; descriptor.read_only = false; - desc_map["kinematics.group_name"] = descriptor; + if (!parameters_interface->has_parameter("kinematics.group_name")) { + auto p_kinematics_group_name = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "name of the force torque sensor in the robot description"; descriptor.read_only = false; - desc_map["ft_sensor.name"] = descriptor; + if (!parameters_interface->has_parameter("ft_sensor.name")) { + auto p_ft_sensor_name = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "frame of the force torque sensor"; descriptor.read_only = false; - desc_map["ft_sensor.frame.id"] = descriptor; + if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { + auto p_ft_sensor_frame_id = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip"; descriptor.read_only = false; - desc_map["ft_sensor.frame.external"] = descriptor; if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { - auto p_ft_sensor_frame_external = rclcpp::ParameterValue(params_.ft_sensor_.frame_.external_); + auto p_ft_sensor_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external, descriptor); } } @@ -356,7 +370,6 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies the coefficient for the sensor's exponential filter"; descriptor.read_only = false; - desc_map["ft_sensor.filter_coefficient"] = descriptor; if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient, @@ -367,15 +380,17 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "control frame used for admittance control"; descriptor.read_only = false; - desc_map["control.frame.id"] = descriptor; + if (!parameters_interface->has_parameter("control.frame.id")) { + auto p_control_frame_id = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("control.frame.id", p_control_frame_id, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies if the control frame is contained in the kinematics chain from the base to the tip"; descriptor.read_only = false; - desc_map["control.frame.external"] = descriptor; if (!parameters_interface->has_parameter("control.frame.external")) { - auto p_control_frame_external = rclcpp::ParameterValue(params_.control_.frame_.external_); + auto p_control_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; parameters_interface->declare_parameter("control.frame.external", p_control_frame_external, descriptor); } } @@ -383,15 +398,18 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "world frame, gravity points down (neg. Z) in this frame"; descriptor.read_only = false; - desc_map["fixed_world_frame.frame.id"] = descriptor; + if (!parameters_interface->has_parameter("fixed_world_frame.frame.id")) { + auto p_fixed_world_frame_frame_id = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("fixed_world_frame.frame.id", p_fixed_world_frame_frame_id, + descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies if the world frame is contained in the kinematics chain from the base to the tip"; descriptor.read_only = false; - desc_map["fixed_world_frame.frame.external"] = descriptor; if (!parameters_interface->has_parameter("fixed_world_frame.frame.external")) { - auto p_fixed_world_frame_frame_external = rclcpp::ParameterValue(params_.fixed_world_frame_.frame_.external_); + auto p_fixed_world_frame_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; parameters_interface->declare_parameter("fixed_world_frame.frame.external", p_fixed_world_frame_frame_external, descriptor); } @@ -400,16 +418,18 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "frame which center of gravity (CoG) is defined in"; descriptor.read_only = false; - desc_map["gravity_compensation.frame.id"] = descriptor; + if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { + auto p_gravity_compensation_frame_id = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id, + descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip"; descriptor.read_only = false; - desc_map["gravity_compensation.frame.external"] = descriptor; if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { - auto p_gravity_compensation_frame_external = rclcpp::ParameterValue( - params_.gravity_compensation_.frame_.external_); + auto p_gravity_compensation_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; parameters_interface->declare_parameter("gravity_compensation.frame.external", p_gravity_compensation_frame_external, descriptor); } @@ -418,29 +438,43 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "position of the center of gravity (CoG) in its frame"; descriptor.read_only = false; - desc_map["gravity_compensation.CoG.pos"] = descriptor; + if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { + auto p_gravity_compensation_CoG_pos = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos, + descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "weight of the end effector, e.g mass * 9.81"; descriptor.read_only = false; - desc_map["gravity_compensation.CoG.force"] = descriptor; + if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { + auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); + parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force, + descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies if the axes x, y, z, rx, ry, and rz are enabled"; descriptor.read_only = false; - desc_map["admittance.selected_axes"] = descriptor; + if (!parameters_interface->has_parameter("admittance.selected_axes")) { + auto p_admittance_selected_axes = rclcpp::ParameterType::PARAMETER_BOOL_ARRAY; + parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation"; rcl_interfaces::msg::FloatingPointRange range; range.from_value = 0.0001; - range.to_value = 100000000.0; + range.to_value = 1000000.0; descriptor.floating_point_range.push_back(range); descriptor.read_only = false; - desc_map["admittance.mass"] = descriptor; + if (!parameters_interface->has_parameter("admittance.mass")) { + auto p_admittance_mass = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface->declare_parameter("admittance.mass", p_admittance_mass, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -450,19 +484,33 @@ namespace admittance_struct_parameters { range.to_value = 10.0; descriptor.floating_point_range.push_back(range); descriptor.read_only = false; - desc_map["admittance.damping_ratio"] = descriptor; + if (!parameters_interface->has_parameter("admittance.damping_ratio")) { + auto p_admittance_damping_ratio = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation"; descriptor.read_only = false; - desc_map["admittance.stiffness"] = descriptor; + if (!parameters_interface->has_parameter("admittance.stiffness")) { + auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); + parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness, descriptor); + } + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics."; + descriptor.read_only = true; + if (!parameters_interface->has_parameter("robot_description")) { + auto p_robot_description = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface->declare_parameter("robot_description", p_robot_description, descriptor); + } } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "if enabled, configurable parameters will be dynamically updated in the control loop"; + descriptor.description = "if enabled, parameters will be dynamically updated in the control loop"; descriptor.read_only = false; - desc_map["enable_parameter_update_without_reactivation"] = descriptor; if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( params_.enable_parameter_update_without_reactivation_); @@ -474,7 +522,6 @@ namespace admittance_struct_parameters { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity"; descriptor.read_only = false; - desc_map["use_feedforward_commanded_input"] = descriptor; if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input, @@ -482,75 +529,109 @@ namespace admittance_struct_parameters { } } + // get parameters and fill struct fields + rclcpp::Parameter param; + gen_param_struct_validators::Result validation_result; - std::map overrides = parameters_interface->get_parameter_overrides(); - declare_overrides(parameters_interface, overrides, desc_map); - - params_.joints_ = parameters_interface->get_parameter("joints").as_string_array(); - params_.command_interfaces_ = parameters_interface->get_parameter("command_interfaces").as_string_array(); - params_.state_interfaces_ = parameters_interface->get_parameter("state_interfaces").as_string_array(); - params_.chainable_command_interfaces_ = parameters_interface->get_parameter( - "chainable_command_interfaces").as_string_array(); - params_.kinematics_.plugin_name_ = parameters_interface->get_parameter("kinematics.plugin_name").as_string(); - params_.kinematics_.plugin_package_ = parameters_interface->get_parameter( - "kinematics.plugin_package").as_string(); - params_.kinematics_.base_ = parameters_interface->get_parameter("kinematics.base").as_string(); - params_.kinematics_.tip_ = parameters_interface->get_parameter("kinematics.tip").as_string(); - params_.kinematics_.alpha_ = parameters_interface->get_parameter("kinematics.alpha").as_double(); - params_.kinematics_.group_name_ = parameters_interface->get_parameter("kinematics.group_name").as_string(); - params_.ft_sensor_.name_ = parameters_interface->get_parameter("ft_sensor.name").as_string(); - params_.ft_sensor_.frame_.id_ = parameters_interface->get_parameter("ft_sensor.frame.id").as_string(); - params_.ft_sensor_.frame_.external_ = parameters_interface->get_parameter("ft_sensor.frame.external").as_bool(); - params_.ft_sensor_.filter_coefficient_ = parameters_interface->get_parameter( - "ft_sensor.filter_coefficient").as_double(); - params_.control_.frame_.id_ = parameters_interface->get_parameter("control.frame.id").as_string(); - params_.control_.frame_.external_ = parameters_interface->get_parameter("control.frame.external").as_bool(); - params_.fixed_world_frame_.frame_.id_ = parameters_interface->get_parameter( - "fixed_world_frame.frame.id").as_string(); - params_.fixed_world_frame_.frame_.external_ = parameters_interface->get_parameter( - "fixed_world_frame.frame.external").as_bool(); - params_.gravity_compensation_.frame_.id_ = parameters_interface->get_parameter( - "gravity_compensation.frame.id").as_string(); - params_.gravity_compensation_.frame_.external_ = parameters_interface->get_parameter( - "gravity_compensation.frame.external").as_bool(); - if (!validate_length(parameters_interface->get_parameter("gravity_compensation.CoG.pos").as_double_array(), 3)) { + param = parameters_interface->get_parameter("joints"); + params_.joints_ = param.as_string_array(); + param = parameters_interface->get_parameter("command_interfaces"); + params_.command_interfaces_ = param.as_string_array(); + param = parameters_interface->get_parameter("state_interfaces"); + params_.state_interfaces_ = param.as_string_array(); + param = parameters_interface->get_parameter("chainable_command_interfaces"); + params_.chainable_command_interfaces_ = param.as_string_array(); + param = parameters_interface->get_parameter("kinematics.plugin_name"); + params_.kinematics_.plugin_name_ = param.as_string(); + param = parameters_interface->get_parameter("kinematics.plugin_package"); + params_.kinematics_.plugin_package_ = param.as_string(); + param = parameters_interface->get_parameter("kinematics.base"); + params_.kinematics_.base_ = param.as_string(); + param = parameters_interface->get_parameter("kinematics.tip"); + params_.kinematics_.tip_ = param.as_string(); + param = parameters_interface->get_parameter("kinematics.alpha"); + params_.kinematics_.alpha_ = param.as_double(); + param = parameters_interface->get_parameter("kinematics.group_name"); + params_.kinematics_.group_name_ = param.as_string(); + param = parameters_interface->get_parameter("ft_sensor.name"); + params_.ft_sensor_.name_ = param.as_string(); + param = parameters_interface->get_parameter("ft_sensor.frame.id"); + params_.ft_sensor_.frame_.id_ = param.as_string(); + param = parameters_interface->get_parameter("ft_sensor.frame.external"); + params_.ft_sensor_.frame_.external_ = param.as_bool(); + param = parameters_interface->get_parameter("ft_sensor.filter_coefficient"); + params_.ft_sensor_.filter_coefficient_ = param.as_double(); + param = parameters_interface->get_parameter("control.frame.id"); + params_.control_.frame_.id_ = param.as_string(); + param = parameters_interface->get_parameter("control.frame.external"); + params_.control_.frame_.external_ = param.as_bool(); + param = parameters_interface->get_parameter("fixed_world_frame.frame.id"); + params_.fixed_world_frame_.frame_.id_ = param.as_string(); + param = parameters_interface->get_parameter("fixed_world_frame.frame.external"); + params_.fixed_world_frame_.frame_.external_ = param.as_bool(); + param = parameters_interface->get_parameter("gravity_compensation.frame.id"); + params_.gravity_compensation_.frame_.id_ = param.as_string(); + param = parameters_interface->get_parameter("gravity_compensation.frame.external"); + params_.gravity_compensation_.frame_.external_ = param.as_bool(); + param = parameters_interface->get_parameter("gravity_compensation.CoG.pos"); + validation_result = gen_param_struct_validators::validate_double_array_len(param, 3); + if (validation_result.success()) { + params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); + } else { throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter gravity_compensation.CoG.pos "); - } - params_.gravity_compensation_.CoG_.pos_ = parameters_interface->get_parameter( - "gravity_compensation.CoG.pos").as_double_array(); - params_.gravity_compensation_.CoG_.force_ = parameters_interface->get_parameter( - "gravity_compensation.CoG.force").as_double(); - if (!validate_length(parameters_interface->get_parameter("admittance.selected_axes").as_bool_array(), 6)) { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.selected_axes "); - } - params_.admittance_.selected_axes_ = parameters_interface->get_parameter( - "admittance.selected_axes").as_bool_array(); - if (!validate_length(parameters_interface->get_parameter("admittance.mass").as_double_array(), 6) || - !validate_bounds(parameters_interface->get_parameter("admittance.mass").as_double_array(), 0.0001, - 100000000.0)) { + "Invalid value set during initialization for parameter gravity_compensation.CoG.pos: " + + validation_result.error_msg()); + } + param = parameters_interface->get_parameter("gravity_compensation.CoG.force"); + params_.gravity_compensation_.CoG_.force_ = param.as_double(); + param = parameters_interface->get_parameter("admittance.selected_axes"); + validation_result = gen_param_struct_validators::validate_bool_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.selected_axes_ = param.as_bool_array(); + } else { throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.mass "); - } - params_.admittance_.mass_ = parameters_interface->get_parameter("admittance.mass").as_double_array(); - if (!validate_length(parameters_interface->get_parameter("admittance.damping_ratio").as_double_array(), 6) || - !validate_bounds(parameters_interface->get_parameter("admittance.damping_ratio").as_double_array(), 0.1, - 10.0)) { + "Invalid value set during initialization for parameter admittance.selected_axes: " + + validation_result.error_msg()); + } + param = parameters_interface->get_parameter("admittance.mass"); + validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0001, 1000000.0); + if (validation_result.success()) { + validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.mass_ = param.as_double_array(); + } else { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.mass: " + + validation_result.error_msg()); + } + } else { throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.damping_ratio "); + "Invalid value set during initialization for parameter admittance.mass: " + validation_result.error_msg()); } - params_.admittance_.damping_ratio_ = parameters_interface->get_parameter( - "admittance.damping_ratio").as_double_array(); - if (!validate_length(parameters_interface->get_parameter("admittance.stiffness").as_double_array(), 6)) { + param = parameters_interface->get_parameter("admittance.damping_ratio"); + validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.1, 10.0); + if (validation_result.success()) { + validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.damping_ratio_ = param.as_double_array(); + } else { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.damping_ratio: " + + validation_result.error_msg()); + } + } else { throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.stiffness "); - } - params_.admittance_.stiffness_ = parameters_interface->get_parameter("admittance.stiffness").as_double_array(); - params_.enable_parameter_update_without_reactivation_ = parameters_interface->get_parameter( - "enable_parameter_update_without_reactivation").as_bool(); - params_.use_feedforward_commanded_input_ = parameters_interface->get_parameter( - "use_feedforward_commanded_input").as_bool(); + "Invalid value set during initialization for parameter admittance.damping_ratio: " + + validation_result.error_msg()); + } + param = parameters_interface->get_parameter("admittance.stiffness"); + params_.admittance_.stiffness_ = param.as_double_array(); + param = parameters_interface->get_parameter("robot_description"); + params_.robot_description_ = param.as_string(); + param = parameters_interface->get_parameter("enable_parameter_update_without_reactivation"); + params_.enable_parameter_update_without_reactivation_ = param.as_bool(); + param = parameters_interface->get_parameter("use_feedforward_commanded_input"); + params_.use_feedforward_commanded_input_ = param.as_bool(); } }; diff --git a/admittance_controller/include/admittance_controller/config/definition.yaml b/admittance_controller/include/admittance_controller/config/definition.yaml index af04312478..974c003595 100644 --- a/admittance_controller/include/admittance_controller/config/definition.yaml +++ b/admittance_controller/include/admittance_controller/config/definition.yaml @@ -1,202 +1,169 @@ admittance_struct: joints: { - default_value: ["UNDEFINED"], + type: string_array, description: "specifies which joints will be used by the controller", - configurable: false, - optional: false + read_only: true } command_interfaces: { - default_value: ["UNDEFINED"], + type: string_array, description: "specifies which command interfaces to claim", - configurable: false, - optional: false + read_only: true } state_interfaces: { - default_value: ["UNDEFINED"], + type: string_array, description: "specifies which state interfaces to claim", - configurable: false, - optional: false + read_only: true } chainable_command_interfaces: { - default_value: ["UNDEFINED"], + type: string_array, description: "specifies which chainable interfaces to claim", - configurable: false, - optional: false + read_only: true } kinematics: plugin_name: { - default_value: "UNDEFINED", - description: "specifies which kinematics plugin to load", - configurable: true, # requires restart - optional: false + type: string, + description: "specifies which kinematics plugin to load" } plugin_package: { - default_value: "UNDEFINED", - description: "specifies the package to load the kinematics plugin from", - configurable: true, # requires restart - optional: false + type: string, + description: "specifies the package to load the kinematics plugin from" } base: { - default_value: "UNDEFINED", - description: "specifies the base link of the robot description used by the kinematics plugin", - configurable: true, # requires restart - optional: false + type: string, + description: "specifies the base link of the robot description used by the kinematics plugin" } tip: { - default_value: "UNDEFINED", - description: "specifies the end effector link of the robot description used by the kinematics plugin", - configurable: true, # requires restart - optional: false + type: string, + description: "specifies the end effector link of the robot description used by the kinematics plugin" } alpha: { + type: double, default_value: 0.0005, - description: "specifies the damping coefficient for the Jacobian pseudo inverse", - configurable: true, # requires restart - optional: true + description: "specifies the damping coefficient for the Jacobian pseudo inverse" } group_name: { - default_value: "UNDEFINED", - description: "specifies the group name for planning with Moveit", - configurable: true, # requires restart - optional: false + type: string, + description: "specifies the group name for planning with Moveit" } ft_sensor: name: { - default_value: "UNDEFINED", - description: "name of the force torque sensor in the robot description", - configurable: true, # requires restart - optional: false + type: string, + description: "name of the force torque sensor in the robot description" } frame: id: { - default_value: "UNDEFINED", - description: "frame of the force torque sensor", - configurable: true, # requires restart - optional: false + type: string, + description: "frame of the force torque sensor" } external: { - default_value: No, - description: "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip", - configurable: true, # requires restart - optional: true + type: bool, + default_value: false, + description: "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip" } filter_coefficient: { + type: double, default_value: 0.005, - description: "specifies the coefficient for the sensor's exponential filter", - configurable: true, # requires restartF - optional: true + description: "specifies the coefficient for the sensor's exponential filter" } control: frame: id: { - default_value: "UNDEFINED", - description: "control frame used for admittance control", - configurable: true, - optional: false + type: string, + description: "control frame used for admittance control" } external: { - default_value: No, - description: "specifies if the control frame is contained in the kinematics chain from the base to the tip", - configurable: true, # requires restart - optional: true + type: bool, + default_value: false, + description: "specifies if the control frame is contained in the kinematics chain from the base to the tip" } fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) frame: id: { - default_value: "UNDEFINED", - description: "world frame, gravity points down (neg. Z) in this frame", - configurable: true, - optional: false + type: string, + description: "world frame, gravity points down (neg. Z) in this frame" } external: { - default_value: No, - description: "specifies if the world frame is contained in the kinematics chain from the base to the tip", - configurable: true, # requires restart - optional: true + type: bool, + default_value: false, + description: "specifies if the world frame is contained in the kinematics chain from the base to the tip" } gravity_compensation: frame: id: { - default_value: "UNDEFINED", - description: "frame which center of gravity (CoG) is defined in", - configurable: true, - optional: false + type: string, + description: "frame which center of gravity (CoG) is defined in" } external: { - default_value: No, - description: "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip", - configurable: true, # requires restart - optional: true + type: bool, + default_value: false, + description: "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip" } CoG: # specifies the center of gravity of the end effector pos: { - default_value: [ .NAN, .NAN, .NAN ], - description: "position of the center of gravity (CoG) in its frame", - configurable: true, - optional: false + type: double_array, + fixed_size: 3, + description: "position of the center of gravity (CoG) in its frame" } force: { + type: double, default_value: .NAN, - description: "weight of the end effector, e.g mass * 9.81", - configurable: true, - optional: false + description: "weight of the end effector, e.g mass * 9.81" } admittance: selected_axes: { - default_value: [ false, false, false, false, false, false ], - description: "specifies if the axes x, y, z, rx, ry, and rz are enabled", - configurable: true, - optional: false + type: bool_array, + fixed_size: 6, + description: "specifies if the axes x, y, z, rx, ry, and rz are enabled" } # Having ".0" at the end is MUST, otherwise there is a loading error # F = M*a + D*v + S*(x - x_d) mass: { - default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN ], + type: double_array, + fixed_size: 6, description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation", - configurable: true, - optional: false, - bounds: [0.0001, 100000000.0] + bounds: [ 0.0001, 1000000.0] } damping_ratio: { - default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN], + type: double_array, + fixed_size: 6, description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))", - configurable: true, - optional: false, - bounds: [0.1, 10] + bounds: [ 0.1, 10.0 ] } stiffness: { + type: double_array, default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN ], description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", - configurable: true, - optional: false, } # general settings + robot_description: { + type: string, + description: "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics.", + read_only: true + } enable_parameter_update_without_reactivation: { + type: bool, default_value: true, - description: "if enabled, configurable parameters will be dynamically updated in the control loop", - configurable: true, - optional: true + description: "if enabled, parameters will be dynamically updated in the control loop" } use_feedforward_commanded_input: { + type: bool, default_value: true, - description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity", - configurable: true, - optional: true - } + description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity" + } \ No newline at end of file diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6a39754a49..281089cc24 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -30,7 +30,7 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" constexpr size_t -ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops + ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops namespace admittance_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -39,7 +39,7 @@ namespace admittance_controller { AdmittanceController::AdmittanceController() = default; void - AdmittanceController::joint_command_callback(const std::shared_ptr msg) { + AdmittanceController::joint_command_callback(const std::shared_ptr msg) { if (controller_is_active_) { rtBuffers.input_joint_command_.writeFromNonRT(msg); } @@ -49,44 +49,34 @@ namespace admittance_controller { // initialize controller config admittance_ = std::make_unique(); -// try { -// params = { -// get_node()->get_parameter("joints").as_string_array(), -// get_node()->get_parameter("command_interfaces").as_string_array(), -// get_node()->get_parameter("state_interfaces").as_string_array(), -// get_node()->get_parameter("chainable_command_interfaces").as_string_array(), -// }; -// } catch (const std::exception &e) { -// RCLCPP_ERROR(get_node()->get_logger(), -// "Exception thrown during init stage while reading parameters with message: %s \n", e.what()); -// return CallbackReturn::ERROR; -// } -// -// for (const auto &tmp: params.state_interface_types_) { -// RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); -// } -// for (const auto &tmp: params.command_interface_types_) { -// RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); -// } -// for (const auto &tmp: params.chainable_command_interface_types_) { -// if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { -// RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); -// } else { -// RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", -// tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); -// return CallbackReturn::ERROR; -// } -// } -// -// try { -// admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); -// admittance_->parameters_ = &admittance_->parameter_handler_->params_; -// } catch (const std::exception &e) { -// RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); -// return CallbackReturn::ERROR; -// } -// -// num_joints_ = params.joint_names_.size(); + + try { + admittance_->parameter_handler_ = std::make_shared( + get_node()->get_node_parameters_interface()); + admittance_->parameters_ = &admittance_->parameter_handler_->params_; + } catch (const std::exception &e) { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + num_joints_ = admittance_->parameters_->joints_.size(); + + for (const auto &tmp: admittance_->parameters_->state_interfaces_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto &tmp: admittance_->parameters_->command_interfaces_) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + for (const auto &tmp: admittance_->parameters_->chainable_command_interfaces_) { + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } else { + RCLCPP_ERROR(get_node()->get_logger(), + "chainable interface type %s is not supported. Supported types are %s and %s", + tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } + } return CallbackReturn::SUCCESS; } @@ -97,10 +87,10 @@ namespace admittance_controller { // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported - std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); + std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); - for (const auto &interface: params.state_interface_types_) { - for (const auto &joint: params.joint_names_) { + for (const auto &interface: admittance_->parameters_->state_interfaces_) { + for (const auto &joint: admittance_->parameters_->joints_) { state_interfaces_config_names.push_back(joint + "/" + interface); } } @@ -118,10 +108,10 @@ namespace admittance_controller { // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported - std::vector command_interfaces_config_names; + std::vector command_interfaces_config_names; - for (const auto &interface: params.command_interface_types_) { - for (const auto &joint: params.joint_names_) { + for (const auto &interface: admittance_->parameters_->command_interfaces_) { + for (const auto &joint: admittance_->parameters_->joints_) { command_interfaces_config_names.push_back(joint + "/" + interface); } } @@ -130,23 +120,24 @@ namespace admittance_controller { command_interfaces_config_names}; } - std::vector AdmittanceController::on_export_reference_interfaces() { + std::vector AdmittanceController::on_export_reference_interfaces() { // create CommandInterface interfaces that other controllers will be able to chain to - std::vector chainable_command_interfaces; - auto num_chainable_interfaces = params.chainable_command_interface_types_.size() * params.joint_names_.size(); + std::vector chainable_command_interfaces; + auto num_chainable_interfaces = + admittance_->parameters_->chainable_command_interfaces_.size() * admittance_->parameters_->joints_.size(); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); chainable_command_interfaces.reserve(num_chainable_interfaces); position_reference_.clear(); velocity_reference_.clear(); - std::unordered_map < std::string, std::vector < double * > * > chainable_interface_map = { + std::unordered_map *> chainable_interface_map = { {hardware_interface::HW_IF_POSITION, &position_reference_}, {hardware_interface::HW_IF_VELOCITY, &velocity_reference_}}; auto index = 0ul; - for (const auto &interface: params.chainable_command_interface_types_) { - for (const auto &joint: params.joint_names_) { + for (const auto &interface: admittance_->parameters_->chainable_command_interfaces_) { + for (const auto &joint: admittance_->parameters_->joints_) { chainable_interface_map[interface]->push_back(reference_interfaces_.data() + index); chainable_command_interfaces.emplace_back( hardware_interface::CommandInterface(std::string(get_node()->get_name()), @@ -187,46 +178,35 @@ namespace admittance_controller { // Print output so users can be sure the interface setup is correct try { - params = { - get_node()->get_parameter("joints").as_string_array(), - get_node()->get_parameter("command_interfaces").as_string_array(), - get_node()->get_parameter("state_interfaces").as_string_array(), - get_node()->get_parameter("chainable_command_interfaces").as_string_array(), - }; + admittance_->parameter_handler_ = std::make_shared( + get_node()->get_node_parameters_interface()); + admittance_->parameters_ = &admittance_->parameter_handler_->params_; } catch (const std::exception &e) { - RCLCPP_ERROR(get_node()->get_logger(), - "Exception thrown during init stage while reading parameters with message: %s \n", e.what()); + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } - for (const auto &tmp: params.state_interface_types_) { + num_joints_ = admittance_->parameters_->joints_.size(); + + for (const auto &tmp: admittance_->parameters_->state_interfaces_) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); } - for (const auto &tmp: params.command_interface_types_) { + for (const auto &tmp: admittance_->parameters_->command_interfaces_) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); } - for (const auto &tmp: params.chainable_command_interface_types_) { + for (const auto &tmp: admittance_->parameters_->chainable_command_interfaces_) { if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); } else { - RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", + RCLCPP_ERROR(get_node()->get_logger(), + "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } } - try { - admittance_->parameter_handler_ = std::make_shared(get_node()->get_node_parameters_interface()); - admittance_->parameters_ = &admittance_->parameter_handler_->params_; - } catch (const std::exception &e) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - num_joints_ = params.joint_names_.size(); - - auto get_interface_list = [](const std::vector &interface_types) { + auto get_interface_list = [](const std::vector &interface_types) { std::stringstream ss_command_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { if (index != 0) { @@ -238,8 +218,8 @@ namespace admittance_controller { }; RCLCPP_INFO( get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", - get_interface_list(params.command_interface_types_).c_str(), - get_interface_list(params.state_interface_types_).c_str()); + get_interface_list(admittance_->parameters_->command_interfaces_).c_str(), + get_interface_list(admittance_->parameters_->state_interfaces_).c_str()); // setup subscribers and publishers input_joint_command_subscriber_ = get_node()->create_subscription( @@ -248,11 +228,11 @@ namespace admittance_controller { s_publisher_ = get_node()->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); rtBuffers.state_publisher_ = - std::make_unique < realtime_tools::RealtimePublisher < ControllerStateMsg >> (s_publisher_); + std::make_unique>(s_publisher_); // Initialize state message rtBuffers.state_publisher_->lock(); - rtBuffers.state_publisher_->msg_.joint_names = params.joint_names_; + rtBuffers.state_publisher_->msg_.joint_names = admittance_->parameters_->joints_; rtBuffers.state_publisher_->msg_.actual_joint_state.positions.resize(num_joints_, 0.0); rtBuffers.state_publisher_->msg_.desired_joint_state.positions.resize(num_joints_, 0.0); rtBuffers.state_publisher_->msg_.error_joint_state.positions.resize(num_joints_, 0.0); @@ -263,7 +243,7 @@ namespace admittance_controller { semantic_components::ForceTorqueSensor(admittance_->parameters_->ft_sensor_.name_)); // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR){ + if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) { return CallbackReturn::ERROR; } @@ -285,28 +265,29 @@ namespace admittance_controller { joint_velocity_state_interface_.clear(); // assign command interfaces - std::unordered_map < std::string, - std::vector < std::reference_wrapper < hardware_interface::LoanedCommandInterface >> * > + std::unordered_map> *> command_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} - }; - for (auto i = 0ul; i < params.command_interface_types_.size(); i++) { - for (auto j = 0ul; j < params.joint_names_.size(); j++) { - command_interface_map[params.command_interface_types_[i]]->emplace_back( + {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} + }; + for (auto i = 0ul; i < admittance_->parameters_->command_interfaces_.size(); i++) { + for (auto j = 0ul; j < admittance_->parameters_->joints_.size(); j++) { + command_interface_map[admittance_->parameters_->command_interfaces_[i]]->emplace_back( command_interfaces_[i * num_joints_ + j]); } } // assign state interfaces - std::unordered_map < std::string, - std::vector < std::reference_wrapper < hardware_interface::LoanedStateInterface >> * > state_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} - }; - for (auto i = 0ul; i < params.state_interface_types_.size(); i++) { - for (auto j = 0ul; j < params.joint_names_.size(); j++) { - state_interface_map[params.state_interface_types_[i]]->emplace_back(state_interfaces_[i * num_joints_ + j]); + std::unordered_map> *> state_interface_map = { + {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, + {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} + }; + for (auto i = 0ul; i < admittance_->parameters_->state_interfaces_.size(); i++) { + for (auto j = 0ul; j < admittance_->parameters_->joints_.size(); j++) { + state_interface_map[admittance_->parameters_->state_interfaces_[i]]->emplace_back( + state_interfaces_[i * num_joints_ + j]); } } @@ -466,5 +447,5 @@ namespace admittance_controller { #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(admittance_controller::AdmittanceController, - controller_interface::ChainableControllerInterface + controller_interface::ChainableControllerInterface ) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 317e763e4f..592e0e61fb 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -188,28 +188,28 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { - ASSERT_TRUE(controller_->params.joint_names_.empty()); - ASSERT_TRUE(controller_->params.command_interface_types_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->joints_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->command_interfaces_.empty()); SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(!controller_->params.joint_names_.empty()); - ASSERT_TRUE(controller_->params.joint_names_.size() == joint_names_.size()); - ASSERT_TRUE(std::equal(controller_->params.joint_names_.begin(), controller_->params.joint_names_.end(), + ASSERT_TRUE(!controller_->admittance_->parameters_->joints_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->joints_.size() == joint_names_.size()); + ASSERT_TRUE(std::equal(controller_->admittance_->parameters_->joints_.begin(), controller_->admittance_->parameters_->joints_.end(), joint_names_.begin(), joint_names_.end())); - ASSERT_TRUE(!controller_->params.command_interface_types_.empty()); - ASSERT_TRUE(controller_->params.command_interface_types_.size() == command_interface_types_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_->command_interfaces_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->command_interfaces_.size() == command_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->params.command_interface_types_.begin(), controller_->params.command_interface_types_.end(), + controller_->admittance_->parameters_->command_interfaces_.begin(), controller_->admittance_->parameters_->command_interfaces_.end(), command_interface_types_.begin(), command_interface_types_.end())); - ASSERT_TRUE(!controller_->params.state_interface_types_.empty()); - ASSERT_TRUE(controller_->params.state_interface_types_.size() == state_interface_types_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_->state_interfaces_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_->state_interfaces_.size() == state_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->params.state_interface_types_.begin(), controller_->params.state_interface_types_.end(), + controller_->admittance_->parameters_->state_interfaces_.begin(), controller_->admittance_->parameters_->state_interfaces_.end(), state_interface_types_.begin(), state_interface_types_.end())); ASSERT_EQ(controller_->admittance_->parameters_->ft_sensor_.name_, ft_sensor_name_); diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 37550447aa..90ee6969c1 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -1,38 +1,19 @@ -load_admittance_controller: - # contains minimal parameters that need to be set to load controller +admittance_controller: ros__parameters: joints: - - joint1 - - joint2 - - command_interfaces: - - velocity - - state_interfaces: - - position - - velocity - - chainable_command_interfaces: - - position - - velocity - - -test_admittance_controller: - # contains minimal needed parameters for kuka_kr6 - ros__parameters: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint command_interfaces: - position state_interfaces: - position + - velocity chainable_command_interfaces: - position @@ -42,25 +23,38 @@ test_admittance_controller: plugin_name: kinematics_interface_kdl/KDLKinematics plugin_package: kinematics_interface_kdl base: base_link # Assumed to be stationary - tip: tool0 - group_name: arm + tip: ee_link + group_name: ur5e_manipulator alpha: 0.0005 ft_sensor: - name: ft_sensor_name + name: tcp_fts_sensor frame: - id: sensor_frame # tool0 Wrench measurements are in this frame + id: ee_link # ee_link Wrench measurements are in this frame external: false # force torque frame exists within URDF kinematic chain filter_coefficient: 0.005 control: frame: - id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + id: ee_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector external: false # control frame exists within URDF kinematic chain - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - external: false # control frame exists within URDF kinematic chain + fixed_world_frame: + frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: ee_link + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 admittance: selected_axes: @@ -74,12 +68,12 @@ test_admittance_controller: # Having ".0" at the end is MUST, otherwise there is a loading error # F = M*a + D*v + S*(x - x_d) mass: - - 5.5 - - 6.6 - - 7.7 - - 8.8 - - 9.9 - - 10.10 + - 3.0 # x + - 3.0 # y + - 3.0 # z + - 0.05 # rx + - 0.05 # ry + - 0.05 # rz damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) - 2.828427 # x @@ -90,11 +84,13 @@ test_admittance_controller: - 2.828427 # rz stiffness: - - 214.1 - - 214.2 - - 214.3 - - 214.4 - - 214.5 - - 214.6 - - + - 50.0 # x + - 50.0 # y + - 50.0 # z + - 1.0 # rx + - 1.0 # ry + - 1.0 # rz + + # general settings + enable_parameter_update_without_reactivation: true + use_feedforward_commanded_input: true \ No newline at end of file From a0f4baec84a6f1975907a9c5c4abf1298195abdd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 6 Jul 2022 16:34:59 -0600 Subject: [PATCH 033/111] general code clean up --- admittance_controller/CMakeLists.txt | 2 +- .../admittance_rule_impl.hpp | 14 +++++------ .../config/definition.yaml | 2 +- .../src/admittance_controller.cpp | 23 +++---------------- 4 files changed, 11 insertions(+), 30 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 089def0aa9..ebdabe9d18 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -32,7 +32,6 @@ find_package(trajectory_msgs REQUIRED) find_package(angles REQUIRED) find_package(rcutils REQUIRED) find_package(tf2_kdl REQUIRED) -find_package(gen_param_struct REQUIRED) # The admittance controller @@ -47,6 +46,7 @@ target_include_directories( ) # uncomment to generate struct +#find_package(gen_param_struct REQUIRED) ## specify which yaml root name to convert to struct #set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config/definition.yaml) ## specify which yaml root name to convert to struct diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 5b76955a94..3548d05602 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -153,13 +153,10 @@ namespace admittance_controller { parameters_->control_.frame_.external_, success); reference_ft_transform_ = get_transform(reference_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); - ft_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, parameters_->ft_sensor_.frame_.external_, success); - ee_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, false, success); - world_transform_ = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.frame_.id_, parameters_->fixed_world_frame_.frame_.external_, success); sensor_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, @@ -177,7 +174,7 @@ namespace admittance_controller { // apply filter and update wrench_world_ vector process_wrench_measurements(measured_wrench, world_rot_ * sensor_rot_, world_rot_ * cog_rot_); - // transform wrench_world_ into control frame + // transform wrench_world_ into base frame Eigen::Matrix wrench_base = world_rot_.transpose() * wrench_world_; // calculate desired cartesian velocity in control frame @@ -194,7 +191,6 @@ namespace admittance_controller { // calculate drift due to integrating the joint positions Eigen::Matrix admittance_position_base = reference_ft_transform_.block<3,1>(0, 3) + admittance_position_.block<3,1>(0, 3); Eigen::Matrix admittance_actually_position_base = ft_transform_.block<3,1>(0,3); - Eigen::Matrix ft_rot = ft_transform_.block<3,3>(0,0); Eigen::Matrix reference_ft_rot = reference_ft_transform_.block<3,3>(0,0); Eigen::Matrix admittance_rot = admittance_position_.block<3,3>(0,0); @@ -210,10 +206,11 @@ namespace admittance_controller { double sign = (tmp >= 0) ? 1.0 : -1.0; correction_velocity.block<3, 1>(0, 1) = .1*sign*theta*V; + // calculate joint velocities that correspondence to cartesian velocities convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_ + correction_velocity, parameters_->ft_sensor_.frame_.id_, joint_vel_, success); - + // calculate joint velocities that correspondence to cartesian accelerations convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_, parameters_->ft_sensor_.frame_.id_, joint_acc_, success); @@ -231,7 +228,6 @@ namespace admittance_controller { state_message_.error_joint_state.positions[i] = reference_joint_state.positions[i] - current_joint_state.positions[i]; } - for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel_[i]; state_message_.error_joint_state.velocities[i] = @@ -336,8 +332,9 @@ namespace admittance_controller { new_wrench(1, 1) = measured_wrench.torque.y; new_wrench(2, 1) = measured_wrench.torque.z; - // transform to base frame + // transform to world frame Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; + // store value for state message measured_wrench_ = new_wrench_base; @@ -345,6 +342,7 @@ namespace admittance_controller { new_wrench_base(2, 0) -= ee_weight[2]; new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight); + // apply smoothing filter for (auto i = 0; i < 6; i++) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_->ft_sensor_.filter_coefficient_); diff --git a/admittance_controller/include/admittance_controller/config/definition.yaml b/admittance_controller/include/admittance_controller/config/definition.yaml index 974c003595..a559510897 100644 --- a/admittance_controller/include/admittance_controller/config/definition.yaml +++ b/admittance_controller/include/admittance_controller/config/definition.yaml @@ -147,7 +147,7 @@ admittance_struct: stiffness: { type: double_array, - default_value: [ .NAN, .NAN, .NAN, .NAN, .NAN, .NAN ], + fixed_size: 6, description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 281089cc24..266feaae1b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -50,17 +50,6 @@ namespace admittance_controller { admittance_ = std::make_unique(); - try { - admittance_->parameter_handler_ = std::make_shared( - get_node()->get_node_parameters_interface()); - admittance_->parameters_ = &admittance_->parameter_handler_->params_; - } catch (const std::exception &e) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - num_joints_ = admittance_->parameters_->joints_.size(); - for (const auto &tmp: admittance_->parameters_->state_interfaces_) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); } @@ -185,9 +174,9 @@ namespace admittance_controller { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } - num_joints_ = admittance_->parameters_->joints_.size(); + // print and validate interface types for (const auto &tmp: admittance_->parameters_->state_interfaces_) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); } @@ -205,7 +194,6 @@ namespace admittance_controller { } } - auto get_interface_list = [](const std::vector &interface_types) { std::stringstream ss_command_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { @@ -325,14 +313,12 @@ namespace admittance_controller { // update input reference from chainable interfaces read_state_reference_interfaces(state_reference_); - // sense: get all controller inputs + // get all controller inputs geometry_msgs::msg::Wrench ft_values; force_torque_sensor_->get_values_as_message(ft_values); read_state_from_hardware(state_current_); - // command: determine desired state from trajectory or pose goal - // and apply admittance controller - + // apply admittance control to reference to determine desired state admittance_->update(state_current_, ft_values, state_reference_, period, state_desired_); // write calculated values to joint interfaces @@ -351,8 +337,6 @@ namespace admittance_controller { last_commanded_state_.accelerations[i] = state_desired_.accelerations[i]; } - // save state reference before applying admittance rule TODO why is this here? - // pre_admittance_point.points[0] = state_reference_; // Publish controller state rtBuffers.state_publisher_->lock(); rtBuffers.state_publisher_->msg_.input_joint_command = pre_admittance_point; @@ -373,7 +357,6 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_cleanup(const rclcpp_lifecycle::State &previous_state) { - return CallbackReturn::SUCCESS; } From 27ea60f5bdf6058c671f33a0c5607738eb914be7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 6 Jul 2022 22:10:54 -0600 Subject: [PATCH 034/111] parameter handling tests passing --- .../admittance_rule_impl.hpp | 5 + .../config/admittance_struct.h | 57 +++-- .../config/definition.yaml | 7 +- .../include/gen_param_struct/validators.hpp | 87 ++++++++ .../test/test_admittance_controller.cpp | 211 ++++++------------ .../test/test_admittance_controller.hpp | 133 +++++------ admittance_controller/test/test_params.yaml | 79 ++++--- 7 files changed, 311 insertions(+), 268 deletions(-) create mode 100644 admittance_controller/include/gen_param_struct/validators.hpp diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 3548d05602..78e491fad9 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -486,6 +486,10 @@ namespace admittance_controller { void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { + // TODO these fields are not used + eigen_to_msg(0*wrench_world_, parameters_->control_.frame_.id_, state_message.input_wrench_command); + state_message.input_pose_command.header.frame_id = parameters_->control_.frame_.id_; + eigen_to_msg(wrench_world_, parameters_->fixed_world_frame_.frame_.id_, state_message.measured_wrench_filtered); eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.frame_.id_, state_message.measured_wrench); eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_->control_.frame_.id_, @@ -498,6 +502,7 @@ namespace admittance_controller { state_message.actual_joint_state = state_message_.actual_joint_state; state_message.error_joint_state = state_message_.error_joint_state; + state_message.admittance_rule_calculated_values = state_message_.admittance_rule_calculated_values; } diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h index 2b62e1278e..d42997d09e 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ b/admittance_controller/include/admittance_controller/config/admittance_struct.h @@ -62,18 +62,14 @@ namespace admittance_struct_parameters { } frame_; struct CoG { std::vector pos_; - double force_ = std::numeric_limits::quiet_NaN(); + double force_; } CoG_; } gravity_compensation_; struct admittance { std::vector selected_axes_; std::vector mass_; std::vector damping_ratio_; - std::vector stiffness_ = {std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(),}; + std::vector stiffness_; } admittance_; std::string robot_description_; bool enable_parameter_update_without_reactivation_ = true; @@ -209,11 +205,21 @@ namespace admittance_struct_parameters { } } if (param.get_name() == "admittance.damping_ratio") { - validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.1, 10.0); + validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.damping_ratio_ = param.as_double_array(); + result.successful = true; + } else { + result.reason = validation_result.error_msg(); + result.successful = false; + } + } + if (param.get_name() == "admittance.stiffness") { + validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0, 100000000.0); if (validation_result.success()) { validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); if (validation_result.success()) { - params_.admittance_.damping_ratio_ = param.as_double_array(); + params_.admittance_.stiffness_ = param.as_double_array(); result.successful = true; } else { result.reason = validation_result.error_msg(); @@ -224,10 +230,6 @@ namespace admittance_struct_parameters { result.successful = false; } } - if (param.get_name() == "admittance.stiffness") { - params_.admittance_.stiffness_ = param.as_double_array(); - result.successful = true; - } if (param.get_name() == "robot_description") { params_.robot_description_ = param.as_string(); result.successful = true; @@ -449,7 +451,7 @@ namespace admittance_struct_parameters { descriptor.description = "weight of the end effector, e.g mass * 9.81"; descriptor.read_only = false; if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { - auto p_gravity_compensation_CoG_force = rclcpp::ParameterValue(params_.gravity_compensation_.CoG_.force_); + auto p_gravity_compensation_CoG_force = rclcpp::ParameterType::PARAMETER_DOUBLE; parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force, descriptor); } @@ -479,10 +481,6 @@ namespace admittance_struct_parameters { { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))"; - rcl_interfaces::msg::FloatingPointRange range; - range.from_value = 0.1; - range.to_value = 10.0; - descriptor.floating_point_range.push_back(range); descriptor.read_only = false; if (!parameters_interface->has_parameter("admittance.damping_ratio")) { auto p_admittance_damping_ratio = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; @@ -492,9 +490,13 @@ namespace admittance_struct_parameters { { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation"; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0; + range.to_value = 100000000.0; + descriptor.floating_point_range.push_back(range); descriptor.read_only = false; if (!parameters_interface->has_parameter("admittance.stiffness")) { - auto p_admittance_stiffness = rclcpp::ParameterValue(params_.admittance_.stiffness_); + auto p_admittance_stiffness = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness, descriptor); } } @@ -609,23 +611,30 @@ namespace admittance_struct_parameters { "Invalid value set during initialization for parameter admittance.mass: " + validation_result.error_msg()); } param = parameters_interface->get_parameter("admittance.damping_ratio"); - validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.1, 10.0); + validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); + if (validation_result.success()) { + params_.admittance_.damping_ratio_ = param.as_double_array(); + } else { + throw rclcpp::exceptions::InvalidParameterValueException( + "Invalid value set during initialization for parameter admittance.damping_ratio: " + + validation_result.error_msg()); + } + param = parameters_interface->get_parameter("admittance.stiffness"); + validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0, 100000000.0); if (validation_result.success()) { validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); if (validation_result.success()) { - params_.admittance_.damping_ratio_ = param.as_double_array(); + params_.admittance_.stiffness_ = param.as_double_array(); } else { throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.damping_ratio: " + + "Invalid value set during initialization for parameter admittance.stiffness: " + validation_result.error_msg()); } } else { throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.damping_ratio: " + + "Invalid value set during initialization for parameter admittance.stiffness: " + validation_result.error_msg()); } - param = parameters_interface->get_parameter("admittance.stiffness"); - params_.admittance_.stiffness_ = param.as_double_array(); param = parameters_interface->get_parameter("robot_description"); params_.robot_description_ = param.as_string(); param = parameters_interface->get_parameter("enable_parameter_update_without_reactivation"); diff --git a/admittance_controller/include/admittance_controller/config/definition.yaml b/admittance_controller/include/admittance_controller/config/definition.yaml index a559510897..f97ed803c2 100644 --- a/admittance_controller/include/admittance_controller/config/definition.yaml +++ b/admittance_controller/include/admittance_controller/config/definition.yaml @@ -49,6 +49,7 @@ admittance_struct: } group_name: { type: string, + default_value: "", description: "specifies the group name for planning with Moveit" } @@ -116,7 +117,7 @@ admittance_struct: } force: { type: double, - default_value: .NAN, + default_value: 0.0, description: "weight of the end effector, e.g mass * 9.81" } @@ -141,14 +142,14 @@ admittance_struct: type: double_array, fixed_size: 6, description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. - The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))", - bounds: [ 0.1, 10.0 ] + The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))" } stiffness: { type: double_array, fixed_size: 6, description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", + bounds: [0.0, 100000000.0] } # general settings diff --git a/admittance_controller/include/gen_param_struct/validators.hpp b/admittance_controller/include/gen_param_struct/validators.hpp new file mode 100644 index 0000000000..6e0d3a9e7b --- /dev/null +++ b/admittance_controller/include/gen_param_struct/validators.hpp @@ -0,0 +1,87 @@ + +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "variant" +#include + +namespace gen_param_struct_validators{ + + class Result { + public: + template + Result( const std::string& format, Args ... args ) + { + int size_s = std::snprintf( nullptr, 0, format.c_str(), args ... ) + 1; // Extra space for '\0' + if( size_s <= 0 ){ throw std::runtime_error( "Error during formatting." ); } + auto size = static_cast( size_s ); + std::unique_ptr buf( new char[ size ] ); + std::snprintf( buf.get(), size, format.c_str(), args ... ); + msg_ = std::string( buf.get(), buf.get() + size - 1 ); // We don't want the '\0' inside + success_ = false; + } + + Result() { + success_ = true; + } + + bool success(){ + return success_; + } + + std::string error_msg(){ + return msg_; + } + + private: + std::string msg_; + bool success_; + }; + + auto OK = Result(); + using ERROR = Result; + + Result validate_string_array_len(const rclcpp::Parameter& parameter, size_t size) { + const auto& string_array = parameter.as_string_array(); + if (string_array.size() != size) { + return ERROR( + "The length of the parameter was incorrect. Expected size is %d, actual size is %d", + size, string_array.size()); + } + return OK; + } + + Result validate_double_array_len(const rclcpp::Parameter& parameter, size_t size) { + const auto& double_array = parameter.as_double_array(); + if (double_array.size() != size) { + return ERROR( + "The length of the parameter was incorrect. Expected size is %d, actual size is %d", + size, double_array.size()); + } + return OK; + } + + Result validate_bool_array_len(const rclcpp::Parameter& parameter, size_t size) { + const auto& bool_array = parameter.as_bool_array(); + if (bool_array.size() != size) { + return ERROR( + "The length of the parameter was incorrect. Expected size is %d, actual size is %d", + size, bool_array.size()); + } + return OK; + } + + Result validate_double_array_bounds(const rclcpp::Parameter& parameter, double lower_bound, double upper_bound) { + const auto &double_array = parameter.as_double_array(); + for (auto val: double_array) { + if (val < lower_bound || val > upper_bound) { + return ERROR( + "The parameter value (%f) was outside the allowed bounds [(%f), (%f)]", + val, lower_bound, upper_bound); + } + } + return OK; + } + +} // namespace gen_param_struct + diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 592e0e61fb..e89818ae99 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -22,165 +22,101 @@ #include -// When there are many mandatory parameters, set all by default and remove one by one in a -// parameterized test -TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_invalid) +// Test on_configure returns ERROR when a parameter is invalid +TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, one_parameter_is_invalid) { - SetUpController(true); + SetUpController(); + auto name = std::get<0>(GetParam()); + auto val = std::get<1>(GetParam()); + rclcpp::Parameter parameter(name, val); + controller_->get_node()->set_parameter(parameter); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -// When there are many mandatory parameters, set all by default and remove one by one in a -// parameterized test -TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_missing) +// Test on_configure returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) { - SetUpController(false); - + SetUpController(GetParam()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -// TODO(anyone): the new gtest version afer 1.8.0 uses INSTANTIATE_TEST_SUITE_P INSTANTIATE_TEST_SUITE_P( - MissingMandatoryParameterDuringConfiguration, - AdmittanceControllerTestParameterizedParameters, - ::testing::Values( - std::make_tuple( - std::string("joints"), - rclcpp::ParameterValue(std::vector()) - ), - std::make_tuple( - std::string("command_interfaces"), - rclcpp::ParameterValue(std::vector()) - ), - std::make_tuple( - std::string("state_interfaces"), - rclcpp::ParameterValue(std::vector()) - ), - std::make_tuple( - std::string("ft_sensor_name"), - rclcpp::ParameterValue("") - ), -// std::make_tuple( -// std::string("use_joint_commands_as_input"), -// rclcpp::ParameterValue(false) -// ), -// std::make_tuple( -// std::string("hardware_state_has_offset"), -// rclcpp::ParameterValue(false) -// ), - std::make_tuple( - std::string("kinematics.base"), - rclcpp::ParameterValue("") - ), - std::make_tuple( - std::string("kinematics.group_name"), - rclcpp::ParameterValue("") - ), - std::make_tuple( - std::string("control_frame"), - rclcpp::ParameterValue("") - ), - std::make_tuple( - std::string("sensor_frame"), - rclcpp::ParameterValue("") - ), - // TODO(anyone): this tests are unstable... -// std::make_tuple( -// std::string("admittance.selected_axes.x"), -// rclcpp::ParameterValue(false) -// ), -// std::make_tuple( -// std::string("admittance.selected_axes.y"), -// rclcpp::ParameterValue(false) -// ), -// std::make_tuple( -// std::string("admittance.selected_axes.z"), -// rclcpp::ParameterValue(false) -// ), -// std::make_tuple( -// std::string("admittance.selected_axes.rx"), -// rclcpp::ParameterValue(false) -// ), -// std::make_tuple( -// std::string("admittance.selected_axes.ry"), -// rclcpp::ParameterValue(false) -// ), -// std::make_tuple( -// std::string("admittance.selected_axes.rz"), -// rclcpp::ParameterValue(false) -// ), - std::make_tuple( - std::string("admittance.mass.x"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.mass.y"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.mass.z"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.mass.rx"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.mass.ry"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.mass.rz"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.damping.x"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.damping.y"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.damping.z"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingParameters, + ::testing::Values( +// "admittance.damping_ratio", optional + "admittance.mass", + "admittance.selected_axes", + "admittance.stiffness", + "chainable_command_interfaces", + "command_interfaces", + "control.frame.external", + "control.frame.id", + "fixed_world_frame.frame.external", + "fixed_world_frame.frame.id", +// "ft_sensor.filter_coefficient", optional + "ft_sensor.frame.external", + "ft_sensor.frame.id", + "ft_sensor.name", +// "gravity_compensation.CoG.force", optional + "gravity_compensation.CoG.pos", + "gravity_compensation.frame.external", + "gravity_compensation.frame.id", + "joints", +// "kinematics.alpha", optional + "kinematics.base", + "kinematics.group_name", + "kinematics.plugin_name", + "kinematics.plugin_package", + "kinematics.tip", + "state_interfaces" + ) +); + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterDuringConfiguration, + AdmittanceControllerTestParameterizedInvalidParameters, + ::testing::Values( + // wrong length COG std::make_tuple( - std::string("admittance.damping.rx"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("gravity_compensation.CoG.pos"), + rclcpp::ParameterValue(std::vector()={1,2,3,4}) ), + // wrong length stiffness std::make_tuple( - std::string("admittance.damping.ry"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector()={1,2,3}) ), + // negative stiffness std::make_tuple( - std::string("admittance.damping.rz"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) - ), - std::make_tuple( - std::string("admittance.stiffness.x"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector()={-1,-2,3,4,5,6}) ), + // wrong length mass std::make_tuple( - std::string("admittance.stiffness.y"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("admittance.mass"), + rclcpp::ParameterValue(std::vector()={1,2,3}) ), + // negative mass std::make_tuple( - std::string("admittance.stiffness.z"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("admittance.mass"), + rclcpp::ParameterValue(std::vector()={-1,-2,3,4,5,6}) ), + // wrong length damping ratio std::make_tuple( - std::string("admittance.stiffness.rx"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("admittance.damping_ratio"), + rclcpp::ParameterValue(std::vector()={1,2,3}) ), + // wrong length selected axes std::make_tuple( - std::string("admittance.stiffness.ry"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("admittance.selected_axes"), + rclcpp::ParameterValue(std::vector()={1,2,3}) ), + // invalid robot description std::make_tuple( - std::string("admittance.stiffness.rz"), - rclcpp::ParameterValue(std::numeric_limits::quiet_NaN()) + std::string("robot_description"), + rclcpp::ParameterValue(std::string()="bad_robot") ) ) ); @@ -188,10 +124,11 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { + SetUpController(); + ASSERT_TRUE(controller_->admittance_->parameters_->joints_.empty()); ASSERT_TRUE(controller_->admittance_->parameters_->command_interfaces_.empty()); - SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -331,7 +268,7 @@ TEST_F(AdmittanceControllerTest, reactivate_success) TEST_F(AdmittanceControllerTest, publish_status_success) { // TODO: Write also a test when Cartesian commands are used. - SetUpController(true, true); + SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -428,7 +365,7 @@ TEST_F(AdmittanceControllerTest, publish_status_success) TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) { - SetUpController(true, true); + SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index d0f7e19ffa..b642083527 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -145,66 +145,33 @@ class AdmittanceControllerTest : public ::testing::Test } protected: - void SetUpController(bool set_parameters = true, bool use_joint_commands_as_input = false) + + void SetUpController(const std::string& controller_name, + const std::vector & parameter_overrides) { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(true).parameter_overrides(parameter_overrides) + .automatically_declare_parameters_from_overrides(true); + SetUpControllerCommon(controller_name, options); + } + + void SetUpController(const std::string& controller_name="test_admittance_controller") { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + SetUpControllerCommon(controller_name, options); + } + + void SetUpControllerCommon(const std::string& controller_name, const rclcpp::NodeOptions& options) { - const auto result = controller_->init("test_admittance_controller"); +// auto options = rclcpp::NodeOptions() +// .allow_undeclared_parameters(true) +// .automatically_declare_parameters_from_overrides(true); + const auto result = controller_->init(controller_name, "", options); ASSERT_EQ(result, controller_interface::return_type::OK); assign_interfaces(); controller_->get_node()->set_parameter({"robot_description", robot_description_}); controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); - -// if (set_parameters) { -// controller_->get_node()->set_parameter({"use_joint_commands_as_input", use_joint_commands_as_input}); -// -// controller_->get_node()->set_parameter({"joints", joint_names_}); -// controller_->get_node()->set_parameter({"command_interfaces", command_interface_types_}); -// controller_->get_node()->set_parameter({"state_interfaces", state_interface_types_}); -// controller_->get_node()->set_parameter({"ft_sensor_name", ft_sensor_name_}); -// controller_->get_node()->set_parameter({"hardware_state_has_offset", hardware_state_has_offset_}); -// -// controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); -// controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); -// // TODO(destogl): enable when IK support is added -//// controller_->get_node()->set_parameter({"IK.plugin", ik_group_name_}); -// controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_}); -// controller_->get_node()->set_parameter({"robot_description", robot_description_}); -// controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); -// -// controller_->get_node()->set_parameter({"control_frame", control_frame_}); -// controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_}); -// controller_->get_node()->set_parameter({"fixed_world_frame", fixed_world_frame_}); -// controller_->get_node()->set_parameter({"sensor_frame", sensor_frame_}); -// -// controller_->get_node()->set_parameter({"admittance.selected_axes.x", admittance_selected_axes_[0]}); -// controller_->get_node()->set_parameter({"admittance.selected_axes.y", admittance_selected_axes_[1]}); -// controller_->get_node()->set_parameter({"admittance.selected_axes.z", admittance_selected_axes_[2]}); -// controller_->get_node()->set_parameter({"admittance.selected_axes.rx", admittance_selected_axes_[3]}); -// controller_->get_node()->set_parameter({"admittance.selected_axes.ry", admittance_selected_axes_[4]}); -// controller_->get_node()->set_parameter({"admittance.selected_axes.rz", admittance_selected_axes_[5]}); -// -// controller_->get_node()->set_parameter({"admittance.mass.x", admittance_mass_[0]}); -// controller_->get_node()->set_parameter({"admittance.mass.y", admittance_mass_[1]}); -// controller_->get_node()->set_parameter({"admittance.mass.z", admittance_mass_[2]}); -// controller_->get_node()->set_parameter({"admittance.mass.rx", admittance_mass_[3]}); -// controller_->get_node()->set_parameter({"admittance.mass.ry", admittance_mass_[4]}); -// controller_->get_node()->set_parameter({"admittance.mass.rz", admittance_mass_[5]}); -// -// controller_->get_node()->set_parameter({"admittance.damping.x", admittance_damping_ratio_[0]}); -// controller_->get_node()->set_parameter({"admittance.damping.y", admittance_damping_ratio_[1]}); -// controller_->get_node()->set_parameter({"admittance.damping.z", admittance_damping_ratio_[2]}); -// controller_->get_node()->set_parameter({"admittance.damping.rx", admittance_damping_ratio_[3]}); -// controller_->get_node()->set_parameter({"admittance.damping.ry", admittance_damping_ratio_[4]}); -// controller_->get_node()->set_parameter({"admittance.damping.rz", admittance_damping_ratio_[5]}); -// -// controller_->get_node()->set_parameter({"admittance.stiffness.x", admittance_stiffness_[0]}); -// controller_->get_node()->set_parameter({"admittance.stiffness.y", admittance_stiffness_[1]}); -// controller_->get_node()->set_parameter({"admittance.stiffness.z", admittance_stiffness_[2]}); -// controller_->get_node()->set_parameter({"admittance.stiffness.rx", admittance_stiffness_[3]}); -// controller_->get_node()->set_parameter({"admittance.stiffness.ry", admittance_stiffness_[4]}); -// controller_->get_node()->set_parameter({"admittance.stiffness.rz", admittance_stiffness_[5]}); -// -// } } void assign_interfaces() @@ -388,10 +355,10 @@ class AdmittanceControllerTest : public ::testing::Test const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; - const std::string control_frame_ = "control_frame"; + const std::string control_frame_ = "tool0"; const std::string endeffector_frame_ = "endeffector_frame"; const std::string fixed_world_frame_ = "fixed_world_frame"; - const std::string sensor_frame_ = "sensor_frame"; + const std::string sensor_frame_ = "link_6"; std::array admittance_selected_axes_ = {true, true, true, true, true, true}; std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; @@ -416,15 +383,18 @@ class AdmittanceControllerTest : public ::testing::Test rclcpp::Node::SharedPtr test_broadcaster_node_; }; + // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest -class AdmittanceControllerTestParameterizedParameters -: public AdmittanceControllerTest, -public ::testing::WithParamInterface> +class AdmittanceControllerTestParameterizedMissingParameters + : public AdmittanceControllerTest, + public ::testing::WithParamInterface { public: virtual void SetUp() { AdmittanceControllerTest::SetUp(); + auto node = std::make_shared("test_admittance_controller"); + overrides_ = node->get_node_parameters_interface()->get_parameter_overrides(); } static void TearDownTestCase() @@ -433,25 +403,44 @@ public ::testing::WithParamInterfaceget_node()->undeclare_parameter(std::get<0>(GetParam())); -// controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); -// rclcpp::Parameter parameter("joints"); -// controller_->get_node()->set_parameter(parameter); -// controller_->get_node()->set_parameter({"joints", ""}); - if (set_parameters){ - auto tmp = std::get<1>(GetParam()); - rclcpp::Parameter parameter(std::get<0>(GetParam()),std::get<1>(GetParam())); - } else{ - controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); + std::vector parameter_overrides; + for (const auto& override : overrides_){ + if (override.first != remove_name){ + rclcpp::Parameter param(override.first, override.second); + parameter_overrides.push_back(param); + } } + AdmittanceControllerTest::SetUpController("test_admittance_controller_no_overrides", parameter_overrides); + } + + std::map overrides_; +}; + -// controller_->get_node()->set_parameter(parameter); +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedInvalidParameters +: public AdmittanceControllerTest, +public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() + { + AdmittanceControllerTest::SetUp(); } + static void TearDownTestCase() + { + AdmittanceControllerTest::TearDownTestCase(); + } + +protected: + void SetUpController(bool val=false) + { + AdmittanceControllerTest::SetUpController("test_admittance_controller"); + } }; #endif // TEST_ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 90ee6969c1..20ad6c7682 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -1,15 +1,12 @@ -admittance_controller: +load_admittance_controller: + # contains minimal parameters that need to be set to load controller ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - joint1 + - joint2 command_interfaces: - - position + - velocity state_interfaces: - position @@ -19,34 +16,56 @@ admittance_controller: - position - velocity + +test_admittance_controller: + # contains minimal needed parameters for kuka_kr6 + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + + chainable_command_interfaces: + - position + - velocity + kinematics: plugin_name: kinematics_interface_kdl/KDLKinematics plugin_package: kinematics_interface_kdl base: base_link # Assumed to be stationary - tip: ee_link - group_name: ur5e_manipulator + tip: tool0 + group_name: arm alpha: 0.0005 ft_sensor: - name: tcp_fts_sensor + name: ft_sensor_name frame: - id: ee_link # ee_link Wrench measurements are in this frame + id: link_6 # tool0 Wrench measurements are in this frame external: false # force torque frame exists within URDF kinematic chain filter_coefficient: 0.005 control: frame: - id: ee_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector external: false # control frame exists within URDF kinematic chain - fixed_world_frame: - frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector external: false # control frame exists within URDF kinematic chain gravity_compensation: frame: - id: ee_link + id: tool0 external: false CoG: # specifies the center of gravity of the end effector @@ -68,12 +87,12 @@ admittance_controller: # Having ".0" at the end is MUST, otherwise there is a loading error # F = M*a + D*v + S*(x - x_d) mass: - - 3.0 # x - - 3.0 # y - - 3.0 # z - - 0.05 # rx - - 0.05 # ry - - 0.05 # rz + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) - 2.828427 # x @@ -84,13 +103,9 @@ admittance_controller: - 2.828427 # rz stiffness: - - 50.0 # x - - 50.0 # y - - 50.0 # z - - 1.0 # rx - - 1.0 # ry - - 1.0 # rz - - # general settings - enable_parameter_update_without_reactivation: true - use_feedforward_commanded_input: true \ No newline at end of file + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 From 737363c77e1dcdebe67f4b2b67fb0d92f6c264f6 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 19 Jul 2022 08:02:11 -0600 Subject: [PATCH 035/111] declare parameters in test --- admittance_controller/test/test_admittance_controller.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b642083527..535a0db9fc 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -149,14 +149,14 @@ class AdmittanceControllerTest : public ::testing::Test void SetUpController(const std::string& controller_name, const std::vector & parameter_overrides) { auto options = rclcpp::NodeOptions() - .allow_undeclared_parameters(true).parameter_overrides(parameter_overrides) + .allow_undeclared_parameters(false).parameter_overrides(parameter_overrides) .automatically_declare_parameters_from_overrides(true); SetUpControllerCommon(controller_name, options); } void SetUpController(const std::string& controller_name="test_admittance_controller") { auto options = rclcpp::NodeOptions() - .allow_undeclared_parameters(true) + .allow_undeclared_parameters(false) .automatically_declare_parameters_from_overrides(true); SetUpControllerCommon(controller_name, options); } @@ -170,6 +170,8 @@ class AdmittanceControllerTest : public ::testing::Test ASSERT_EQ(result, controller_interface::return_type::OK); assign_interfaces(); + controller_->get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + controller_->get_node()->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); controller_->get_node()->set_parameter({"robot_description", robot_description_}); controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); } From 6936c5d3648e1c09066adf75001236551d502b30 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 27 Jul 2022 16:41:57 -0600 Subject: [PATCH 036/111] change parameter struct format --- admittance_controller/CMakeLists.txt | 14 +- .../admittance_controller.hpp | 33 +- .../admittance_controller/admittance_rule.hpp | 250 ++++-- .../admittance_rule_impl.hpp | 407 +++++----- .../admittance_controller_parameters.hpp | 726 ++++++++++++++++++ .../config/admittance_struct.h | 648 ---------------- .../include/gen_param_struct/validators.hpp | 87 --- admittance_controller/package.xml | 4 +- .../src/admittance_controller.cpp | 217 +++--- .../admittance_controller_parameters.yaml} | 32 +- .../test/6d_robot_description.hpp | 313 ++++++++ .../test/test_admittance_controller.cpp | 255 +++--- .../test/test_admittance_controller.hpp | 30 +- 13 files changed, 1702 insertions(+), 1314 deletions(-) create mode 100644 admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp delete mode 100644 admittance_controller/include/admittance_controller/config/admittance_struct.h delete mode 100644 admittance_controller/include/gen_param_struct/validators.hpp rename admittance_controller/{include/admittance_controller/config/definition.yaml => src/admittance_controller_parameters.yaml} (91%) create mode 100644 admittance_controller/test/6d_robot_description.hpp diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index ebdabe9d18..e86d15f3f8 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -45,14 +45,11 @@ target_include_directories( include ) -# uncomment to generate struct -#find_package(gen_param_struct REQUIRED) -## specify which yaml root name to convert to struct -#set(YAML_FILE ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config/definition.yaml) -## specify which yaml root name to convert to struct -#set(OUT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include/admittance_controller/config) -# -#generate_param_struct_header(admittance_controller ${OUT_DIR} ${YAML_FILE}) +# uncomment to generate parameter listener code +#find_package(generate_parameter_library REQUIRED) +#generate_parameter_library(admittance_controller_parameters +# src/admittance_controller_parameters.yaml +# ) ament_target_dependencies( @@ -130,7 +127,6 @@ if (BUILD_TESTING) ros2_control_test_assets ) # test admittance controller function -# ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp) add_test_with_yaml_input(test_admittance_controller "test/test_admittance_controller.cpp" ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) target_include_directories(test_admittance_controller PRIVATE include) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 5e22660017..3cf868142e 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -39,7 +39,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" - // TODO(destogl): this is only temporary to work with servo. It should be either trajectory_msgs/msg/JointTrajectoryPoint or std_msgs/msg/Float64MultiArray #include "trajectory_msgs/msg/joint_trajectory.hpp" @@ -50,15 +49,10 @@ namespace admittance_controller { using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - struct RTBuffers { - realtime_tools::RealtimeBuffer> input_joint_command_; - std::unique_ptr> state_publisher_; - }; - class AdmittanceController : public controller_interface::ChainableControllerInterface { public: ADMITTANCE_CONTROLLER_PUBLIC - AdmittanceController(); + AdmittanceController() = default; ADMITTANCE_CONTROLLER_PUBLIC CallbackReturn on_init() override; @@ -96,8 +90,9 @@ namespace admittance_controller { bool on_set_chained_mode(bool chained_mode) override; - int num_joints_{}; + int num_joints_; + // vectors to hold joint hardware interfaces std::vector> joint_position_command_interface_; std::vector> joint_velocity_command_interface_; std::vector> joint_acceleration_command_interface_; @@ -106,23 +101,30 @@ namespace admittance_controller { std::vector> joint_velocity_state_interface_; std::vector> joint_acceleration_state_interface_; - std::vector position_reference_; - std::vector velocity_reference_; + // + std::vector position_reference_; + std::vector velocity_reference_; // Admittance rule and dependent variables; std::unique_ptr admittance_; - // joint limiter TODO + + // force torque sensor std::unique_ptr force_torque_sensor_; + // ROS subscribers - rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; - rclcpp::Publisher::SharedPtr s_publisher_ = nullptr; + rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; + rclcpp::Publisher::SharedPtr s_publisher_; + // ROS messages std::shared_ptr joint_command_msg; + // real-time buffer - RTBuffers rtBuffers; + realtime_tools::RealtimeBuffer> input_joint_command_; + std::unique_ptr> state_publisher_; // controller running state - bool controller_is_active_{}; + bool controller_is_active_; + const std::set allowed_state_interface_types_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, @@ -137,7 +139,6 @@ namespace admittance_controller { trajectory_msgs::msg::JointTrajectory pre_admittance_point; // helper methods - void joint_command_callback(const std::shared_ptr msg); void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 4d3a58c76d..297c3575c6 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -18,7 +18,6 @@ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #include - #include #include #include "tf2_ros/transform_listener.h" @@ -29,36 +28,132 @@ #include "controller_interface/controller_interface.hpp" #include "control_toolbox/filters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "config/admittance_struct.h" +#include "config/admittance_controller_parameters.hpp" // kinematics plugins #include "kinematics_interface/kinematics_interface_base.hpp" #include "pluginlib/class_loader.hpp" -namespace { // Utility namespace - +namespace utility { // Numerical accuracy checks. Used as deadbands. const double ROT_AXIS_EPSILON = 1e-6; - - } // utility namespace namespace admittance_controller { + struct Transforms { + Transforms() { + base_control_.setIdentity(); + base_ft_.setIdentity(); + base_tip_.setIdentity(); + world_base_.setIdentity(); + base_sensor_.setIdentity(); + base_cog_.setIdentity(); + } + + Eigen::Matrix base_control_; + Eigen::Matrix base_ft_; + Eigen::Matrix base_tip_; + Eigen::Matrix world_base_; + Eigen::Matrix base_sensor_; + Eigen::Matrix base_cog_; + }; + + struct Rotations { + Rotations() { + base_control_.setIdentity(); + base_ft_.setIdentity(); + base_tip_.setIdentity(); + world_base_.setIdentity(); + base_world_.setIdentity(); + base_sensor_.setIdentity(); + base_cog_.setIdentity(); + world_sensor_.setIdentity(); + world_cog_.setIdentity(); + } + + Eigen::Matrix base_control_; + Eigen::Matrix base_ft_; + Eigen::Matrix base_tip_; + Eigen::Matrix world_base_; + Eigen::Matrix base_world_; + Eigen::Matrix base_sensor_; + Eigen::Matrix base_cog_; + Eigen::Matrix world_sensor_; + Eigen::Matrix world_cog_; + }; + + struct AdmittanceState { + AdmittanceState() = default; + + AdmittanceState(int num_joints) { + admittance_position_.setIdentity(); + admittance_velocity_.setZero(); + admittance_acceleration_.setZero(); + damping_.setZero(); + mass_.setOnes(); + mass_inv_.setZero(); + stiffness_.setZero(); + selected_axes_.setZero(); + joint_pos_ = Eigen::Matrix(num_joints); + joint_vel_ = Eigen::Matrix(num_joints); + joint_acc_ = Eigen::Matrix(num_joints); + feedforward_vel_.setZero(); + } + + Eigen::Matrix joint_pos_; + Eigen::Matrix joint_vel_; + Eigen::Matrix joint_acc_; + Eigen::Matrix damping_; + Eigen::Matrix mass_; + Eigen::Matrix mass_inv_; + Eigen::Matrix selected_axes_; + Eigen::Matrix stiffness_; + Eigen::Matrix admittance_acceleration_; + Eigen::Matrix admittance_velocity_; + Eigen::Matrix admittance_position_; + Eigen::Matrix wrench_base; + Eigen::Matrix feedforward_vel_; + + }; + class AdmittanceRule { public: - AdmittanceRule() { - parameters_ = ¶meters_copy; + AdmittanceRule(const std::shared_ptr &lifecycl_node) { + parameter_handler_ = std::make_shared( + lifecycl_node->get_node_parameters_interface()); + parameters_ = parameter_handler_->get_params(); + num_joints_ = parameters_.joints.size(); + admittance_state_ = AdmittanceState(num_joints_); + } controller_interface::return_type configure(const std::shared_ptr &node, int num_joint); - controller_interface::return_type reset(); + controller_interface::return_type reset(int num_joints); + + /** + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does + * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation + * are calculated without an error + * \param[in] current_joint_state + * \param[in] reference_joint_state + * \param[in] success + */ + bool get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state); /** - * Calculate 'desired joint states' based on the 'measured force' and 'reference joint state'. + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field + * members (ee_weight_, cog_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + */ + void apply_parameters_update(); + + /** + * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * 'current_joint_state'. * * \param[in] current_joint_state * \param[in] measured_wrench @@ -73,25 +168,45 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states); + /** + * Fill fields of `state_message` from current admittance controller state. + * + * \param[in] state_message + */ void get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message); - public: // admittance config parameters - std::shared_ptr parameter_handler_; - admittance_struct_parameters::admittance_struct::params *parameters_; - admittance_struct_parameters::admittance_struct::params parameters_copy; - + std::shared_ptr parameter_handler_; + admittance_controller::Params parameters_; protected: /** - * All values are in the controller frame + * Calculate the admittance rule from given the robot's current joint angles. the control frame rotation, the + * current force torque transform, the reference force torque transform, the force torque sensor frame id, + * the time delta, and the current admittance controller state. The admittance controller state input is updated + * with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin calls + * failed. + * \param[in] current_joint_positions + * \param[in] base_control + * \param[in] base_ft + * \param[in] ref_base_ft + * \param[in] ft_sensor_frame + * \param[in] dt + * \param[in] admittance_state + * \param[out] success */ - void calculate_admittance_rule( - const Eigen::Matrix &wrench, - const Eigen::Matrix &desired_vel, - const double dt - ); + bool calculate_admittance_rule(const std::vector ¤t_joint_positions, + const Eigen::Matrix &base_control, + Eigen::Matrix base_ft, + Eigen::Matrix ref_base_ft, + const std::string &ft_sensor_frame, + double dt, + AdmittanceState &admittance_state); + /** + * + * \param[in] current_joint_positions + */ void process_wrench_measurements( const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &sensor_rot, const Eigen::Matrix &cog_rot @@ -99,84 +214,71 @@ namespace admittance_controller { Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d &R) const; - void convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, + bool convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, const Eigen::Matrix &cartesian_delta, const std::string &link_name, - Eigen::Matrix &joint_delta, bool &success); - - Eigen::Matrix convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, - const std::vector &joint_delta, - const std::string &link_name, - bool &success); + Eigen::Matrix &joint_delta); - void normalize_rotation(Eigen::Matrix &R); + bool convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, + const std::vector &joint_delta, + const std::string &link_name, + Eigen::Matrix &cartesian_delta); + /** + * Normalizes given rotation matrix `R` + * \param[in] R + */ + static void normalize_rotation(Eigen::Matrix &R); - Eigen::Matrix - get_transform(const std::vector &positions, const std::string &link_name, bool external, bool &success); + /** + * Calculates the transform from the specified link to the robot's base link at the given joint positions. If + * `external` is set to true, then the link transform with will queried using TF lookup. + * \param[in] positions + * \param[in] link_name + * \param[in] external + * \param[in] transform + */ + bool get_transform(const std::vector &positions, const std::string &link_name, bool external, + Eigen::Matrix &transform); void eigen_to_msg(const Eigen::Matrix &wrench, const std::string &frame_id, geometry_msgs::msg::WrenchStamped &wrench_msg); template - void vec_to_eigen(const std::vector data, T2 &matrix); + void vec_to_eigen(const std::vector& data, T2 &matrix); + + // number of robot joint + int num_joints_; // Kinematics interface plugin loader std::shared_ptr> kinematics_loader_; std::unique_ptr kinematics_; - // number of robot joint - int num_joints_; - // buffers to pass data to kinematics interface std::vector transform_buffer_vec_; std::vector joint_buffer_vec_; std::vector cart_buffer_vec_; - std::vector jacobian_buffer_vec_; - - // admittance controller values - Eigen::Matrix admittance_acceleration_; - Eigen::Matrix admittance_velocity_; - Eigen::Matrix admittance_position_; - // transforms - Eigen::Matrix ee_transform_; - Eigen::Matrix reference_ft_transform_; - Eigen::Matrix sensor_transform_; - Eigen::Matrix control_transform_; - Eigen::Matrix cog_transform_; - Eigen::Matrix world_transform_; - Eigen::Matrix ft_transform_; - - // rotations - Eigen::Matrix ee_rot_; - Eigen::Matrix control_rot_; - Eigen::Matrix sensor_rot_; - Eigen::Matrix cog_rot_; - Eigen::Matrix world_rot_; - - // external force - Eigen::Matrix wrench_world_; + // measured wrench in ft frame Eigen::Matrix measured_wrench_; + // filtered wrench in world frame + Eigen::Matrix wrench_world_; + + // admittance controllers internal state + AdmittanceState admittance_state_; + + // transforms needed for admittance update + Transforms trans_; + Transforms trans_ref_; + + // rotations needed for admittance update + Rotations rotations_; + // position of center of gravity in cog_frame Eigen::Vector3d cog_; + // force applied to sensor due to weight of end effector Eigen::Vector3d ee_weight; - // admittance controller values in joint space - Eigen::Matrix joint_pos_; - Eigen::Matrix joint_vel_; - Eigen::Matrix joint_acc_; - - Eigen::Matrix damping_; - Eigen::Matrix mass_; - Eigen::Matrix mass_inv_; - Eigen::Matrix selected_axes_; - Eigen::Matrix stiffness_; - - // jacobian - Eigen::Matrix jacobian_; - Eigen::Matrix identity; - // ROS trajectory_msgs::msg::JointTrajectoryPoint admittance_rule_calculated_values_; control_msgs::msg::AdmittanceControllerState state_message_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 78e491fad9..407a98159e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -37,22 +37,22 @@ namespace admittance_controller { tf_listener_ = std::make_shared(*tf_buffer_); // initialize memory and values to zero (non-realtime function) - reset(); + reset(num_joints); // Load the differential IK plugin - if (!parameters_->kinematics_.plugin_name_.empty()) { + if (!parameters_.kinematics.plugin_name.empty()) { try { kinematics_loader_ = std::make_shared>( - parameters_->kinematics_.plugin_package_, "kinematics_interface::KinematicsBaseClass"); + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsBaseClass"); kinematics_ = std::unique_ptr( - kinematics_loader_->createUnmanagedInstance(parameters_->kinematics_.plugin_name_)); - if (!kinematics_->initialize(node, parameters_->kinematics_.tip_)) { + kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize(node, parameters_.kinematics.tip)) { return controller_interface::return_type::ERROR; } } catch (pluginlib::PluginlibException &ex) { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", - parameters_->kinematics_.plugin_name_.c_str(), ex.what()); + parameters_.kinematics.plugin_name.c_str(), ex.what()); return controller_interface::return_type::ERROR; } } else { @@ -64,64 +64,92 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - controller_interface::return_type AdmittanceRule::reset() { + controller_interface::return_type AdmittanceRule::reset(int num_joints) { // reset all values back to zero //allocate dynamic buffers - joint_buffer_vec_.assign(num_joints_, 0.0); + joint_buffer_vec_.assign(num_joints, 0.0); transform_buffer_vec_.assign(16, 0.0); cart_buffer_vec_.assign(6, 0.0); - jacobian_buffer_vec_.assign(6 * num_joints_, 0.0); - state_message_.error_joint_state.positions.resize(num_joints_, 0.0); - state_message_.error_joint_state.velocities.resize(num_joints_, 0.0); + // reset state message fields + state_message_.error_joint_state.positions.resize(num_joints, 0.0); + state_message_.error_joint_state.velocities.resize(num_joints, 0.0); state_message_.admittance_rule_calculated_values.positions.resize(6, 0.0); state_message_.admittance_rule_calculated_values.velocities.resize(6, 0.0); state_message_.admittance_rule_calculated_values.accelerations.resize(6, 0.0); + // reset admittance state + admittance_state_ = AdmittanceState(num_joints); - // admittance state vectors in joint space - joint_pos_ = Eigen::Matrix(num_joints_); - joint_vel_ = Eigen::Matrix(num_joints_); - joint_acc_ = Eigen::Matrix(num_joints_); - - // transforms - ee_transform_.setIdentity(); - reference_ft_transform_.setIdentity(); - sensor_transform_.setIdentity(); - control_transform_.setIdentity(); - cog_transform_.setIdentity(); - world_transform_.setIdentity(); - ft_transform_.setIdentity(); - - // admittance values - admittance_position_.setIdentity(); - admittance_velocity_.setZero(); - admittance_acceleration_.setZero(); + // reset transforms and rotations + trans_ = Transforms(); + trans_ref_ = Transforms(); + rotations_ = Rotations(); + + // reset wrenches wrench_world_.setZero(); - jacobian_ = Eigen::Matrix(6, num_joints_); - jacobian_.setZero(); - identity = Eigen::Matrix(num_joints_, num_joints_); - identity.setIdentity(); - damping_.setZero(); - mass_.setOnes(); - mass_inv_.setZero(); - stiffness_.setZero(); - selected_axes_.setZero(); - - // parameters - if (parameter_handler_->params_.enable_parameter_update_without_reactivation_) { - // point to the dynamic data - parameters_ = ¶meter_handler_->params_; - } else { - // make a copy of the data - parameters_copy = parameter_handler_->params_; - parameters_ = ¶meters_copy; - } + measured_wrench_.setZero(); return controller_interface::return_type::OK; } + void AdmittanceRule::apply_parameters_update(){ + if (parameter_handler_->is_old(parameters_)){ + parameters_ = parameter_handler_->get_params(); + } + // update param values + ee_weight[2] = -parameters_.gravity_compensation.CoG.force; + vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_); + vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass_); + vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness_); + vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes_); + + for (auto i = 0ul; i < 6; ++i) { + admittance_state_.mass_inv_[i] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping_[i] = parameters_.admittance.damping_ratio[i] * 2 * sqrt(admittance_state_.mass_[i] * admittance_state_.stiffness_[i]); + } + + } + + bool AdmittanceRule::get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state){ + // get all reference transforms + bool success = get_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, + parameters_.ft_sensor.frame.external, trans_ref_.base_ft_); + success &= get_transform(reference_joint_state.positions, parameters_.kinematics.tip, + false, trans_ref_.base_tip_); + success &= get_transform(reference_joint_state.positions, parameters_.fixed_world_frame.frame.id, + parameters_.fixed_world_frame.frame.external, trans_ref_.world_base_); + success &= get_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, + parameters_.ft_sensor.frame.external, trans_ref_.base_sensor_); + success &= get_transform(reference_joint_state.positions, parameters_.gravity_compensation.frame.id, + parameters_.gravity_compensation.frame.external, trans_ref_.base_cog_); + + // get all current transforms + success &= get_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id, + parameters_.ft_sensor.frame.external, trans_.base_ft_); + success &= get_transform(current_joint_state.positions, parameters_.kinematics.tip, + false, trans_.base_tip_); + success &= get_transform(current_joint_state.positions, parameters_.fixed_world_frame.frame.id, + parameters_.fixed_world_frame.frame.external, trans_.world_base_); + success &= get_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id, + parameters_.ft_sensor.frame.external, trans_.base_sensor_); + success &= get_transform(current_joint_state.positions, parameters_.gravity_compensation.frame.id, + parameters_.gravity_compensation.frame.external, trans_.base_cog_); + rotations_.base_control_ = trans_.base_control_.block<3,3>(0,0); + rotations_.base_ft_ = trans_.base_ft_.block<3,3>(0,0); + rotations_.base_tip_ = trans_.base_tip_.block<3,3>(0,0); + rotations_.world_base_ = trans_.world_base_.block<3,3>(0,0); + rotations_.base_world_ = rotations_.world_base_.transpose(); + rotations_.base_sensor_ = trans_.base_sensor_.block<3,3>(0,0); + rotations_.base_cog_ = trans_.base_cog_.block<3,3>(0,0); + rotations_.world_sensor_ = rotations_.world_base_ * rotations_.base_sensor_; + rotations_.world_cog_ = rotations_.world_base_ * rotations_.base_cog_; + + return success; + } + // Update from reference joint states controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, @@ -132,88 +160,28 @@ namespace admittance_controller { double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; - // update param values - ee_weight.setZero(); - ee_weight[2] = -parameters_->gravity_compensation_.CoG_.force_; - vec_to_eigen(parameters_->gravity_compensation_.CoG_.pos_, cog_); - vec_to_eigen(parameters_->admittance_.mass_, mass_); - vec_to_eigen(parameters_->admittance_.stiffness_, stiffness_); - vec_to_eigen(parameters_->admittance_.selected_axes_, selected_axes_); - - for (auto i = 0ul; i < 6; ++i) { - mass_inv_[i] = 1.0 / parameters_->admittance_.mass_[i]; - damping_[i] = parameters_->admittance_.damping_ratio_[i] * 2 * sqrt(mass_[i] * stiffness_[i]); + if (parameters_.enable_parameter_update_without_reactivation){ + apply_parameters_update(); } - // keep track of failed kinematics interface calls - bool success = true; - - // get all needed transforms - control_transform_ = get_transform(reference_joint_state.positions, parameters_->control_.frame_.id_, - parameters_->control_.frame_.external_, success); - reference_ft_transform_ = get_transform(reference_joint_state.positions, parameters_->ft_sensor_.frame_.id_, - parameters_->ft_sensor_.frame_.external_, success); - ft_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, - parameters_->ft_sensor_.frame_.external_, success); - ee_transform_ = get_transform(current_joint_state.positions, parameters_->kinematics_.tip_, - false, success); - world_transform_ = get_transform(current_joint_state.positions, parameters_->fixed_world_frame_.frame_.id_, - parameters_->fixed_world_frame_.frame_.external_, success); - sensor_transform_ = get_transform(current_joint_state.positions, parameters_->ft_sensor_.frame_.id_, - parameters_->ft_sensor_.frame_.external_, success); - cog_transform_ = get_transform(current_joint_state.positions, parameters_->gravity_compensation_.frame_.id_, - parameters_->gravity_compensation_.frame_.external_, success); - - // get all needed rotations - ee_rot_ = ee_transform_.block<3, 3>(0, 0); - control_rot_ = control_transform_.block<3, 3>(0, 0); - sensor_rot_ = sensor_transform_.block<3, 3>(0, 0); - cog_rot_ = cog_transform_.block<3, 3>(0, 0); - world_rot_ = world_transform_.block<3, 3>(0, 0); + bool success = get_all_transforms(current_joint_state, reference_joint_state); // apply filter and update wrench_world_ vector - process_wrench_measurements(measured_wrench, world_rot_ * sensor_rot_, world_rot_ * cog_rot_); + process_wrench_measurements(measured_wrench, rotations_.world_sensor_, rotations_.world_cog_); // transform wrench_world_ into base frame - Eigen::Matrix wrench_base = world_rot_.transpose() * wrench_world_; + admittance_state_.wrench_base = rotations_.base_world_ * wrench_world_; - // calculate desired cartesian velocity in control frame - Eigen::Matrix desired_vel = convert_joint_deltas_to_cartesian_deltas(current_joint_state.positions, - current_joint_state.velocities, - parameters_->ft_sensor_.frame_.id_, - success); - // set desired_vel to zero if not using feed forward input - desired_vel = desired_vel * parameters_->use_feedforward_commanded_input_; + // calculate feedforward cartesian velocity in control frame + success &= convert_joint_deltas_to_cartesian_deltas(reference_joint_state.positions, + reference_joint_state.velocities, + parameters_.ft_sensor.frame.id, + admittance_state_.feedforward_vel_); + admittance_state_.feedforward_vel_ *= parameters_.use_feedforward_commanded_input; // Compute admittance control law - calculate_admittance_rule(wrench_base, desired_vel, dt); - - // calculate drift due to integrating the joint positions - Eigen::Matrix admittance_position_base = reference_ft_transform_.block<3,1>(0, 3) + admittance_position_.block<3,1>(0, 3); - Eigen::Matrix admittance_actually_position_base = ft_transform_.block<3,1>(0,3); - Eigen::Matrix ft_rot = ft_transform_.block<3,3>(0,0); - Eigen::Matrix reference_ft_rot = reference_ft_transform_.block<3,3>(0,0); - Eigen::Matrix admittance_rot = admittance_position_.block<3,3>(0,0); - Eigen::Matrix R = admittance_rot.transpose()*(reference_ft_rot.transpose()*reference_ft_rot); - - // calculate correction term - Eigen::Matrix correction_velocity; - correction_velocity.block<3, 1>(0, 0) = (admittance_position_base - admittance_actually_position_base); - Eigen::Vector3d V = get_rotation_axis(R); - double theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); - auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) - + V[2] * (R(0, 1) - R(1, 0)); - double sign = (tmp >= 0) ? 1.0 : -1.0; - correction_velocity.block<3, 1>(0, 1) = .1*sign*theta*V; - - // calculate joint velocities that correspondence to cartesian velocities - convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_velocity_ + correction_velocity, - parameters_->ft_sensor_.frame_.id_, - joint_vel_, success); - // calculate joint velocities that correspondence to cartesian accelerations - convert_cartesian_deltas_to_joint_deltas(current_joint_state.positions, admittance_acceleration_, - parameters_->ft_sensor_.frame_.id_, - joint_acc_, success); + success &= calculate_admittance_rule(current_joint_state.positions, rotations_.base_control_, trans_.base_ft_, + trans_ref_.base_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference if (!success) { @@ -223,18 +191,16 @@ namespace admittance_controller { // update joint desired joint state for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { - joint_pos_[i] += joint_vel_[i] * dt;//- .2 * joint_pos_[i] * (1.0 / 1000.0); - desired_joint_state.positions[i] = reference_joint_state.positions[i] + joint_pos_[i]; + desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos_[i]; state_message_.error_joint_state.positions[i] = reference_joint_state.positions[i] - current_joint_state.positions[i]; } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { - desired_joint_state.velocities[i] = reference_joint_state.velocities[i] + joint_vel_[i]; - state_message_.error_joint_state.velocities[i] = - reference_joint_state.velocities[i] - current_joint_state.velocities[i]; + desired_joint_state.velocities[i] = admittance_state_.joint_vel_[i]; + state_message_.error_joint_state.velocities[i] = reference_joint_state.velocities[i] - current_joint_state.velocities[i]; } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { - desired_joint_state.accelerations[i] = joint_acc_[i]; + desired_joint_state.accelerations[i] = admittance_state_.joint_acc_[i]; } // update admittance state message @@ -244,25 +210,27 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - void AdmittanceRule::calculate_admittance_rule( - const Eigen::Matrix &wrench, - const Eigen::Matrix &desired_vel, - const double dt - ) { + bool AdmittanceRule::calculate_admittance_rule(const std::vector& current_joint_positions, + const Eigen::Matrix& base_control, + Eigen::Matrix base_ft, + Eigen::Matrix ref_base_ft, + const std::string & ft_sensor_frame, + double dt, + AdmittanceState &admittance_state) { // reshape inputs - auto admittance_velocity_flat = Eigen::Matrix(admittance_velocity_.data()); - auto wrench_flat = Eigen::Matrix(wrench.data()); - const auto desired_vel_flat = Eigen::Matrix(desired_vel.data()); + auto admittance_velocity_flat = Eigen::Matrix(admittance_state.admittance_velocity_.data()); + auto wrench_flat = Eigen::Matrix(admittance_state.wrench_base.data()); + const auto desired_vel_flat = Eigen::Matrix(admittance_state.feedforward_vel_.data()); // create stiffness matrix Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); Eigen::Matrix K_rot = Eigen::Matrix::Zero(); - K_pos.diagonal() = stiffness_.block<3,1>(0,0); - K_rot.diagonal() = stiffness_.block<3,1>(3,0); - K_pos = control_rot_.transpose()*K_pos*control_rot_; - K_rot = control_rot_.transpose()*K_rot*control_rot_; + K_pos.diagonal() = admittance_state.stiffness_.block<3,1>(0,0); + K_rot.diagonal() = admittance_state.stiffness_.block<3,1>(3,0); + K_pos = base_control.transpose()*K_pos*base_control; + K_rot = base_control.transpose()*K_rot*base_control; K.block<3,3>(0,0) = K_pos; K.block<3,3>(3,3) = K_rot; @@ -270,53 +238,79 @@ namespace admittance_controller { Eigen::Matrix D = Eigen::Matrix::Zero(); Eigen::Matrix D_pos = Eigen::Matrix::Zero(); Eigen::Matrix D_rot = Eigen::Matrix::Zero(); - D_pos.diagonal() = damping_.block<3,1>(0,0); - D_rot.diagonal() = damping_.block<3,1>(3,0); - D_pos = control_rot_.transpose()*D_pos*control_rot_; - D_rot = control_rot_.transpose()*D_rot*control_rot_; + D_pos.diagonal() = admittance_state.damping_.block<3,1>(0,0); + D_rot.diagonal() = admittance_state.damping_.block<3,1>(3,0); + D_pos = base_control.transpose()*D_pos*base_control; + D_rot = base_control.transpose()*D_rot*base_control; D.block<3,3>(0,0) = D_pos; D.block<3,3>(3,3) = D_rot; // calculate pose error Eigen::Matrix pose_error; - pose_error.block<3, 1>(0, 0) = -admittance_position_.block<3, 1>(0, 3); + pose_error.block<3, 1>(0, 0) = -admittance_state.admittance_position_.block<3, 1>(0, 3); // calculate rotation error - Eigen::Matrix R = admittance_position_.block<3, 3>(0, 0); - Eigen::Vector3d V = get_rotation_axis(R); - double theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); - // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped - auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) - + V[2] * (R(0, 1) - R(1, 0)); - double sign = (tmp >= 0) ? 1.0 : -1.0; - pose_error.block<3, 1>(3, 0) = sign * theta * V; + Eigen::Matrix admittance_rotation = admittance_state.admittance_position_.block<3, 3>(0, 0); + Eigen::Vector3d V = get_rotation_axis(admittance_rotation); + double theta = acos(std::clamp((1.0 / 2.0) * (admittance_rotation.trace() - 1), -1.0, 1.0)); + pose_error.block<3, 1>(3, 0) = theta * V; // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - Eigen::Matrix admittance_acceleration_flat = mass_inv_.cwiseProduct(wrench_flat + + Eigen::Matrix admittance_acceleration_flat = admittance_state.mass_inv_.cwiseProduct(wrench_flat + D*(desired_vel_flat-admittance_velocity_flat) + K*pose_error); // reshape admittance outputs - admittance_acceleration_ = Eigen::Matrix(admittance_acceleration_flat.data()); - admittance_velocity_ += admittance_acceleration_*dt; + admittance_state.admittance_acceleration_ = Eigen::Matrix(admittance_acceleration_flat.data()); - admittance_position_.block<3, 1>(0, 3) += admittance_velocity_.block<3,1>(0,0)*dt; + // integrate velocity + admittance_state.admittance_velocity_ += admittance_state.admittance_acceleration_*dt; + admittance_state.admittance_position_.block<3, 1>(0, 3) += admittance_state.admittance_velocity_.block<3,1>(0,0)*dt; + // integrate rotational velocity Eigen::Matrix3d skew_symmetric; - skew_symmetric << 0, -admittance_velocity_(2, 1), admittance_velocity_(1, 1), - admittance_velocity_(2, 1), 0, -admittance_velocity_(0, 1), - -admittance_velocity_(1, 1), admittance_velocity_(0, 1), 0; - - Eigen::Matrix3d R_dot = skew_symmetric * R; - R += R_dot * dt; - normalize_rotation(R); - admittance_position_.block<3, 3>(0, 0) = R; - - // store calculated values - state_message_.admittance_rule_calculated_values.positions[0] = atan2(admittance_position_(2, 1), - admittance_position_(2, 2)); - state_message_.admittance_rule_calculated_values.positions[0] = asin(admittance_position_(2, 0)); - state_message_.admittance_rule_calculated_values.positions[0] = -atan2(admittance_position_(1, 0), - admittance_position_(0, 0)); + auto rot_vel = admittance_state.admittance_velocity_; + skew_symmetric << 0, -rot_vel(2, 1), rot_vel(1, 1), + rot_vel(2, 1), 0, -rot_vel(0, 1), + -rot_vel(1, 1), rot_vel(0, 1), 0; + Eigen::Matrix3d R_dot = skew_symmetric * admittance_rotation; + admittance_rotation += R_dot * dt; + normalize_rotation(admittance_rotation); + admittance_state.admittance_position_.block<3, 3>(0, 0) = admittance_rotation; + + // update admittance joint values + + // calculate position drift due to integrating the joint positions + auto admittance_position_base = ref_base_ft.block<3,1>(0, 3) + admittance_state.admittance_position_.block<3,1>(0, 3); + Eigen::Matrix admittance_cur_position_base = base_ft.block<3,1>(0,3); + Eigen::Matrix ft_rot = base_ft.block<3,3>(0,0); + Eigen::Matrix reference_ft_rot = ref_base_ft.block<3,3>(0,0); + + // calculate rotation drift due to integrating the joint positions + Eigen::Matrix admittance_rot = admittance_state.admittance_position_.block<3,3>(0,0); + Eigen::Matrix R = admittance_rot.transpose()*(reference_ft_rot.transpose()*reference_ft_rot); + + // calculate correction term + Eigen::Matrix correction_velocity; + correction_velocity.block<3, 1>(0, 0) = (admittance_position_base - admittance_cur_position_base); + V = get_rotation_axis(R); + theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); + correction_velocity.block<3, 1>(0, 1) = .1*theta*V; + + // calculate joint velocities that correspondence to cartesian velocities + bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, admittance_state.admittance_velocity_ + correction_velocity, + ft_sensor_frame, + admittance_state.joint_vel_); + // calculate joint velocities that correspondence to cartesian accelerations + success &= convert_cartesian_deltas_to_joint_deltas(current_joint_positions, admittance_state.admittance_acceleration_, + ft_sensor_frame, + admittance_state.joint_acc_); + + for (auto i = 0ul; i < admittance_state.joint_pos_.size(); i++) { + admittance_state.joint_vel_[i] += admittance_state.joint_acc_[i]*dt; + admittance_state.joint_pos_[i] += admittance_state.joint_vel_[i]*dt; + } + + return success; } @@ -345,7 +339,7 @@ namespace admittance_controller { // apply smoothing filter for (auto i = 0; i < 6; i++) { wrench_world_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_world_(i), parameters_->ft_sensor_.filter_coefficient_); + new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); } } @@ -372,7 +366,7 @@ namespace admittance_controller { auto R13 = R_buffer[2]; auto R33 = R_buffer[8]; // degenerate: one axis rotation - if (abs(R12 + R13) < ROT_AXIS_EPSILON && R11 > 0) { + if (abs(R12 + R13) < utility::ROT_AXIS_EPSILON && R11 > 0) { v2 = 0; v3 = 0; V[i % 3] = v1; @@ -382,7 +376,7 @@ namespace admittance_controller { break; } // degenerate: two axis rotation - if (abs(R12 + R13) < ROT_AXIS_EPSILON && R11 < 0) { + if (abs(R12 + R13) < utility::ROT_AXIS_EPSILON && R11 < 0) { v1 = 0; v2 = 1; v3 = R32 / (1 - R33); @@ -398,7 +392,7 @@ namespace admittance_controller { if (!solved) { v3 = (-R(0, 1) - ((R(1, 1) - 1) * (1 - R(0, 0))) / R(1, 0)); // if v3 is zero, special case - if (abs(v3) > ROT_AXIS_EPSILON) { + if (abs(v3) > utility::ROT_AXIS_EPSILON) { v3 = v3 / (R(2, 1) - ((R(1, 1) - 1) * (R(2, 0))) / (R(1, 0))); } v2 = (1 - R(0, 0) - R(2, 0) * v3) / R(1, 0); @@ -409,7 +403,13 @@ namespace admittance_controller { } V.normalize(); - return V; + + // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped + auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) + + V[2] * (R(0, 1) - R(1, 0)); + double sign = (tmp >= 0) ? 1.0 : -1.0; + + return sign*V; } void AdmittanceRule::normalize_rotation(Eigen::Matrix3d &R) { @@ -436,14 +436,13 @@ namespace admittance_controller { } - Eigen::Matrix - AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, - bool &success) { - Eigen::Matrix transform; - if (external) { + bool + AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, Eigen::Matrix& transform) { + bool success; + if (external) { transform.setIdentity(); try { - auto transform_msg = tf_buffer_->lookupTransform(parameters_->kinematics_.base_, link_name, tf2::TimePointZero); + auto transform_msg = tf_buffer_->lookupTransform(parameters_.kinematics.base, link_name, tf2::TimePointZero); transform.block<3, 3>(0, 0) = Eigen::Matrix(tf2::transformToKDL(transform_msg).M.data); transform(0, 3) = transform_msg.transform.translation.x; transform(1, 3) = transform_msg.transform.translation.y; @@ -454,50 +453,48 @@ namespace admittance_controller { success = false; } } else { - success &= kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); + success = kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); vec_to_eigen(transform_buffer_vec_, transform); } - return transform; - + return success; } - void AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, + bool AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, const Eigen::Matrix &cartesian_delta, const std::string &link_name, - Eigen::Matrix &joint_delta, - bool &success) { + Eigen::Matrix &joint_delta) { memcpy(cart_buffer_vec_.data(), cartesian_delta.data(), 6 * sizeof(double)); - success &= kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec_, link_name, + bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec_, link_name, joint_buffer_vec_); joint_delta = Eigen::Map>(joint_buffer_vec_.data(), 6, 1); + return success; } - Eigen::Matrix - AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, + bool AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, const std::vector &joint_delta, const std::string &link_name, - bool &success) { - Eigen::Matrix cartesian_delta; - success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, link_name, + Eigen::Matrix& cartesian_delta) { + + bool success = kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, link_name, cart_buffer_vec_); vec_to_eigen(cart_buffer_vec_, cartesian_delta); - return cartesian_delta; + return success; } void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { // TODO these fields are not used - eigen_to_msg(0*wrench_world_, parameters_->control_.frame_.id_, state_message.input_wrench_command); - state_message.input_pose_command.header.frame_id = parameters_->control_.frame_.id_; + eigen_to_msg(0*wrench_world_, parameters_.control.frame.id, state_message.input_wrench_command); + state_message.input_pose_command.header.frame_id = parameters_.control.frame.id; - eigen_to_msg(wrench_world_, parameters_->fixed_world_frame_.frame_.id_, state_message.measured_wrench_filtered); - eigen_to_msg(measured_wrench_, parameters_->fixed_world_frame_.frame_.id_, state_message.measured_wrench); - eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_->control_.frame_.id_, - state_message.measured_wrench_control_frame); - eigen_to_msg(ee_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_->kinematics_.tip_, - state_message.measured_wrench_endeffector_frame); + eigen_to_msg(wrench_world_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench_filtered); + eigen_to_msg(measured_wrench_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench); +// eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_.control_.frame_.id_, +// state_message.measured_wrench_control_frame); +// eigen_to_msg(ee_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_.kinematics_.tip_, +// state_message.measured_wrench_endeffector_frame); - state_message.joint_names = parameters_->joints_; + state_message.joint_names = parameters_.joints; state_message.desired_joint_state = state_message_.desired_joint_state; state_message.actual_joint_state = state_message_.actual_joint_state; state_message.error_joint_state = state_message_.error_joint_state; @@ -520,7 +517,7 @@ namespace admittance_controller { } template - void AdmittanceRule::vec_to_eigen(const std::vector data, T2 &matrix) { + void AdmittanceRule::vec_to_eigen(const std::vector& data, T2 &matrix) { for (auto col = 0; col < matrix.cols(); col++) { for (auto row = 0; row < matrix.rows(); row++) { matrix(row, col) = data[row + col*matrix.rows()]; diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp new file mode 100644 index 0000000000..0e36f97d74 --- /dev/null +++ b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp @@ -0,0 +1,726 @@ +// auto-generated DO NOT EDIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gen_param_struct_validators { + class Result { + public: + template + Result(const std::string& format, Args... args) { + msg_ = fmt::format(format, args...); + success_ = false; + } + + Result() = default; + + operator rcl_interfaces::msg::SetParametersResult() const { + rcl_interfaces::msg::SetParametersResult result; + result.successful = success_; + result.reason = msg_; + return result; + } + + bool success() { return success_; } + + std::string error_msg() { return msg_; } + + private: + std::string msg_; + bool success_ = true; + }; + + auto OK = Result(); + using ERROR = Result; + + template + bool contains(std::vector const& vec, T const& val) { + return std::find(vec.cbegin(), vec.cend(), val) != vec.cend(); +} + +template +bool is_unique(std::vector const& x) { +auto vec = x; +std::sort(vec.begin(), vec.end()); +return std::adjacent_find(vec.cbegin(), vec.cend()) == vec.cend(); +} + +template +Result unique(rclcpp::Parameter const& parameter) { + if (!is_unique(parameter.get_value>())) { + return ERROR("Parameter '{}' must only contain unique values", + parameter.get_name()); + } + return OK; +} + +template +Result subset_of(rclcpp::Parameter const& parameter, + std::vector valid_values) { + auto const& input_values = parameter.get_value>(); + + for (auto const& value : input_values) { + if (!contains(valid_values, value)) { + return ERROR("Invalid entry '{}' for parameter '{}'. Not in set: {}", + value, parameter.get_name(), valid_values); + } + } + + return OK; +} + +template +Result fixed_size(const rclcpp::Parameter& parameter, size_t size) { + auto param_value = parameter.get_value>(); + if (param_value.size() != size) { + return ERROR("Invalid length '{}' for parameter '{}'. Required length: {}", + param_value.size(), parameter.get_name().c_str(), size); + } + return OK; +} + +template +Result size_gt(rclcpp::Parameter const& parameter, size_t size) { + auto const& values = parameter.get_value>(); + if (values.size() > size) { + return OK; + } + + return ERROR( + "Invalid length '{}' for parameter '{}'. Required greater than: {}", + values.size(), parameter.get_name(), size); +} + +template +Result size_lt(rclcpp::Parameter const& parameter, size_t size) { + auto const& values = parameter.get_value>(); + if (values.size() < size) { + return OK; + } + + return ERROR("Invalid length '{}' for parameter '{}'. Required less than: {}", + values.size(), parameter.get_name(), size); +} + +template +Result element_bounds(const rclcpp::Parameter& parameter, T lower, T upper) { + auto param_value = parameter.get_value>(); + for (auto val : param_value) { + if (val < lower || val > upper) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", + val, parameter.get_name(), lower, upper); + } + } + return OK; +} + +template +Result lower_element_bounds(const rclcpp::Parameter& parameter, T lower) { + auto param_value = parameter.get_value>(); + for (auto val : param_value) { + if (val < lower) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", + val, parameter.get_name(), lower); + } + } + return OK; +} + +template +Result upper_element_bounds(const rclcpp::Parameter& parameter, T upper) { + auto param_value = parameter.get_value>(); + for (auto val : param_value) { + if (val > upper) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", + val, parameter.get_name(), upper); + } + } + return OK; +} + +template +Result bounds(const rclcpp::Parameter& parameter, T lower, T upper) { + auto param_value = parameter.get_value(); + if (param_value < lower || param_value > upper) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", + param_value, parameter.get_name(), lower, upper); + } + return OK; +} + +template +Result lower_bounds(const rclcpp::Parameter& parameter, T lower) { + auto param_value = parameter.get_value(); + if (param_value < lower) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", + param_value, parameter.get_name(), lower); + } + return OK; +} + +template +Result upper_bounds(const rclcpp::Parameter& parameter, T upper) { + auto param_value = parameter.get_value(); + if (param_value > upper) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", + param_value, parameter.get_name(), upper); + } + return OK; +} + +template +Result one_of(rclcpp::Parameter const& parameter, std::vector collection) { + auto param_value = parameter.get_value(); + + if (std::find(collection.cbegin(), collection.cend(), param_value) == + collection.end()) { + return ERROR("The parameter '{}' with the value '{}' not in the set: {}", + parameter.get_name(), param_value, + fmt::format("{}", fmt::join(collection, ", "))); + } + + return OK; +} + +} // namespace gen_param_struct_validators + +namespace admittance_controller { + struct Params { + std::vector joints; + std::vector command_interfaces; + std::vector state_interfaces; + std::vector chainable_command_interfaces; + std::string robot_description; + bool enable_parameter_update_without_reactivation = true; + bool use_feedforward_commanded_input = true; + struct Kinematics { + std::string plugin_name; + std::string plugin_package; + std::string base; + std::string tip; + double alpha = 0.0005; + std::string group_name = ""; + } kinematics; + struct FtSensor { + std::string name; + double filter_coefficient = 0.005; + struct Frame { + std::string id; + bool external = false; + } frame; + } ft_sensor; + struct Control { + struct Frame { + std::string id; + bool external = false; + } frame; + } control; + struct FixedWorldFrame { + struct Frame { + std::string id; + bool external = false; + } frame; + } fixed_world_frame; + struct GravityCompensation { + struct Frame { + std::string id; + bool external = false; + } frame; + struct Cog { + std::vector pos; + double force = 0.0; + } CoG; + } gravity_compensation; + struct Admittance { + std::vector selected_axes; + std::vector mass; + std::vector damping_ratio; + std::vector stiffness; + } admittance; + // for detecting if the parameter struct has been updated + rclcpp::Time __stamp; + }; + + class ParamListener{ + public: + // throws rclcpp::exceptions::InvalidParameterValueException on initialization if invalid parameter are loaded + ParamListener(rclcpp::Node::SharedPtr node) + : ParamListener(node->get_node_parameters_interface()) {} + + ParamListener(rclcpp_lifecycle::LifecycleNode::SharedPtr node) + : ParamListener(node->get_node_parameters_interface()) {} + + ParamListener(const std::shared_ptr& parameters_interface){ + parameters_interface_ = parameters_interface; + declare_params(); + auto update_param_cb = [this](const std::vector ¶meters){return this->update(parameters);}; + handle_ = parameters_interface_->add_on_set_parameters_callback(update_param_cb); + clock_ = rclcpp::Clock(); + } + + Params get_params() const{ + return params_; + } + + bool is_old(Params const& other) const { + return params_.__stamp != other.__stamp; + } + + void refresh_dynamic_parameters() { + // TODO remove any destroyed dynamic parameters + + // declare any new dynamic parameters + rclcpp::Parameter param; + + } + + rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { + // Copy params_ so we only update it if all validation succeeds + Params updated_params = params_; + + for (const auto ¶m: parameters) { + if (param.get_name() == "joints") { + updated_params.joints = param.as_string_array(); + } + if (param.get_name() == "command_interfaces") { + updated_params.command_interfaces = param.as_string_array(); + } + if (param.get_name() == "state_interfaces") { + updated_params.state_interfaces = param.as_string_array(); + } + if (param.get_name() == "chainable_command_interfaces") { + updated_params.chainable_command_interfaces = param.as_string_array(); + } + if (param.get_name() == "kinematics.plugin_name") { + updated_params.kinematics.plugin_name = param.as_string(); + } + if (param.get_name() == "kinematics.plugin_package") { + updated_params.kinematics.plugin_package = param.as_string(); + } + if (param.get_name() == "kinematics.base") { + updated_params.kinematics.base = param.as_string(); + } + if (param.get_name() == "kinematics.tip") { + updated_params.kinematics.tip = param.as_string(); + } + if (param.get_name() == "kinematics.alpha") { + updated_params.kinematics.alpha = param.as_double(); + } + if (param.get_name() == "kinematics.group_name") { + updated_params.kinematics.group_name = param.as_string(); + } + if (param.get_name() == "ft_sensor.name") { + updated_params.ft_sensor.name = param.as_string(); + } + if (param.get_name() == "ft_sensor.frame.id") { + updated_params.ft_sensor.frame.id = param.as_string(); + } + if (param.get_name() == "ft_sensor.frame.external") { + updated_params.ft_sensor.frame.external = param.as_bool(); + } + if (param.get_name() == "ft_sensor.filter_coefficient") { + updated_params.ft_sensor.filter_coefficient = param.as_double(); + } + if (param.get_name() == "control.frame.id") { + updated_params.control.frame.id = param.as_string(); + } + if (param.get_name() == "control.frame.external") { + updated_params.control.frame.external = param.as_bool(); + } + if (param.get_name() == "fixed_world_frame.frame.id") { + updated_params.fixed_world_frame.frame.id = param.as_string(); + } + if (param.get_name() == "fixed_world_frame.frame.external") { + updated_params.fixed_world_frame.frame.external = param.as_bool(); + } + if (param.get_name() == "gravity_compensation.frame.id") { + updated_params.gravity_compensation.frame.id = param.as_string(); + } + if (param.get_name() == "gravity_compensation.frame.external") { + updated_params.gravity_compensation.frame.external = param.as_bool(); + } + if (param.get_name() == "gravity_compensation.CoG.pos") { + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 3); + !validation_result.success()) { + return validation_result; + } + updated_params.gravity_compensation.CoG.pos = param.as_double_array(); + } + if (param.get_name() == "gravity_compensation.CoG.force") { + updated_params.gravity_compensation.CoG.force = param.as_double(); + } + if (param.get_name() == "admittance.selected_axes") { + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + return validation_result; + } + updated_params.admittance.selected_axes = param.as_bool_array(); + } + if (param.get_name() == "admittance.mass") { + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + return validation_result; + } + if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0001, 1000000.0); + !validation_result.success()) { + return validation_result; + } + updated_params.admittance.mass = param.as_double_array(); + } + if (param.get_name() == "admittance.damping_ratio") { + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + return validation_result; + } + updated_params.admittance.damping_ratio = param.as_double_array(); + } + if (param.get_name() == "admittance.stiffness") { + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + return validation_result; + } + if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0, 100000000.0); + !validation_result.success()) { + return validation_result; + } + updated_params.admittance.stiffness = param.as_double_array(); + } + if (param.get_name() == "robot_description") { + updated_params.robot_description = param.as_string(); + } + if (param.get_name() == "enable_parameter_update_without_reactivation") { + updated_params.enable_parameter_update_without_reactivation = param.as_bool(); + } + if (param.get_name() == "use_feedforward_commanded_input") { + updated_params.use_feedforward_commanded_input = param.as_bool(); + } + } + + updated_params.__stamp = clock_.now(); + params_ = updated_params; + return gen_param_struct_validators::OK; + } + + void declare_params(){ + // declare all parameters and give default values to non-required ones + if (!parameters_interface_->has_parameter("joints")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which joints will be used by the controller"; + descriptor.read_only = true; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface_->declare_parameter("joints", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("command_interfaces")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which command interfaces to claim"; + descriptor.read_only = true; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface_->declare_parameter("command_interfaces", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("state_interfaces")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which state interfaces to claim"; + descriptor.read_only = true; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface_->declare_parameter("state_interfaces", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("chainable_command_interfaces")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which chainable interfaces to claim"; + descriptor.read_only = true; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + parameters_interface_->declare_parameter("chainable_command_interfaces", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("kinematics.plugin_name")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies which kinematics plugin to load"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("kinematics.plugin_name", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("kinematics.plugin_package")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the package to load the kinematics plugin from"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("kinematics.plugin_package", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("kinematics.base")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the base link of the robot description used by the kinematics plugin"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("kinematics.base", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("kinematics.tip")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the end effector link of the robot description used by the kinematics plugin"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("kinematics.tip", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("kinematics.alpha")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the damping coefficient for the Jacobian pseudo inverse"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.kinematics.alpha); + parameters_interface_->declare_parameter("kinematics.alpha", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("kinematics.group_name")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the group name for planning with Moveit"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.kinematics.group_name); + parameters_interface_->declare_parameter("kinematics.group_name", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("ft_sensor.name")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "name of the force torque sensor in the robot description"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("ft_sensor.name", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("ft_sensor.frame.id")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "frame of the force torque sensor"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("ft_sensor.frame.id", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("ft_sensor.frame.external")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.ft_sensor.frame.external); + parameters_interface_->declare_parameter("ft_sensor.frame.external", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("ft_sensor.filter_coefficient")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the coefficient for the sensor's exponential filter"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.ft_sensor.filter_coefficient); + parameters_interface_->declare_parameter("ft_sensor.filter_coefficient", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("control.frame.id")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "control frame used for admittance control"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("control.frame.id", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("control.frame.external")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the control frame is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.control.frame.external); + parameters_interface_->declare_parameter("control.frame.external", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("fixed_world_frame.frame.id")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "world frame, gravity points down (neg. Z) in this frame"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("fixed_world_frame.frame.id", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("fixed_world_frame.frame.external")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the world frame is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.fixed_world_frame.frame.external); + parameters_interface_->declare_parameter("fixed_world_frame.frame.external", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("gravity_compensation.frame.id")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "frame which center of gravity (CoG) is defined in"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("gravity_compensation.frame.id", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("gravity_compensation.frame.external")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.gravity_compensation.frame.external); + parameters_interface_->declare_parameter("gravity_compensation.frame.external", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("gravity_compensation.CoG.pos")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "position of the center of gravity (CoG) in its frame"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface_->declare_parameter("gravity_compensation.CoG.pos", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("gravity_compensation.CoG.force")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "weight of the end effector, e.g mass * 9.81"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.gravity_compensation.CoG.force); + parameters_interface_->declare_parameter("gravity_compensation.CoG.force", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("admittance.selected_axes")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies if the axes x, y, z, rx, ry, and rz are enabled"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_BOOL_ARRAY; + parameters_interface_->declare_parameter("admittance.selected_axes", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("admittance.mass")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface_->declare_parameter("admittance.mass", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("admittance.damping_ratio")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface_->declare_parameter("admittance.damping_ratio", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("admittance.stiffness")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + parameters_interface_->declare_parameter("admittance.stiffness", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("robot_description")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics."; + descriptor.read_only = true; + auto parameter = rclcpp::ParameterType::PARAMETER_STRING; + parameters_interface_->declare_parameter("robot_description", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("enable_parameter_update_without_reactivation")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "if enabled, parameters will be dynamically updated in the control loop"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.enable_parameter_update_without_reactivation); + parameters_interface_->declare_parameter("enable_parameter_update_without_reactivation", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("use_feedforward_commanded_input")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.use_feedforward_commanded_input); + parameters_interface_->declare_parameter("use_feedforward_commanded_input", parameter, descriptor); + } + // get parameters and fill struct fields + rclcpp::Parameter param; + param = parameters_interface_->get_parameter("joints"); + params_.joints = param.as_string_array(); + param = parameters_interface_->get_parameter("command_interfaces"); + params_.command_interfaces = param.as_string_array(); + param = parameters_interface_->get_parameter("state_interfaces"); + params_.state_interfaces = param.as_string_array(); + param = parameters_interface_->get_parameter("chainable_command_interfaces"); + params_.chainable_command_interfaces = param.as_string_array(); + param = parameters_interface_->get_parameter("kinematics.plugin_name"); + params_.kinematics.plugin_name = param.as_string(); + param = parameters_interface_->get_parameter("kinematics.plugin_package"); + params_.kinematics.plugin_package = param.as_string(); + param = parameters_interface_->get_parameter("kinematics.base"); + params_.kinematics.base = param.as_string(); + param = parameters_interface_->get_parameter("kinematics.tip"); + params_.kinematics.tip = param.as_string(); + param = parameters_interface_->get_parameter("kinematics.alpha"); + params_.kinematics.alpha = param.as_double(); + param = parameters_interface_->get_parameter("kinematics.group_name"); + params_.kinematics.group_name = param.as_string(); + param = parameters_interface_->get_parameter("ft_sensor.name"); + params_.ft_sensor.name = param.as_string(); + param = parameters_interface_->get_parameter("ft_sensor.frame.id"); + params_.ft_sensor.frame.id = param.as_string(); + param = parameters_interface_->get_parameter("ft_sensor.frame.external"); + params_.ft_sensor.frame.external = param.as_bool(); + param = parameters_interface_->get_parameter("ft_sensor.filter_coefficient"); + params_.ft_sensor.filter_coefficient = param.as_double(); + param = parameters_interface_->get_parameter("control.frame.id"); + params_.control.frame.id = param.as_string(); + param = parameters_interface_->get_parameter("control.frame.external"); + params_.control.frame.external = param.as_bool(); + param = parameters_interface_->get_parameter("fixed_world_frame.frame.id"); + params_.fixed_world_frame.frame.id = param.as_string(); + param = parameters_interface_->get_parameter("fixed_world_frame.frame.external"); + params_.fixed_world_frame.frame.external = param.as_bool(); + param = parameters_interface_->get_parameter("gravity_compensation.frame.id"); + params_.gravity_compensation.frame.id = param.as_string(); + param = parameters_interface_->get_parameter("gravity_compensation.frame.external"); + params_.gravity_compensation.frame.external = param.as_bool(); + param = parameters_interface_->get_parameter("gravity_compensation.CoG.pos"); + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 3); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'gravity_compensation.CoG.pos': " + validation_result.error_msg())); + } + params_.gravity_compensation.CoG.pos = param.as_double_array(); + param = parameters_interface_->get_parameter("gravity_compensation.CoG.force"); + params_.gravity_compensation.CoG.force = param.as_double(); + param = parameters_interface_->get_parameter("admittance.selected_axes"); + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.selected_axes': " + validation_result.error_msg())); + } + params_.admittance.selected_axes = param.as_bool_array(); + param = parameters_interface_->get_parameter("admittance.mass"); + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.mass': " + validation_result.error_msg())); + } + if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0001, 1000000.0); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.mass': " + validation_result.error_msg())); + } + params_.admittance.mass = param.as_double_array(); + param = parameters_interface_->get_parameter("admittance.damping_ratio"); + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.damping_ratio': " + validation_result.error_msg())); + } + params_.admittance.damping_ratio = param.as_double_array(); + param = parameters_interface_->get_parameter("admittance.stiffness"); + if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.stiffness': " + validation_result.error_msg())); + } + if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0, 100000000.0); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.stiffness': " + validation_result.error_msg())); + } + params_.admittance.stiffness = param.as_double_array(); + param = parameters_interface_->get_parameter("robot_description"); + params_.robot_description = param.as_string(); + param = parameters_interface_->get_parameter("enable_parameter_update_without_reactivation"); + params_.enable_parameter_update_without_reactivation = param.as_bool(); + param = parameters_interface_->get_parameter("use_feedforward_commanded_input"); + params_.use_feedforward_commanded_input = param.as_bool(); + + + params_.__stamp = clock_.now(); + } + + private: + Params params_; + rclcpp::Clock clock_; + std::shared_ptr handle_; + std::shared_ptr parameters_interface_; + }; + +} // namespace admittance_controller \ No newline at end of file diff --git a/admittance_controller/include/admittance_controller/config/admittance_struct.h b/admittance_controller/include/admittance_controller/config/admittance_struct.h deleted file mode 100644 index d42997d09e..0000000000 --- a/admittance_controller/include/admittance_controller/config/admittance_struct.h +++ /dev/null @@ -1,648 +0,0 @@ -// this is auto-generated code - -#pragma once - -#include -#include -#include -#include - - -namespace admittance_struct_parameters { - - struct admittance_struct { - std::shared_ptr handle_; - - // throws rclcpp::exceptions::InvalidParameterValueException on initialization if invalid parameter are loaded - admittance_struct(const std::shared_ptr ¶meters_interface) { - declare_params(parameters_interface); - auto update_param_cb = [this](const std::vector ¶meters) { - return this->update(parameters); - }; - handle_ = parameters_interface->add_on_set_parameters_callback(update_param_cb); - } - - struct params { - std::vector joints_; - std::vector command_interfaces_; - std::vector state_interfaces_; - std::vector chainable_command_interfaces_; - struct kinematics { - std::string plugin_name_; - std::string plugin_package_; - std::string base_; - std::string tip_; - double alpha_ = 0.0005; - std::string group_name_; - } kinematics_; - struct ft_sensor { - std::string name_; - struct frame { - std::string id_; - bool external_; - } frame_; - double filter_coefficient_ = 0.005; - } ft_sensor_; - struct control { - struct frame { - std::string id_; - bool external_; - } frame_; - } control_; - struct fixed_world_frame { - struct frame { - std::string id_; - bool external_; - } frame_; - } fixed_world_frame_; - struct gravity_compensation { - struct frame { - std::string id_; - bool external_; - } frame_; - struct CoG { - std::vector pos_; - double force_; - } CoG_; - } gravity_compensation_; - struct admittance { - std::vector selected_axes_; - std::vector mass_; - std::vector damping_ratio_; - std::vector stiffness_; - } admittance_; - std::string robot_description_; - bool enable_parameter_update_without_reactivation_ = true; - bool use_feedforward_commanded_input_ = true; - - } params_; - - rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = false; - gen_param_struct_validators::Result validation_result; - - result.reason = "success"; - for (const auto ¶m: parameters) { - if (param.get_name() == "joints") { - params_.joints_ = param.as_string_array(); - result.successful = true; - } - if (param.get_name() == "command_interfaces") { - params_.command_interfaces_ = param.as_string_array(); - result.successful = true; - } - if (param.get_name() == "state_interfaces") { - params_.state_interfaces_ = param.as_string_array(); - result.successful = true; - } - if (param.get_name() == "chainable_command_interfaces") { - params_.chainable_command_interfaces_ = param.as_string_array(); - result.successful = true; - } - if (param.get_name() == "kinematics.plugin_name") { - params_.kinematics_.plugin_name_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "kinematics.plugin_package") { - params_.kinematics_.plugin_package_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "kinematics.base") { - params_.kinematics_.base_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "kinematics.tip") { - params_.kinematics_.tip_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "kinematics.alpha") { - params_.kinematics_.alpha_ = param.as_double(); - result.successful = true; - } - if (param.get_name() == "kinematics.group_name") { - params_.kinematics_.group_name_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "ft_sensor.name") { - params_.ft_sensor_.name_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "ft_sensor.frame.id") { - params_.ft_sensor_.frame_.id_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "ft_sensor.frame.external") { - params_.ft_sensor_.frame_.external_ = param.as_bool(); - result.successful = true; - } - if (param.get_name() == "ft_sensor.filter_coefficient") { - params_.ft_sensor_.filter_coefficient_ = param.as_double(); - result.successful = true; - } - if (param.get_name() == "control.frame.id") { - params_.control_.frame_.id_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "control.frame.external") { - params_.control_.frame_.external_ = param.as_bool(); - result.successful = true; - } - if (param.get_name() == "fixed_world_frame.frame.id") { - params_.fixed_world_frame_.frame_.id_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "fixed_world_frame.frame.external") { - params_.fixed_world_frame_.frame_.external_ = param.as_bool(); - result.successful = true; - } - if (param.get_name() == "gravity_compensation.frame.id") { - params_.gravity_compensation_.frame_.id_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "gravity_compensation.frame.external") { - params_.gravity_compensation_.frame_.external_ = param.as_bool(); - result.successful = true; - } - if (param.get_name() == "gravity_compensation.CoG.pos") { - validation_result = gen_param_struct_validators::validate_double_array_len(param, 3); - if (validation_result.success()) { - params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); - result.successful = true; - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } - if (param.get_name() == "gravity_compensation.CoG.force") { - params_.gravity_compensation_.CoG_.force_ = param.as_double(); - result.successful = true; - } - if (param.get_name() == "admittance.selected_axes") { - validation_result = gen_param_struct_validators::validate_bool_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.selected_axes_ = param.as_bool_array(); - result.successful = true; - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } - if (param.get_name() == "admittance.mass") { - validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0001, 1000000.0); - if (validation_result.success()) { - validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.mass_ = param.as_double_array(); - result.successful = true; - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } - if (param.get_name() == "admittance.damping_ratio") { - validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.damping_ratio_ = param.as_double_array(); - result.successful = true; - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } - if (param.get_name() == "admittance.stiffness") { - validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0, 100000000.0); - if (validation_result.success()) { - validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.stiffness_ = param.as_double_array(); - result.successful = true; - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } else { - result.reason = validation_result.error_msg(); - result.successful = false; - } - } - if (param.get_name() == "robot_description") { - params_.robot_description_ = param.as_string(); - result.successful = true; - } - if (param.get_name() == "enable_parameter_update_without_reactivation") { - params_.enable_parameter_update_without_reactivation_ = param.as_bool(); - result.successful = true; - } - if (param.get_name() == "use_feedforward_commanded_input") { - params_.use_feedforward_commanded_input_ = param.as_bool(); - result.successful = true; - } - - } - return result; - } - - void declare_params(const std::shared_ptr ¶meters_interface) { - // declare all parameters and give default values to non-required ones - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which joints will be used by the controller"; - descriptor.read_only = true; - if (!parameters_interface->has_parameter("joints")) { - auto p_joints = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface->declare_parameter("joints", p_joints, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which command interfaces to claim"; - descriptor.read_only = true; - if (!parameters_interface->has_parameter("command_interfaces")) { - auto p_command_interfaces = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface->declare_parameter("command_interfaces", p_command_interfaces, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which state interfaces to claim"; - descriptor.read_only = true; - if (!parameters_interface->has_parameter("state_interfaces")) { - auto p_state_interfaces = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface->declare_parameter("state_interfaces", p_state_interfaces, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which chainable interfaces to claim"; - descriptor.read_only = true; - if (!parameters_interface->has_parameter("chainable_command_interfaces")) { - auto p_chainable_command_interfaces = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface->declare_parameter("chainable_command_interfaces", p_chainable_command_interfaces, - descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which kinematics plugin to load"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("kinematics.plugin_name")) { - auto p_kinematics_plugin_name = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("kinematics.plugin_name", p_kinematics_plugin_name, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the package to load the kinematics plugin from"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("kinematics.plugin_package")) { - auto p_kinematics_plugin_package = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("kinematics.plugin_package", p_kinematics_plugin_package, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the base link of the robot description used by the kinematics plugin"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("kinematics.base")) { - auto p_kinematics_base = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("kinematics.base", p_kinematics_base, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the end effector link of the robot description used by the kinematics plugin"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("kinematics.tip")) { - auto p_kinematics_tip = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("kinematics.tip", p_kinematics_tip, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the damping coefficient for the Jacobian pseudo inverse"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("kinematics.alpha")) { - auto p_kinematics_alpha = rclcpp::ParameterValue(params_.kinematics_.alpha_); - parameters_interface->declare_parameter("kinematics.alpha", p_kinematics_alpha, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the group name for planning with Moveit"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("kinematics.group_name")) { - auto p_kinematics_group_name = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("kinematics.group_name", p_kinematics_group_name, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "name of the force torque sensor in the robot description"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("ft_sensor.name")) { - auto p_ft_sensor_name = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("ft_sensor.name", p_ft_sensor_name, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "frame of the force torque sensor"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("ft_sensor.frame.id")) { - auto p_ft_sensor_frame_id = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("ft_sensor.frame.id", p_ft_sensor_frame_id, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("ft_sensor.frame.external")) { - auto p_ft_sensor_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; - parameters_interface->declare_parameter("ft_sensor.frame.external", p_ft_sensor_frame_external, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the coefficient for the sensor's exponential filter"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("ft_sensor.filter_coefficient")) { - auto p_ft_sensor_filter_coefficient = rclcpp::ParameterValue(params_.ft_sensor_.filter_coefficient_); - parameters_interface->declare_parameter("ft_sensor.filter_coefficient", p_ft_sensor_filter_coefficient, - descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "control frame used for admittance control"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("control.frame.id")) { - auto p_control_frame_id = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("control.frame.id", p_control_frame_id, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the control frame is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("control.frame.external")) { - auto p_control_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; - parameters_interface->declare_parameter("control.frame.external", p_control_frame_external, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "world frame, gravity points down (neg. Z) in this frame"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("fixed_world_frame.frame.id")) { - auto p_fixed_world_frame_frame_id = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("fixed_world_frame.frame.id", p_fixed_world_frame_frame_id, - descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the world frame is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("fixed_world_frame.frame.external")) { - auto p_fixed_world_frame_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; - parameters_interface->declare_parameter("fixed_world_frame.frame.external", - p_fixed_world_frame_frame_external, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "frame which center of gravity (CoG) is defined in"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("gravity_compensation.frame.id")) { - auto p_gravity_compensation_frame_id = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("gravity_compensation.frame.id", p_gravity_compensation_frame_id, - descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("gravity_compensation.frame.external")) { - auto p_gravity_compensation_frame_external = rclcpp::ParameterType::PARAMETER_BOOL; - parameters_interface->declare_parameter("gravity_compensation.frame.external", - p_gravity_compensation_frame_external, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "position of the center of gravity (CoG) in its frame"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("gravity_compensation.CoG.pos")) { - auto p_gravity_compensation_CoG_pos = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface->declare_parameter("gravity_compensation.CoG.pos", p_gravity_compensation_CoG_pos, - descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "weight of the end effector, e.g mass * 9.81"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("gravity_compensation.CoG.force")) { - auto p_gravity_compensation_CoG_force = rclcpp::ParameterType::PARAMETER_DOUBLE; - parameters_interface->declare_parameter("gravity_compensation.CoG.force", p_gravity_compensation_CoG_force, - descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the axes x, y, z, rx, ry, and rz are enabled"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("admittance.selected_axes")) { - auto p_admittance_selected_axes = rclcpp::ParameterType::PARAMETER_BOOL_ARRAY; - parameters_interface->declare_parameter("admittance.selected_axes", p_admittance_selected_axes, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation"; - rcl_interfaces::msg::FloatingPointRange range; - range.from_value = 0.0001; - range.to_value = 1000000.0; - descriptor.floating_point_range.push_back(range); - descriptor.read_only = false; - if (!parameters_interface->has_parameter("admittance.mass")) { - auto p_admittance_mass = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface->declare_parameter("admittance.mass", p_admittance_mass, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("admittance.damping_ratio")) { - auto p_admittance_damping_ratio = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface->declare_parameter("admittance.damping_ratio", p_admittance_damping_ratio, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation"; - rcl_interfaces::msg::FloatingPointRange range; - range.from_value = 0.0; - range.to_value = 100000000.0; - descriptor.floating_point_range.push_back(range); - descriptor.read_only = false; - if (!parameters_interface->has_parameter("admittance.stiffness")) { - auto p_admittance_stiffness = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface->declare_parameter("admittance.stiffness", p_admittance_stiffness, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics."; - descriptor.read_only = true; - if (!parameters_interface->has_parameter("robot_description")) { - auto p_robot_description = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface->declare_parameter("robot_description", p_robot_description, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "if enabled, parameters will be dynamically updated in the control loop"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("enable_parameter_update_without_reactivation")) { - auto p_enable_parameter_update_without_reactivation = rclcpp::ParameterValue( - params_.enable_parameter_update_without_reactivation_); - parameters_interface->declare_parameter("enable_parameter_update_without_reactivation", - p_enable_parameter_update_without_reactivation, descriptor); - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity"; - descriptor.read_only = false; - if (!parameters_interface->has_parameter("use_feedforward_commanded_input")) { - auto p_use_feedforward_commanded_input = rclcpp::ParameterValue(params_.use_feedforward_commanded_input_); - parameters_interface->declare_parameter("use_feedforward_commanded_input", p_use_feedforward_commanded_input, - descriptor); - } - } - - // get parameters and fill struct fields - rclcpp::Parameter param; - gen_param_struct_validators::Result validation_result; - - param = parameters_interface->get_parameter("joints"); - params_.joints_ = param.as_string_array(); - param = parameters_interface->get_parameter("command_interfaces"); - params_.command_interfaces_ = param.as_string_array(); - param = parameters_interface->get_parameter("state_interfaces"); - params_.state_interfaces_ = param.as_string_array(); - param = parameters_interface->get_parameter("chainable_command_interfaces"); - params_.chainable_command_interfaces_ = param.as_string_array(); - param = parameters_interface->get_parameter("kinematics.plugin_name"); - params_.kinematics_.plugin_name_ = param.as_string(); - param = parameters_interface->get_parameter("kinematics.plugin_package"); - params_.kinematics_.plugin_package_ = param.as_string(); - param = parameters_interface->get_parameter("kinematics.base"); - params_.kinematics_.base_ = param.as_string(); - param = parameters_interface->get_parameter("kinematics.tip"); - params_.kinematics_.tip_ = param.as_string(); - param = parameters_interface->get_parameter("kinematics.alpha"); - params_.kinematics_.alpha_ = param.as_double(); - param = parameters_interface->get_parameter("kinematics.group_name"); - params_.kinematics_.group_name_ = param.as_string(); - param = parameters_interface->get_parameter("ft_sensor.name"); - params_.ft_sensor_.name_ = param.as_string(); - param = parameters_interface->get_parameter("ft_sensor.frame.id"); - params_.ft_sensor_.frame_.id_ = param.as_string(); - param = parameters_interface->get_parameter("ft_sensor.frame.external"); - params_.ft_sensor_.frame_.external_ = param.as_bool(); - param = parameters_interface->get_parameter("ft_sensor.filter_coefficient"); - params_.ft_sensor_.filter_coefficient_ = param.as_double(); - param = parameters_interface->get_parameter("control.frame.id"); - params_.control_.frame_.id_ = param.as_string(); - param = parameters_interface->get_parameter("control.frame.external"); - params_.control_.frame_.external_ = param.as_bool(); - param = parameters_interface->get_parameter("fixed_world_frame.frame.id"); - params_.fixed_world_frame_.frame_.id_ = param.as_string(); - param = parameters_interface->get_parameter("fixed_world_frame.frame.external"); - params_.fixed_world_frame_.frame_.external_ = param.as_bool(); - param = parameters_interface->get_parameter("gravity_compensation.frame.id"); - params_.gravity_compensation_.frame_.id_ = param.as_string(); - param = parameters_interface->get_parameter("gravity_compensation.frame.external"); - params_.gravity_compensation_.frame_.external_ = param.as_bool(); - param = parameters_interface->get_parameter("gravity_compensation.CoG.pos"); - validation_result = gen_param_struct_validators::validate_double_array_len(param, 3); - if (validation_result.success()) { - params_.gravity_compensation_.CoG_.pos_ = param.as_double_array(); - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter gravity_compensation.CoG.pos: " + - validation_result.error_msg()); - } - param = parameters_interface->get_parameter("gravity_compensation.CoG.force"); - params_.gravity_compensation_.CoG_.force_ = param.as_double(); - param = parameters_interface->get_parameter("admittance.selected_axes"); - validation_result = gen_param_struct_validators::validate_bool_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.selected_axes_ = param.as_bool_array(); - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.selected_axes: " + - validation_result.error_msg()); - } - param = parameters_interface->get_parameter("admittance.mass"); - validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0001, 1000000.0); - if (validation_result.success()) { - validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.mass_ = param.as_double_array(); - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.mass: " + - validation_result.error_msg()); - } - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.mass: " + validation_result.error_msg()); - } - param = parameters_interface->get_parameter("admittance.damping_ratio"); - validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.damping_ratio_ = param.as_double_array(); - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.damping_ratio: " + - validation_result.error_msg()); - } - param = parameters_interface->get_parameter("admittance.stiffness"); - validation_result = gen_param_struct_validators::validate_double_array_bounds(param, 0.0, 100000000.0); - if (validation_result.success()) { - validation_result = gen_param_struct_validators::validate_double_array_len(param, 6); - if (validation_result.success()) { - params_.admittance_.stiffness_ = param.as_double_array(); - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.stiffness: " + - validation_result.error_msg()); - } - } else { - throw rclcpp::exceptions::InvalidParameterValueException( - "Invalid value set during initialization for parameter admittance.stiffness: " + - validation_result.error_msg()); - } - param = parameters_interface->get_parameter("robot_description"); - params_.robot_description_ = param.as_string(); - param = parameters_interface->get_parameter("enable_parameter_update_without_reactivation"); - params_.enable_parameter_update_without_reactivation_ = param.as_bool(); - param = parameters_interface->get_parameter("use_feedforward_commanded_input"); - params_.use_feedforward_commanded_input_ = param.as_bool(); - - } - }; - -} // namespace admittance_struct_parameters diff --git a/admittance_controller/include/gen_param_struct/validators.hpp b/admittance_controller/include/gen_param_struct/validators.hpp deleted file mode 100644 index 6e0d3a9e7b..0000000000 --- a/admittance_controller/include/gen_param_struct/validators.hpp +++ /dev/null @@ -1,87 +0,0 @@ - -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "variant" -#include - -namespace gen_param_struct_validators{ - - class Result { - public: - template - Result( const std::string& format, Args ... args ) - { - int size_s = std::snprintf( nullptr, 0, format.c_str(), args ... ) + 1; // Extra space for '\0' - if( size_s <= 0 ){ throw std::runtime_error( "Error during formatting." ); } - auto size = static_cast( size_s ); - std::unique_ptr buf( new char[ size ] ); - std::snprintf( buf.get(), size, format.c_str(), args ... ); - msg_ = std::string( buf.get(), buf.get() + size - 1 ); // We don't want the '\0' inside - success_ = false; - } - - Result() { - success_ = true; - } - - bool success(){ - return success_; - } - - std::string error_msg(){ - return msg_; - } - - private: - std::string msg_; - bool success_; - }; - - auto OK = Result(); - using ERROR = Result; - - Result validate_string_array_len(const rclcpp::Parameter& parameter, size_t size) { - const auto& string_array = parameter.as_string_array(); - if (string_array.size() != size) { - return ERROR( - "The length of the parameter was incorrect. Expected size is %d, actual size is %d", - size, string_array.size()); - } - return OK; - } - - Result validate_double_array_len(const rclcpp::Parameter& parameter, size_t size) { - const auto& double_array = parameter.as_double_array(); - if (double_array.size() != size) { - return ERROR( - "The length of the parameter was incorrect. Expected size is %d, actual size is %d", - size, double_array.size()); - } - return OK; - } - - Result validate_bool_array_len(const rclcpp::Parameter& parameter, size_t size) { - const auto& bool_array = parameter.as_bool_array(); - if (bool_array.size() != size) { - return ERROR( - "The length of the parameter was incorrect. Expected size is %d, actual size is %d", - size, bool_array.size()); - } - return OK; - } - - Result validate_double_array_bounds(const rclcpp::Parameter& parameter, double lower_bound, double upper_bound) { - const auto &double_array = parameter.as_double_array(); - for (auto val: double_array) { - if (val < lower_bound || val > upper_bound) { - return ERROR( - "The parameter value (%f) was outside the allowed bounds [(%f), (%f)]", - val, lower_bound, upper_bound); - } - } - return OK; - } - -} // namespace gen_param_struct - diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index dd92c8d421..555681dcb4 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -29,7 +29,9 @@ tf2_geometry_msgs tf2_ros trajectory_msgs - gen_param_struct + + + ament_cmake_gmock control_msgs diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 266feaae1b..6d53f00e8f 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,7 +16,7 @@ #include "admittance_controller/admittance_controller.hpp" -#include +#include #include #include #include @@ -35,38 +35,15 @@ constexpr size_t namespace admittance_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - - AdmittanceController::AdmittanceController() = default; - - void - AdmittanceController::joint_command_callback(const std::shared_ptr msg) { - if (controller_is_active_) { - rtBuffers.input_joint_command_.writeFromNonRT(msg); - } - } - CallbackReturn AdmittanceController::on_init() { // initialize controller config - - admittance_ = std::make_unique(); - - for (const auto &tmp: admittance_->parameters_->state_interfaces_) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); - } - for (const auto &tmp: admittance_->parameters_->command_interfaces_) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); - } - for (const auto &tmp: admittance_->parameters_->chainable_command_interfaces_) { - if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); - } else { - RCLCPP_ERROR(get_node()->get_logger(), - "chainable interface type %s is not supported. Supported types are %s and %s", - tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); - return CallbackReturn::ERROR; - } + try { + admittance_ = std::make_unique(get_node()); + num_joints_ = admittance_->parameters_.joints.size(); + } catch (const std::exception &e) { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; } @@ -78,8 +55,8 @@ namespace admittance_controller { std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); - for (const auto &interface: admittance_->parameters_->state_interfaces_) { - for (const auto &joint: admittance_->parameters_->joints_) { + for (const auto &interface: admittance_->parameters_.state_interfaces) { + for (const auto &joint: admittance_->parameters_.joints) { state_interfaces_config_names.push_back(joint + "/" + interface); } } @@ -99,8 +76,8 @@ namespace admittance_controller { std::vector command_interfaces_config_names; - for (const auto &interface: admittance_->parameters_->command_interfaces_) { - for (const auto &joint: admittance_->parameters_->joints_) { + for (const auto &interface: admittance_->parameters_.command_interfaces) { + for (const auto &joint: admittance_->parameters_.joints) { command_interfaces_config_names.push_back(joint + "/" + interface); } } @@ -110,11 +87,11 @@ namespace admittance_controller { } std::vector AdmittanceController::on_export_reference_interfaces() { - // create CommandInterface interfaces that other controllers will be able to chain to + // create CommandInterface interfaces that other controllers will be able to chain with std::vector chainable_command_interfaces; auto num_chainable_interfaces = - admittance_->parameters_->chainable_command_interfaces_.size() * admittance_->parameters_->joints_.size(); + admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); chainable_command_interfaces.reserve(num_chainable_interfaces); @@ -125,8 +102,8 @@ namespace admittance_controller { {hardware_interface::HW_IF_VELOCITY, &velocity_reference_}}; auto index = 0ul; - for (const auto &interface: admittance_->parameters_->chainable_command_interfaces_) { - for (const auto &joint: admittance_->parameters_->joints_) { + for (const auto &interface: admittance_->parameters_.chainable_command_interfaces) { + for (const auto &joint: admittance_->parameters_.joints) { chainable_interface_map[interface]->push_back(reference_interfaces_.data() + index); chainable_command_interfaces.emplace_back( hardware_interface::CommandInterface(std::string(get_node()->get_name()), @@ -136,54 +113,19 @@ namespace admittance_controller { } } - return chainable_command_interfaces; - } - - controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { - // update input reference from ros subscriber message - - joint_command_msg = *rtBuffers.input_joint_command_.readFromRT(); - - // if message exists, load values into references - if (joint_command_msg.get()) { - for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { - *position_reference_[i] = joint_command_msg->positions[i]; - } - for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { - *velocity_reference_[i] = joint_command_msg->velocities[i]; - } - } - - return controller_interface::return_type::OK; - } - - bool AdmittanceController::on_set_chained_mode(bool chained_mode) { - // this method sets the chained mode to value of argument chained_mode if true is returned. - - return true; + return chainable_command_interfaces;; } CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { - // Print output so users can be sure the interface setup is correct - - try { - admittance_->parameter_handler_ = std::make_shared( - get_node()->get_node_parameters_interface()); - admittance_->parameters_ = &admittance_->parameter_handler_->params_; - } catch (const std::exception &e) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - num_joints_ = admittance_->parameters_->joints_.size(); // print and validate interface types - for (const auto &tmp: admittance_->parameters_->state_interfaces_) { + for (const auto &tmp: admittance_->parameters_.state_interfaces) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); } - for (const auto &tmp: admittance_->parameters_->command_interfaces_) { + for (const auto &tmp: admittance_->parameters_.command_interfaces) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); } - for (const auto &tmp: admittance_->parameters_->chainable_command_interfaces_) { + for (const auto &tmp: admittance_->parameters_.chainable_command_interfaces) { if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); } else { @@ -206,29 +148,33 @@ namespace admittance_controller { }; RCLCPP_INFO( get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", - get_interface_list(admittance_->parameters_->command_interfaces_).c_str(), - get_interface_list(admittance_->parameters_->state_interfaces_).c_str()); + get_interface_list(admittance_->parameters_.command_interfaces).c_str(), + get_interface_list(admittance_->parameters_.state_interfaces).c_str()); // setup subscribers and publishers + auto joint_command_callback = [this](const std::shared_ptr msg) { + if (controller_is_active_) { + input_joint_command_.writeFromNonRT(msg); + } + }; input_joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_commands", rclcpp::SystemDefaultsQoS(), - std::bind(&AdmittanceController::joint_command_callback, this, std::placeholders::_1)); + "~/joint_commands", rclcpp::SystemDefaultsQoS(), joint_command_callback); s_publisher_ = get_node()->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); - rtBuffers.state_publisher_ = + state_publisher_ = std::make_unique>(s_publisher_); // Initialize state message - rtBuffers.state_publisher_->lock(); - rtBuffers.state_publisher_->msg_.joint_names = admittance_->parameters_->joints_; - rtBuffers.state_publisher_->msg_.actual_joint_state.positions.resize(num_joints_, 0.0); - rtBuffers.state_publisher_->msg_.desired_joint_state.positions.resize(num_joints_, 0.0); - rtBuffers.state_publisher_->msg_.error_joint_state.positions.resize(num_joints_, 0.0); - rtBuffers.state_publisher_->unlock(); + state_publisher_->lock(); + state_publisher_->msg_.joint_names = admittance_->parameters_.joints; + state_publisher_->msg_.actual_joint_state.positions.resize(num_joints_, 0.0); + state_publisher_->msg_.desired_joint_state.positions.resize(num_joints_, 0.0); + state_publisher_->msg_.error_joint_state.positions.resize(num_joints_, 0.0); + state_publisher_->unlock(); // Initialize FTS semantic semantic_component force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(admittance_->parameters_->ft_sensor_.name_)); + semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); // configure admittance rule if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) { @@ -238,13 +184,17 @@ namespace admittance_controller { return LifecycleNodeInterface::on_configure(previous_state); } - CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { // on_activate is called when the lifecycle node activates. controller_is_active_ = true; + // update parameters if any have changed + if (admittance_->parameter_handler_->is_old(admittance_->parameters_)){ + admittance_->parameters_ = admittance_->parameter_handler_->get_params(); + } + // Initialize Admittance Rule and update admittance config - admittance_->reset(); + admittance_->reset(num_joints_); // clear out vectors in case of restart joint_position_command_interface_.clear(); @@ -259,9 +209,9 @@ namespace admittance_controller { {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} }; - for (auto i = 0ul; i < admittance_->parameters_->command_interfaces_.size(); i++) { - for (auto j = 0ul; j < admittance_->parameters_->joints_.size(); j++) { - command_interface_map[admittance_->parameters_->command_interfaces_[i]]->emplace_back( + for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) { + for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { + command_interface_map[admittance_->parameters_.command_interfaces[i]]->emplace_back( command_interfaces_[i * num_joints_ + j]); } } @@ -272,9 +222,9 @@ namespace admittance_controller { {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} }; - for (auto i = 0ul; i < admittance_->parameters_->state_interfaces_.size(); i++) { - for (auto j = 0ul; j < admittance_->parameters_->joints_.size(); j++) { - state_interface_map[admittance_->parameters_->state_interfaces_[i]]->emplace_back( + for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { + for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { + state_interface_map[admittance_->parameters_.state_interfaces[i]]->emplace_back( state_interfaces_[i * num_joints_ + j]); } } @@ -282,16 +232,18 @@ namespace admittance_controller { // initialize interface of the FTS semantic semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); - // handle state after restart or initial startup - read_state_from_hardware(last_state_reference_); - - // if last_state_reference_ positions is empty, we have no information about the state, sending a command is dangerous - if (last_state_reference_.positions.empty()) { + // if joint_position_state_interface_ is empty, we have no information about the state, continuing is dangerous + if (joint_position_state_interface_.empty()) { RCLCPP_ERROR(get_node()->get_logger(), - "Failed to read from position from hardware state interface. Please ensure that a position state interface exist."); + "No joint position state interface exist."); return CallbackReturn::ERROR; } + last_state_reference_.positions.resize(joint_position_state_interface_.size()); + + // handle state after restart or initial startup + read_state_from_hardware(last_state_reference_); + // reset dynamic fields in case non-zero last_state_reference_.velocities.assign(num_joints_, 0.0); last_state_reference_.accelerations.assign(num_joints_, 0.0); @@ -302,10 +254,29 @@ namespace admittance_controller { // initialize state reference state_reference_ = last_state_reference_; state_desired_ = state_reference_; + state_current_ = state_reference_; return CallbackReturn::SUCCESS; } + controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { + // update input reference from ros subscriber message + + joint_command_msg = *input_joint_command_.readFromRT(); + + // if message exists, load values into references + if (joint_command_msg.get()) { + for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { + *position_reference_[i] = joint_command_msg->positions[i]; + } + for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { + *velocity_reference_[i] = joint_command_msg->velocities[i]; + } + } + + return controller_interface::return_type::OK; + } + controller_interface::return_type AdmittanceController::update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) { // Realtime constraints are required in this function @@ -327,7 +298,6 @@ namespace admittance_controller { // if in open loop, feed the commanded state back into the input (open_loop_buffer) last_commanded_state_.positions[i] = state_desired_.positions[i]; } - double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { joint_velocity_command_interface_[i].get().set_value(state_desired_.velocities[i]); last_commanded_state_.velocities[i] = state_desired_.velocities[i]; @@ -338,13 +308,13 @@ namespace admittance_controller { } // Publish controller state - rtBuffers.state_publisher_->lock(); - rtBuffers.state_publisher_->msg_.input_joint_command = pre_admittance_point; - rtBuffers.state_publisher_->msg_.desired_joint_state = state_desired_; - rtBuffers.state_publisher_->msg_.actual_joint_state = state_current_; - rtBuffers.state_publisher_->msg_.error_joint_state = state_error_; - admittance_->get_controller_state(rtBuffers.state_publisher_->msg_); - rtBuffers.state_publisher_->unlockAndPublish(); + state_publisher_->lock(); + state_publisher_->msg_.input_joint_command = pre_admittance_point; + state_publisher_->msg_.desired_joint_state = state_desired_; + state_publisher_->msg_.actual_joint_state = state_current_; + state_publisher_->msg_.error_joint_state = state_error_; + admittance_->get_controller_state(state_publisher_->msg_); + state_publisher_->unlockAndPublish(); return controller_interface::return_type::OK; } @@ -361,7 +331,7 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { - admittance_->reset(); + admittance_->reset(num_joints_); return CallbackReturn::SUCCESS; } @@ -370,27 +340,22 @@ namespace admittance_controller { // Fill fields of state_current argument from hardware state interfaces. If the hardware does not exist or // the values are nan, the corresponding state field will be set to empty. - state_current.positions.resize(joint_position_state_interface_.size()); - state_current.velocities.resize(joint_velocity_state_interface_.size()); - state_current.accelerations.resize(joint_acceleration_state_interface_.size()); - - // fill state message with values from hardware state interfaces // if any interface has nan values, assume state_current is the last command state - for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { + for (auto i = 0ul; i < state_current.positions.size(); i++) { state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); if (std::isnan(state_current.positions[i])) { state_current.positions = last_commanded_state_.positions; break; } } - for (auto i = 0ul; i < joint_velocity_state_interface_.size(); i++) { + for (auto i = 0ul; i < state_current.velocities.size(); i++) { state_current.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); if (std::isnan(state_current.velocities[i])) { state_current.velocities = last_commanded_state_.velocities; break; } } - for (auto i = 0ul; i < joint_acceleration_state_interface_.size(); i++) { + for (auto i = 0ul; i < state_current.accelerations.size(); i++) { state_current.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); if (std::isnan(state_current.accelerations[i])) { state_current.accelerations = last_commanded_state_.accelerations; @@ -405,17 +370,14 @@ namespace admittance_controller { // Fill fields of state_reference argument from controller references. If the interface does not exist or // the values are nan, the corresponding field will be set to empty - state_reference.positions.resize(position_reference_.size(), 0.0); - state_reference.velocities.resize(velocity_reference_.size(), 0.0); - - for (auto i = 0ul; i < position_reference_.size(); i++) { + for (auto i = 0ul; i < state_reference.positions.size(); i++) { state_reference.positions[i] = *position_reference_[i]; if (std::isnan(state_reference.positions[i])) { state_reference.positions = last_state_reference_.positions; break; } } - for (auto i = 0ul; i < velocity_reference_.size(); i++) { + for (auto i = 0ul; i < state_reference.velocities.size(); i++) { state_reference.velocities[i] = *velocity_reference_[i]; if (std::isnan(state_reference.velocities[i])) { state_reference.velocities = last_state_reference_.velocities; @@ -425,6 +387,11 @@ namespace admittance_controller { } + bool AdmittanceController::on_set_chained_mode(bool chained_mode) { + // this method sets the chained mode to value of argument chained_mode if true is returned. + return true; + } + } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp" diff --git a/admittance_controller/include/admittance_controller/config/definition.yaml b/admittance_controller/src/admittance_controller_parameters.yaml similarity index 91% rename from admittance_controller/include/admittance_controller/config/definition.yaml rename to admittance_controller/src/admittance_controller_parameters.yaml index f97ed803c2..f00963d6f8 100644 --- a/admittance_controller/include/admittance_controller/config/definition.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -1,4 +1,4 @@ -admittance_struct: +admittance_controller: joints: { type: string_array, description: "specifies which joints will be used by the controller", @@ -112,8 +112,10 @@ admittance_struct: CoG: # specifies the center of gravity of the end effector pos: { type: double_array, - fixed_size: 3, - description: "position of the center of gravity (CoG) in its frame" + description: "position of the center of gravity (CoG) in its frame", + validation: { + fixed_size<>: 3 + } } force: { type: double, @@ -125,31 +127,39 @@ admittance_struct: selected_axes: { type: bool_array, - fixed_size: 6, - description: "specifies if the axes x, y, z, rx, ry, and rz are enabled" + description: "specifies if the axes x, y, z, rx, ry, and rz are enabled", + validation: { + fixed_size<>: 6 + } } # Having ".0" at the end is MUST, otherwise there is a loading error # F = M*a + D*v + S*(x - x_d) mass: { type: double_array, - fixed_size: 6, description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation", - bounds: [ 0.0001, 1000000.0] + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0001, 1000000.0 ] + } } damping_ratio: { type: double_array, - fixed_size: 6, description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. - The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))" + The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))", + validation: { + fixed_size<>: 6 + } } stiffness: { type: double_array, - fixed_size: 6, description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", - bounds: [0.0, 100000000.0] + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0, 100000000.0 ] + } } # general settings diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/6d_robot_description.hpp new file mode 100644 index 0000000000..f67b5bd054 --- /dev/null +++ b/admittance_controller/test/6d_robot_description.hpp @@ -0,0 +1,313 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ + +#include + +namespace ros2_control_test_assets +{ +const auto valid_6d_robot_urdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ros2_control_kuka_demo_driver/KukaSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + +)"; + +const auto valid_6d_robot_srdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace ros2_control_test_assets + +#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index e89818ae99..b3c8eb09ab 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -126,59 +126,57 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) SetUpController(); - ASSERT_TRUE(controller_->admittance_->parameters_->joints_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->command_interfaces_.empty()); - + ASSERT_TRUE(controller_->admittance_ == nullptr); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(!controller_->admittance_->parameters_->joints_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->joints_.size() == joint_names_.size()); - ASSERT_TRUE(std::equal(controller_->admittance_->parameters_->joints_.begin(), controller_->admittance_->parameters_->joints_.end(), + ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); + ASSERT_TRUE(std::equal(controller_->admittance_->parameters_.joints.begin(), controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_->command_interfaces_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->command_interfaces_.size() == command_interface_types_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.command_interfaces.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.command_interfaces.size() == command_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_->command_interfaces_.begin(), controller_->admittance_->parameters_->command_interfaces_.end(), + controller_->admittance_->parameters_.command_interfaces.begin(), controller_->admittance_->parameters_.command_interfaces.end(), command_interface_types_.begin(), command_interface_types_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_->state_interfaces_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->state_interfaces_.size() == state_interface_types_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.state_interfaces.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_->state_interfaces_.begin(), controller_->admittance_->parameters_->state_interfaces_.end(), + controller_->admittance_->parameters_.state_interfaces.begin(), controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), state_interface_types_.end())); - ASSERT_EQ(controller_->admittance_->parameters_->ft_sensor_.name_, ft_sensor_name_); - ASSERT_EQ(controller_->admittance_->parameters_->kinematics_.base_, ik_base_frame_); - ASSERT_EQ(controller_->admittance_->parameters_->kinematics_.group_name_, ik_group_name_); - ASSERT_EQ(controller_->admittance_->parameters_->ft_sensor_.frame_.id_, sensor_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.kinematics.group_name, ik_group_name_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); - ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.selected_axes_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.selected_axes_.size() == admittance_selected_axes_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.admittance.selected_axes.size() == admittance_selected_axes_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_->admittance_.selected_axes_.begin(), - controller_->admittance_->parameters_->admittance_.selected_axes_.end(), + controller_->admittance_->parameters_.admittance.selected_axes.begin(), + controller_->admittance_->parameters_.admittance.selected_axes.end(), admittance_selected_axes_.begin(), admittance_selected_axes_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.mass_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.mass_.size() == admittance_mass_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.mass.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_->admittance_.mass_.begin(), controller_->admittance_->parameters_->admittance_.mass_.end(), + controller_->admittance_->parameters_.admittance.mass.begin(), controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), admittance_mass_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.damping_ratio_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.damping_ratio_.size() == admittance_damping_ratio_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.damping_ratio.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.admittance.damping_ratio.size() == admittance_damping_ratio_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_->admittance_.damping_ratio_.begin(), - controller_->admittance_->parameters_->admittance_.damping_ratio_.end(), + controller_->admittance_->parameters_.admittance.damping_ratio.begin(), + controller_->admittance_->parameters_.admittance.damping_ratio.end(), admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); - ASSERT_TRUE(!controller_->admittance_->parameters_->admittance_.stiffness_.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_->admittance_.stiffness_.size() == admittance_stiffness_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.stiffness.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.admittance.stiffness.size() == admittance_stiffness_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_->admittance_.stiffness_.begin(), - controller_->admittance_->parameters_->admittance_.stiffness_.end(), + controller_->admittance_->parameters_.admittance.stiffness.begin(), + controller_->admittance_->parameters_.admittance.stiffness.end(), admittance_stiffness_.begin(), admittance_stiffness_.end())); } @@ -265,103 +263,104 @@ TEST_F(AdmittanceControllerTest, reactivate_success) controller_interface::return_type::OK); } -TEST_F(AdmittanceControllerTest, publish_status_success) -{ - // TODO: Write also a test when Cartesian commands are used. - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - broadcast_tfs(); - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - // Check that wrench command are all zero since not used - ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); - ASSERT_EQ(msg.input_wrench_command.wrench.force.x, 0.0); - ASSERT_EQ(msg.input_wrench_command.wrench.force.y, 0.0); - ASSERT_EQ(msg.input_wrench_command.wrench.force.z, 0.0); - ASSERT_EQ(msg.input_wrench_command.wrench.torque.x, 0.0); - ASSERT_EQ(msg.input_wrench_command.wrench.torque.y, 0.0); - ASSERT_EQ(msg.input_wrench_command.wrench.torque.z, 0.0); - - // Check Cartesian command message - ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.x)); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.y)); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.z)); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.x)); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.y)); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.z)); - ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.w)); - - // Check joint command message - ASSERT_TRUE(std::equal( - msg.input_joint_command.joint_names.begin(), msg.input_joint_command.joint_names.end(), - joint_names_.begin(), joint_names_.end())); - ASSERT_EQ(msg.input_joint_command.points.size(), 1u); - ASSERT_TRUE(std::equal(msg.input_joint_command.points[0].positions.begin(), msg.input_joint_command.points[0].positions.end(), joint_state_values_.begin(), joint_state_values_.end())); - - ASSERT_TRUE(std::find_if_not(msg.input_joint_command.points[0].velocities.begin(), msg.input_joint_command.points[0].velocities.end(), - [](const auto & value){ return value == 0.0;}) == msg.input_joint_command.points[0].velocities.end()); - - // Check messages filled from AdmittanceRule.cpp - ASSERT_EQ(msg.measured_wrench.header.frame_id, sensor_frame_); - ASSERT_EQ(msg.measured_wrench.wrench.force.x, fts_state_values_[0]); - ASSERT_EQ(msg.measured_wrench.wrench.force.y, fts_state_values_[1]); - ASSERT_EQ(msg.measured_wrench.wrench.force.z, fts_state_values_[2]); - ASSERT_EQ(msg.measured_wrench.wrench.torque.x, fts_state_values_[3]); - ASSERT_EQ(msg.measured_wrench.wrench.torque.y, fts_state_values_[4]); - ASSERT_EQ(msg.measured_wrench.wrench.torque.z, fts_state_values_[5]); - - ASSERT_EQ(msg.measured_wrench_control_frame.header.frame_id, control_frame_); - ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.x, 0.0); - ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.y, 0.0); - ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.z, 0.0); - ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.x, 0.0); - ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.y, 0.0); - ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.z, 0.0); - - ASSERT_EQ(msg.desired_pose.header.frame_id, control_frame_); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.x)); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.y)); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.z)); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.x)); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.y)); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.z)); - ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.w)); - - ASSERT_EQ(msg.relative_desired_pose.header.frame_id, control_frame_); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.x)); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.y)); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.z)); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.x)); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.y)); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.z)); - ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.w)); - - // Check joint related messages - ASSERT_TRUE(std::equal( - msg.joint_names.begin(), msg.joint_names.end(), joint_names_.begin(), joint_names_.end())); - - ASSERT_TRUE(std::equal( - msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), - joint_state_values_.begin(), joint_state_values_.end())); - - ASSERT_TRUE(std::equal( - msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), - joint_state_values_.begin(), joint_state_values_.end())); - - ASSERT_TRUE(std::equal( - msg.desired_joint_state.positions.begin(), msg.desired_joint_state.positions.end(), - joint_state_values_.begin(), joint_state_values_.end())); - - ASSERT_TRUE(std::find_if_not(msg.error_joint_state.positions.begin(), msg.error_joint_state.positions.end(), - [](const auto & value){ return value == 0.0;}) == msg.error_joint_state.positions.end()); -} +// TODO (pac48) this test needs to be enabled and edited once a admittance state message definition is finalized +//TEST_F(AdmittanceControllerTest, publish_status_success) +//{ +// +// SetUpController(); +// +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// +// broadcast_tfs(); +// ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); +// +// ControllerStateMsg msg; +// subscribe_and_get_messages(msg); +// +// // Check that wrench command are all zero since not used +// ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); +// ASSERT_EQ(msg.input_wrench_command.wrench.force.x, 0.0); +// ASSERT_EQ(msg.input_wrench_command.wrench.force.y, 0.0); +// ASSERT_EQ(msg.input_wrench_command.wrench.force.z, 0.0); +// ASSERT_EQ(msg.input_wrench_command.wrench.torque.x, 0.0); +// ASSERT_EQ(msg.input_wrench_command.wrench.torque.y, 0.0); +// ASSERT_EQ(msg.input_wrench_command.wrench.torque.z, 0.0); +// +// // Check Cartesian command message +// ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.x)); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.y)); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.z)); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.x)); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.y)); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.z)); +// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.w)); +// +// // Check joint command message +// ASSERT_TRUE(std::equal( +// msg.input_joint_command.joint_names.begin(), msg.input_joint_command.joint_names.end(), +// joint_names_.begin(), joint_names_.end())); +// ASSERT_EQ(msg.input_joint_command.points.size(), 1u); +// ASSERT_TRUE(std::equal(msg.input_joint_command.points[0].positions.begin(), msg.input_joint_command.points[0].positions.end(), joint_state_values_.begin(), joint_state_values_.end())); +// +// ASSERT_TRUE(std::find_if_not(msg.input_joint_command.points[0].velocities.begin(), msg.input_joint_command.points[0].velocities.end(), +// [](const auto & value){ return value == 0.0;}) == msg.input_joint_command.points[0].velocities.end()); +// +// // Check messages filled from AdmittanceRule.cpp +// ASSERT_EQ(msg.measured_wrench.header.frame_id, sensor_frame_); +// ASSERT_EQ(msg.measured_wrench.wrench.force.x, fts_state_values_[0]); +// ASSERT_EQ(msg.measured_wrench.wrench.force.y, fts_state_values_[1]); +// ASSERT_EQ(msg.measured_wrench.wrench.force.z, fts_state_values_[2]); +// ASSERT_EQ(msg.measured_wrench.wrench.torque.x, fts_state_values_[3]); +// ASSERT_EQ(msg.measured_wrench.wrench.torque.y, fts_state_values_[4]); +// ASSERT_EQ(msg.measured_wrench.wrench.torque.z, fts_state_values_[5]); +// +// ASSERT_EQ(msg.measured_wrench_control_frame.header.frame_id, control_frame_); +// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.x, 0.0); +// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.y, 0.0); +// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.z, 0.0); +// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.x, 0.0); +// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.y, 0.0); +// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.z, 0.0); +// +// ASSERT_EQ(msg.desired_pose.header.frame_id, control_frame_); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.x)); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.y)); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.z)); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.x)); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.y)); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.z)); +// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.w)); +// +// ASSERT_EQ(msg.relative_desired_pose.header.frame_id, control_frame_); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.x)); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.y)); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.z)); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.x)); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.y)); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.z)); +// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.w)); +// +// // Check joint related messages +// ASSERT_TRUE(std::equal( +// msg.joint_names.begin(), msg.joint_names.end(), joint_names_.begin(), joint_names_.end())); +// +// ASSERT_TRUE(std::equal( +// msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), +// joint_state_values_.begin(), joint_state_values_.end())); +// +// ASSERT_TRUE(std::equal( +// msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), +// joint_state_values_.begin(), joint_state_values_.end())); +// +// ASSERT_TRUE(std::equal( +// msg.desired_joint_state.positions.begin(), msg.desired_joint_state.positions.end(), +// joint_state_values_.begin(), joint_state_values_.end())); +// +// ASSERT_TRUE(std::find_if_not(msg.error_joint_state.positions.begin(), msg.error_joint_state.positions.end(), +// [](const auto & value){ return value == 0.0;}) == msg.error_joint_state.positions.end()); +//} TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 535a0db9fc..f95dcfbc1d 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -29,7 +29,7 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "ros2_control_test_assets/6d_robot_description.hpp" +#include "6d_robot_description.hpp" #include "semantic_components/force_torque_sensor.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/utilities.hpp" @@ -74,6 +74,19 @@ class TestableAdmittanceController FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); public: + + + CallbackReturn on_init() override + { + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + + return admittance_controller::AdmittanceController::on_init(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { auto ret = @@ -107,6 +120,8 @@ class TestableAdmittanceController private: rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; rclcpp::WaitSet input_pose_command_subscriber_wait_set_; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; class AdmittanceControllerTest : public ::testing::Test @@ -163,17 +178,12 @@ class AdmittanceControllerTest : public ::testing::Test void SetUpControllerCommon(const std::string& controller_name, const rclcpp::NodeOptions& options) { -// auto options = rclcpp::NodeOptions() -// .allow_undeclared_parameters(true) -// .automatically_declare_parameters_from_overrides(true); const auto result = controller_->init(controller_name, "", options); ASSERT_EQ(result, controller_interface::return_type::OK); + controller_->export_reference_interfaces(); assign_interfaces(); - controller_->get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); - controller_->get_node()->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); - controller_->get_node()->set_parameter({"robot_description", robot_description_}); - controller_->get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + } void assign_interfaces() @@ -354,8 +364,8 @@ class AdmittanceControllerTest : public ::testing::Test const std::string ik_base_frame_ = "base_link"; const std::string ik_tip_frame_ = "tool0"; const std::string ik_group_name_ = "arm"; - const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; - const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; +// const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; +// const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; const std::string control_frame_ = "tool0"; const std::string endeffector_frame_ = "endeffector_frame"; From 382a1b60b2abe317e9591de986f273b23d1a15eb Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 27 Jul 2022 19:00:05 -0600 Subject: [PATCH 037/111] passing tests --- admittance_controller/README.md | 2 +- .../admittance_controller/admittance_rule.hpp | 8 +- .../admittance_rule_impl.hpp | 4 +- .../admittance_controller_parameters.hpp | 259 +++++++++--------- .../src/admittance_controller.cpp | 33 ++- .../src/admittance_controller_parameters.yaml | 10 +- .../test/test_admittance_controller.cpp | 46 ++-- .../test/test_admittance_controller.hpp | 33 ++- .../forward_command_controller.hpp | 2 +- ...i_interface_forward_command_controller.hpp | 2 +- .../src/forward_command_controller.cpp | 2 +- ...i_interface_forward_command_controller.cpp | 2 +- 12 files changed, 198 insertions(+), 205 deletions(-) diff --git a/admittance_controller/README.md b/admittance_controller/README.md index e182700c9f..33838bbc9e 100644 --- a/admittance_controller/README.md +++ b/admittance_controller/README.md @@ -1,7 +1,7 @@ admittance_controller ========================================== -Implemenation of admittance controllers for different input and output interface. +Implementation of admittance controllers for different input and output interface. Pluginlib-Library: admittance_controller Plugin: admittance_controller/AdmittanceController (controller_interface::ControllerInterface) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 297c3575c6..54d438627f 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -96,9 +96,9 @@ namespace admittance_controller { mass_inv_.setZero(); stiffness_.setZero(); selected_axes_.setZero(); - joint_pos_ = Eigen::Matrix(num_joints); - joint_vel_ = Eigen::Matrix(num_joints); - joint_acc_ = Eigen::Matrix(num_joints); + joint_pos_ = Eigen::Matrix::Zero(num_joints); + joint_vel_ = Eigen::Matrix::Zero(num_joints); + joint_acc_ = Eigen::Matrix::Zero(num_joints); feedforward_vel_.setZero(); } @@ -204,7 +204,7 @@ namespace admittance_controller { AdmittanceState &admittance_state); /** - * + * * \param[in] current_joint_positions */ void process_wrench_measurements( diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 407a98159e..3b22cae71f 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -87,9 +87,10 @@ namespace admittance_controller { trans_ref_ = Transforms(); rotations_ = Rotations(); - // reset wrenches + // reset forces wrench_world_.setZero(); measured_wrench_.setZero(); + ee_weight.setZero(); return controller_interface::return_type::OK; } @@ -306,7 +307,6 @@ namespace admittance_controller { admittance_state.joint_acc_); for (auto i = 0ul; i < admittance_state.joint_pos_.size(); i++) { - admittance_state.joint_vel_[i] += admittance_state.joint_acc_[i]*dt; admittance_state.joint_pos_[i] += admittance_state.joint_vel_[i]*dt; } diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp index 0e36f97d74..7c41a32b78 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp +++ b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp @@ -46,159 +46,159 @@ namespace gen_param_struct_validators { template bool contains(std::vector const& vec, T const& val) { - return std::find(vec.cbegin(), vec.cend(), val) != vec.cend(); -} - -template -bool is_unique(std::vector const& x) { -auto vec = x; -std::sort(vec.begin(), vec.end()); -return std::adjacent_find(vec.cbegin(), vec.cend()) == vec.cend(); -} - -template -Result unique(rclcpp::Parameter const& parameter) { - if (!is_unique(parameter.get_value>())) { - return ERROR("Parameter '{}' must only contain unique values", - parameter.get_name()); + return std::find(vec.cbegin(), vec.cend(), val) != vec.cend(); } - return OK; -} - -template -Result subset_of(rclcpp::Parameter const& parameter, - std::vector valid_values) { - auto const& input_values = parameter.get_value>(); - - for (auto const& value : input_values) { - if (!contains(valid_values, value)) { - return ERROR("Invalid entry '{}' for parameter '{}'. Not in set: {}", - value, parameter.get_name(), valid_values); + + template + bool is_unique(std::vector const& x) { + auto vec = x; + std::sort(vec.begin(), vec.end()); + return std::adjacent_find(vec.cbegin(), vec.cend()) == vec.cend(); + } + + template + Result unique(rclcpp::Parameter const& parameter) { + if (!is_unique(parameter.get_value>())) { + return ERROR("Parameter '{}' must only contain unique values", + parameter.get_name()); } + return OK; } - return OK; -} + template + Result subset_of(rclcpp::Parameter const& parameter, + std::vector valid_values) { + auto const& input_values = parameter.get_value>(); -template -Result fixed_size(const rclcpp::Parameter& parameter, size_t size) { - auto param_value = parameter.get_value>(); - if (param_value.size() != size) { - return ERROR("Invalid length '{}' for parameter '{}'. Required length: {}", - param_value.size(), parameter.get_name().c_str(), size); + for (auto const& value : input_values) { + if (!contains(valid_values, value)) { + return ERROR("Invalid entry '{}' for parameter '{}'. Not in set: {}", + value, parameter.get_name(), valid_values); + } + } + + return OK; } - return OK; -} -template -Result size_gt(rclcpp::Parameter const& parameter, size_t size) { - auto const& values = parameter.get_value>(); - if (values.size() > size) { + template + Result fixed_size(const rclcpp::Parameter& parameter, size_t size) { + auto param_value = parameter.get_value>(); + if (param_value.size() != size) { + return ERROR("Invalid length '{}' for parameter '{}'. Required length: {}", + param_value.size(), parameter.get_name().c_str(), size); + } return OK; } - return ERROR( - "Invalid length '{}' for parameter '{}'. Required greater than: {}", - values.size(), parameter.get_name(), size); -} + template + Result size_gt(rclcpp::Parameter const& parameter, size_t size) { + auto const& values = parameter.get_value>(); + if (values.size() > size) { + return OK; + } + + return ERROR( + "Invalid length '{}' for parameter '{}'. Required greater than: {}", + values.size(), parameter.get_name(), size); + } + + template + Result size_lt(rclcpp::Parameter const& parameter, size_t size) { + auto const& values = parameter.get_value>(); + if (values.size() < size) { + return OK; + } + + return ERROR("Invalid length '{}' for parameter '{}'. Required less than: {}", + values.size(), parameter.get_name(), size); + } -template -Result size_lt(rclcpp::Parameter const& parameter, size_t size) { - auto const& values = parameter.get_value>(); - if (values.size() < size) { + template + Result element_bounds(const rclcpp::Parameter& parameter, T lower, T upper) { + auto param_value = parameter.get_value>(); + for (auto val : param_value) { + if (val < lower || val > upper) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", + val, parameter.get_name(), lower, upper); + } + } return OK; } - return ERROR("Invalid length '{}' for parameter '{}'. Required less than: {}", - values.size(), parameter.get_name(), size); -} + template + Result lower_element_bounds(const rclcpp::Parameter& parameter, T lower) { + auto param_value = parameter.get_value>(); + for (auto val : param_value) { + if (val < lower) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", + val, parameter.get_name(), lower); + } + } + return OK; + } -template -Result element_bounds(const rclcpp::Parameter& parameter, T lower, T upper) { - auto param_value = parameter.get_value>(); - for (auto val : param_value) { - if (val < lower || val > upper) { + template + Result upper_element_bounds(const rclcpp::Parameter& parameter, T upper) { + auto param_value = parameter.get_value>(); + for (auto val : param_value) { + if (val > upper) { + return ERROR( + "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", + val, parameter.get_name(), upper); + } + } + return OK; + } + + template + Result bounds(const rclcpp::Parameter& parameter, T lower, T upper) { + auto param_value = parameter.get_value(); + if (param_value < lower || param_value > upper) { return ERROR( "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", - val, parameter.get_name(), lower, upper); + param_value, parameter.get_name(), lower, upper); } + return OK; } - return OK; -} - -template -Result lower_element_bounds(const rclcpp::Parameter& parameter, T lower) { - auto param_value = parameter.get_value>(); - for (auto val : param_value) { - if (val < lower) { + + template + Result lower_bounds(const rclcpp::Parameter& parameter, T lower) { + auto param_value = parameter.get_value(); + if (param_value < lower) { return ERROR( "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", - val, parameter.get_name(), lower); + param_value, parameter.get_name(), lower); } + return OK; } - return OK; -} - -template -Result upper_element_bounds(const rclcpp::Parameter& parameter, T upper) { - auto param_value = parameter.get_value>(); - for (auto val : param_value) { - if (val > upper) { + + template + Result upper_bounds(const rclcpp::Parameter& parameter, T upper) { + auto param_value = parameter.get_value(); + if (param_value > upper) { return ERROR( "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", - val, parameter.get_name(), upper); + param_value, parameter.get_name(), upper); } + return OK; } - return OK; -} - -template -Result bounds(const rclcpp::Parameter& parameter, T lower, T upper) { - auto param_value = parameter.get_value(); - if (param_value < lower || param_value > upper) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", - param_value, parameter.get_name(), lower, upper); - } - return OK; -} -template -Result lower_bounds(const rclcpp::Parameter& parameter, T lower) { - auto param_value = parameter.get_value(); - if (param_value < lower) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", - param_value, parameter.get_name(), lower); - } - return OK; -} + template + Result one_of(rclcpp::Parameter const& parameter, std::vector collection) { + auto param_value = parameter.get_value(); + + if (std::find(collection.cbegin(), collection.cend(), param_value) == + collection.end()) { + return ERROR("The parameter '{}' with the value '{}' not in the set: {}", + parameter.get_name(), param_value, + fmt::format("{}", fmt::join(collection, ", "))); + } -template -Result upper_bounds(const rclcpp::Parameter& parameter, T upper) { - auto param_value = parameter.get_value(); - if (param_value > upper) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", - param_value, parameter.get_name(), upper); - } - return OK; -} - -template -Result one_of(rclcpp::Parameter const& parameter, std::vector collection) { - auto param_value = parameter.get_value(); - - if (std::find(collection.cbegin(), collection.cend(), param_value) == - collection.end()) { - return ERROR("The parameter '{}' with the value '{}' not in the set: {}", - parameter.get_name(), param_value, - fmt::format("{}", fmt::join(collection, ", "))); + return OK; } - return OK; -} - } // namespace gen_param_struct_validators namespace admittance_controller { @@ -216,7 +216,6 @@ namespace admittance_controller { std::string base; std::string tip; double alpha = 0.0005; - std::string group_name = ""; } kinematics; struct FtSensor { std::string name; @@ -323,9 +322,6 @@ namespace admittance_controller { if (param.get_name() == "kinematics.alpha") { updated_params.kinematics.alpha = param.as_double(); } - if (param.get_name() == "kinematics.group_name") { - updated_params.kinematics.group_name = param.as_string(); - } if (param.get_name() == "ft_sensor.name") { updated_params.ft_sensor.name = param.as_string(); } @@ -483,13 +479,6 @@ namespace admittance_controller { auto parameter = rclcpp::ParameterValue(params_.kinematics.alpha); parameters_interface_->declare_parameter("kinematics.alpha", parameter, descriptor); } - if (!parameters_interface_->has_parameter("kinematics.group_name")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the group name for planning with Moveit"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.kinematics.group_name); - parameters_interface_->declare_parameter("kinematics.group_name", parameter, descriptor); - } if (!parameters_interface_->has_parameter("ft_sensor.name")) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "name of the force torque sensor in the robot description"; @@ -643,8 +632,6 @@ namespace admittance_controller { params_.kinematics.tip = param.as_string(); param = parameters_interface_->get_parameter("kinematics.alpha"); params_.kinematics.alpha = param.as_double(); - param = parameters_interface_->get_parameter("kinematics.group_name"); - params_.kinematics.group_name = param.as_string(); param = parameters_interface_->get_parameter("ft_sensor.name"); params_.ft_sensor.name = param.as_string(); param = parameters_interface_->get_parameter("ft_sensor.frame.id"); @@ -723,4 +710,4 @@ namespace admittance_controller { std::shared_ptr parameters_interface_; }; -} // namespace admittance_controller \ No newline at end of file +} // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6d53f00e8f..be8c64ed8d 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -52,8 +52,10 @@ namespace admittance_controller { // claim the joint + interface combinations for state interfaces from the resource manager. Finally, // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported - - std::vector state_interfaces_config_names; //= force_torque_sensor_->get_state_interface_names(); + std::vector state_interfaces_config_names; + if (!admittance_){ + return {controller_interface::interface_configuration_type::INDIVIDUAL,{}}; + } for (const auto &interface: admittance_->parameters_.state_interfaces) { for (const auto &joint: admittance_->parameters_.joints) { @@ -73,8 +75,10 @@ namespace admittance_controller { // claim the joint + interface combinations for command interfaces from the resource manager. Finally, // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported - std::vector command_interfaces_config_names; + if (!admittance_){ + return {controller_interface::interface_configuration_type::INDIVIDUAL,{}}; + } for (const auto &interface: admittance_->parameters_.command_interfaces) { for (const auto &joint: admittance_->parameters_.joints) { @@ -88,8 +92,11 @@ namespace admittance_controller { std::vector AdmittanceController::on_export_reference_interfaces() { // create CommandInterface interfaces that other controllers will be able to chain with - std::vector chainable_command_interfaces; + if (!admittance_){ + return chainable_command_interfaces; + } + auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); @@ -117,7 +124,13 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { - + try { + admittance_ = std::make_unique(get_node()); + num_joints_ = admittance_->parameters_.joints.size(); + } catch (const std::exception &e) { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } // print and validate interface types for (const auto &tmp: admittance_->parameters_.state_interfaces) { RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); @@ -341,21 +354,21 @@ namespace admittance_controller { // the values are nan, the corresponding state field will be set to empty. // if any interface has nan values, assume state_current is the last command state - for (auto i = 0ul; i < state_current.positions.size(); i++) { + for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); if (std::isnan(state_current.positions[i])) { state_current.positions = last_commanded_state_.positions; break; } } - for (auto i = 0ul; i < state_current.velocities.size(); i++) { + for (auto i = 0ul; i < joint_velocity_state_interface_.size(); i++) { state_current.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); if (std::isnan(state_current.velocities[i])) { state_current.velocities = last_commanded_state_.velocities; break; } } - for (auto i = 0ul; i < state_current.accelerations.size(); i++) { + for (auto i = 0ul; i < joint_acceleration_state_interface_.size(); i++) { state_current.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); if (std::isnan(state_current.accelerations[i])) { state_current.accelerations = last_commanded_state_.accelerations; @@ -370,14 +383,14 @@ namespace admittance_controller { // Fill fields of state_reference argument from controller references. If the interface does not exist or // the values are nan, the corresponding field will be set to empty - for (auto i = 0ul; i < state_reference.positions.size(); i++) { + for (auto i = 0ul; i < position_reference_.size(); i++) { state_reference.positions[i] = *position_reference_[i]; if (std::isnan(state_reference.positions[i])) { state_reference.positions = last_state_reference_.positions; break; } } - for (auto i = 0ul; i < state_reference.velocities.size(); i++) { + for (auto i = 0ul; i < velocity_reference_.size(); i++) { state_reference.velocities[i] = *velocity_reference_[i]; if (std::isnan(state_reference.velocities[i])) { state_reference.velocities = last_state_reference_.velocities; diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index f00963d6f8..67814cef02 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -47,12 +47,6 @@ admittance_controller: default_value: 0.0005, description: "specifies the damping coefficient for the Jacobian pseudo inverse" } - group_name: { - type: string, - default_value: "", - description: "specifies the group name for planning with Moveit" - } - ft_sensor: name: { type: string, @@ -146,7 +140,7 @@ admittance_controller: damping_ratio: { type: double_array, - description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. + description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))", validation: { fixed_size<>: 6 @@ -177,4 +171,4 @@ admittance_controller: type: bool, default_value: true, description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity" - } \ No newline at end of file + } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index b3c8eb09ab..ed91813403 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -22,22 +22,22 @@ #include -// Test on_configure returns ERROR when a parameter is invalid -TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, one_parameter_is_invalid) -{ - SetUpController(); - auto name = std::get<0>(GetParam()); - auto val = std::get<1>(GetParam()); - rclcpp::Parameter parameter(name, val); - controller_->get_node()->set_parameter(parameter); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} +//// Test on_configure returns ERROR when a parameter is invalid +//TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, one_parameter_is_invalid) +//{ +// SetUpController(); +// auto name = std::get<0>(GetParam()); +// auto val = std::get<1>(GetParam()); +// rclcpp::Parameter parameter(name, val); +// controller_->get_node()->set_parameter(parameter); +// +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +//} // Test on_configure returns ERROR when a required parameter is missing TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) { - SetUpController(GetParam()); + ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } @@ -51,22 +51,21 @@ INSTANTIATE_TEST_SUITE_P( "admittance.stiffness", "chainable_command_interfaces", "command_interfaces", - "control.frame.external", +// "control.frame.external", optional "control.frame.id", - "fixed_world_frame.frame.external", +// "fixed_world_frame.frame.external", optional "fixed_world_frame.frame.id", // "ft_sensor.filter_coefficient", optional - "ft_sensor.frame.external", +// "ft_sensor.frame.external", optional "ft_sensor.frame.id", "ft_sensor.name", // "gravity_compensation.CoG.force", optional "gravity_compensation.CoG.pos", - "gravity_compensation.frame.external", +// "gravity_compensation.frame.external", optional "gravity_compensation.frame.id", "joints", // "kinematics.alpha", optional "kinematics.base", - "kinematics.group_name", "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", @@ -124,9 +123,9 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { - SetUpController(); + auto result = SetUpController(); - ASSERT_TRUE(controller_->admittance_ == nullptr); + ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -149,7 +148,6 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); - ASSERT_EQ(controller_->admittance_->parameters_.kinematics.group_name, ik_group_name_); ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); @@ -373,8 +371,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) broadcast_tfs(); ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // After first update state values should be written to command values - ASSERT_TRUE(std::equal(joint_state_values_.begin(), joint_state_values_.end(), joint_command_values_.begin(), joint_command_values_.end())); + // After first update state, commanded position should be near the start state + for (auto i = 0ul; i < joint_state_values_.size(); i++){ + ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + } ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -387,7 +387,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) broadcast_tfs(); ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(joint_command_values_[0], 0.0, COMMON_THRESHOLD); + EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); subscribe_and_get_messages(msg); ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index f95dcfbc1d..7fd50b4ea0 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -140,8 +140,8 @@ class AdmittanceControllerTest : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); force_command_publisher_ = command_publisher_node_->create_publisher( "/test_admittance_controller/force_commands", rclcpp::SystemDefaultsQoS()); - pose_command_publisher_ = command_publisher_node_->create_publisher( - "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); +// pose_command_publisher_ = command_publisher_node_->create_publisher( +// "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); joint_command_publisher_ = command_publisher_node_->create_publisher( "/test_admittance_controller/joint_commands", rclcpp::SystemDefaultsQoS()); @@ -161,29 +161,30 @@ class AdmittanceControllerTest : public ::testing::Test protected: - void SetUpController(const std::string& controller_name, + controller_interface::return_type SetUpController(const std::string& controller_name, const std::vector & parameter_overrides) { auto options = rclcpp::NodeOptions() .allow_undeclared_parameters(false).parameter_overrides(parameter_overrides) .automatically_declare_parameters_from_overrides(true); - SetUpControllerCommon(controller_name, options); + return SetUpControllerCommon(controller_name, options); } - void SetUpController(const std::string& controller_name="test_admittance_controller") { + controller_interface::return_type SetUpController(const std::string& controller_name="test_admittance_controller") { auto options = rclcpp::NodeOptions() .allow_undeclared_parameters(false) .automatically_declare_parameters_from_overrides(true); - SetUpControllerCommon(controller_name, options); + return SetUpControllerCommon(controller_name, options); } - void SetUpControllerCommon(const std::string& controller_name, const rclcpp::NodeOptions& options) + controller_interface::return_type SetUpControllerCommon(const std::string& controller_name, const rclcpp::NodeOptions& options) { - const auto result = controller_->init(controller_name, "", options); - ASSERT_EQ(result, controller_interface::return_type::OK); + auto result = controller_->init(controller_name, "", options); controller_->export_reference_interfaces(); assign_interfaces(); + return result; + } void assign_interfaces() @@ -268,9 +269,7 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) - { - }; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr){}; auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/state", 10, subs_callback); @@ -307,7 +306,7 @@ class AdmittanceControllerTest : public ::testing::Test // wait_for_topic(force_command_publisher_->get_topic_name()); // } - wait_for_topic(pose_command_publisher_->get_topic_name()); +// wait_for_topic(pose_command_publisher_->get_topic_name()); ControllerCommandWrenchMsg force_msg; force_msg.header.frame_id = sensor_frame_; @@ -332,7 +331,7 @@ class AdmittanceControllerTest : public ::testing::Test // if (controller_->admittance_->unified_mode_) { // force_command_publisher_->publish(force_msg); // } - pose_command_publisher_->publish(pose_msg); +// pose_command_publisher_->publish(pose_msg); wait_for_topic(joint_command_publisher_->get_topic_name()); @@ -389,7 +388,7 @@ class AdmittanceControllerTest : public ::testing::Test std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr force_command_publisher_; - rclcpp::Publisher::SharedPtr pose_command_publisher_; +// rclcpp::Publisher::SharedPtr pose_command_publisher_; rclcpp::Publisher::SharedPtr joint_command_publisher_; rclcpp::Node::SharedPtr test_subscription_node_; rclcpp::Node::SharedPtr test_broadcaster_node_; @@ -415,7 +414,7 @@ class AdmittanceControllerTestParameterizedMissingParameters } protected: - void SetUpController(const std::string& remove_name) + controller_interface::return_type SetUpController(const std::string& remove_name) { std::vector parameter_overrides; for (const auto& override : overrides_){ @@ -425,7 +424,7 @@ class AdmittanceControllerTestParameterizedMissingParameters } } - AdmittanceControllerTest::SetUpController("test_admittance_controller_no_overrides", parameter_overrides); + return AdmittanceControllerTest::SetUpController("test_admittance_controller_no_overrides", parameter_overrides); } std::map overrides_; diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 46c0a00ba6..1b3c997a2c 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -112,4 +112,4 @@ namespace forward_command_controller } // namespace forward_command_controller -#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ \ No newline at end of file +#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index ff69a0e457..ddf26fe1c3 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -113,4 +113,4 @@ namespace forward_command_controller } // namespace forward_command_controller -#endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ \ No newline at end of file +#endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0c623229f6..b003782416 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -23,4 +23,4 @@ PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ChainableForwardCommandController, - controller_interface::ChainableControllerInterface) \ No newline at end of file + controller_interface::ChainableControllerInterface) diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index c09b37e96d..328907ec78 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -24,4 +24,4 @@ PLUGINLIB_EXPORT_CLASS( controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ChainableMultiInterfaceForwardCommandController, - controller_interface::ChainableControllerInterface) \ No newline at end of file + controller_interface::ChainableControllerInterface) From cbd124c6fe0bf607e977eac8d274fd2c5623396e Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 28 Jul 2022 11:35:18 -0600 Subject: [PATCH 038/111] add error checks on all public methods --- .../src/admittance_controller.cpp | 73 ++++++++++++------- 1 file changed, 48 insertions(+), 25 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index be8c64ed8d..04294b9199 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -39,11 +39,21 @@ namespace admittance_controller { // initialize controller config try { admittance_ = std::make_unique(get_node()); - num_joints_ = admittance_->parameters_.joints.size(); } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } + // allocate dynamic memory + num_joints_ = admittance_->parameters_.joints.size(); + last_state_reference_.positions.assign(num_joints_, 0.0); + last_state_reference_.velocities.assign(num_joints_, 0.0); + last_state_reference_.accelerations.assign(num_joints_, 0.0); + + last_commanded_state_ = last_state_reference_; + state_reference_ = last_state_reference_; + state_desired_ = last_state_reference_; + state_current_ = last_state_reference_; + return CallbackReturn::SUCCESS; } @@ -52,11 +62,11 @@ namespace admittance_controller { // claim the joint + interface combinations for state interfaces from the resource manager. Finally, // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported - std::vector state_interfaces_config_names; if (!admittance_){ return {controller_interface::interface_configuration_type::INDIVIDUAL,{}}; } + std::vector state_interfaces_config_names; for (const auto &interface: admittance_->parameters_.state_interfaces) { for (const auto &joint: admittance_->parameters_.joints) { state_interfaces_config_names.push_back(joint + "/" + interface); @@ -75,11 +85,11 @@ namespace admittance_controller { // claim the joint + interface combinations for command interfaces from the resource manager. Finally, // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported - std::vector command_interfaces_config_names; if (!admittance_){ return {controller_interface::interface_configuration_type::INDIVIDUAL,{}}; } + std::vector command_interfaces_config_names; for (const auto &interface: admittance_->parameters_.command_interfaces) { for (const auto &joint: admittance_->parameters_.joints) { command_interfaces_config_names.push_back(joint + "/" + interface); @@ -92,11 +102,11 @@ namespace admittance_controller { std::vector AdmittanceController::on_export_reference_interfaces() { // create CommandInterface interfaces that other controllers will be able to chain with - std::vector chainable_command_interfaces; if (!admittance_){ - return chainable_command_interfaces; + return {}; } + std::vector chainable_command_interfaces; auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); @@ -199,8 +209,11 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { // on_activate is called when the lifecycle node activates. - controller_is_active_ = true; + if (!admittance_){ + return CallbackReturn::ERROR; + } + controller_is_active_ = true; // update parameters if any have changed if (admittance_->parameter_handler_->is_old(admittance_->parameters_)){ admittance_->parameters_ = admittance_->parameter_handler_->get_params(); @@ -252,20 +265,18 @@ namespace admittance_controller { return CallbackReturn::ERROR; } - last_state_reference_.positions.resize(joint_position_state_interface_.size()); - // handle state after restart or initial startup - read_state_from_hardware(last_state_reference_); + read_state_from_hardware(state_reference_); // reset dynamic fields in case non-zero - last_state_reference_.velocities.assign(num_joints_, 0.0); - last_state_reference_.accelerations.assign(num_joints_, 0.0); + state_reference_.velocities.assign(num_joints_, 0.0); + state_reference_.accelerations.assign(num_joints_, 0.0); // initialize last_commanded_state_ last read state reference - last_commanded_state_ = last_state_reference_; + last_commanded_state_ = state_reference_; + last_state_reference_ = state_reference_; // initialize state reference - state_reference_ = last_state_reference_; state_desired_ = state_reference_; state_current_ = state_reference_; @@ -274,6 +285,9 @@ namespace admittance_controller { controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { // update input reference from ros subscriber message + if (!admittance_){ + return controller_interface::return_type::ERROR; + } joint_command_msg = *input_joint_command_.readFromRT(); @@ -293,6 +307,9 @@ namespace admittance_controller { controller_interface::return_type AdmittanceController::update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) { // Realtime constraints are required in this function + if (!admittance_){ + return controller_interface::return_type::ERROR; + } // update input reference from chainable interfaces read_state_reference_interfaces(state_reference_); @@ -344,34 +361,37 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { + if (!admittance_){ + return CallbackReturn::ERROR; + } admittance_->reset(num_joints_); return CallbackReturn::SUCCESS; } void AdmittanceController::read_state_from_hardware( - trajectory_msgs::msg::JointTrajectoryPoint &state_current) { - // Fill fields of state_current argument from hardware state interfaces. If the hardware does not exist or + trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { + // Fill fields of state_reference argument from hardware state interfaces. If the hardware does not exist or // the values are nan, the corresponding state field will be set to empty. - // if any interface has nan values, assume state_current is the last command state + // if any interface has nan values, assume state_reference is the last command state for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { - state_current.positions[i] = joint_position_state_interface_[i].get().get_value(); - if (std::isnan(state_current.positions[i])) { - state_current.positions = last_commanded_state_.positions; + state_reference.positions[i] = joint_position_state_interface_[i].get().get_value(); + if (std::isnan(state_reference.positions[i])) { + state_reference.positions = last_commanded_state_.positions; break; } } for (auto i = 0ul; i < joint_velocity_state_interface_.size(); i++) { - state_current.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); - if (std::isnan(state_current.velocities[i])) { - state_current.velocities = last_commanded_state_.velocities; + state_reference.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); + if (std::isnan(state_reference.velocities[i])) { + state_reference.velocities = last_commanded_state_.velocities; break; } } for (auto i = 0ul; i < joint_acceleration_state_interface_.size(); i++) { - state_current.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); - if (std::isnan(state_current.accelerations[i])) { - state_current.accelerations = last_commanded_state_.accelerations; + state_reference.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); + if (std::isnan(state_reference.accelerations[i])) { + state_reference.accelerations = last_commanded_state_.accelerations; break; } } @@ -390,6 +410,8 @@ namespace admittance_controller { break; } } + last_state_reference_.positions = state_reference.positions; + for (auto i = 0ul; i < velocity_reference_.size(); i++) { state_reference.velocities[i] = *velocity_reference_[i]; if (std::isnan(state_reference.velocities[i])) { @@ -397,6 +419,7 @@ namespace admittance_controller { break; } } + last_state_reference_.velocities = state_reference.velocities; } From 967f64f3b265b506770fcc7937652e2674e96ff0 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 28 Jul 2022 17:08:15 -0600 Subject: [PATCH 039/111] code clean up --- .../admittance_controller.hpp | 5 +--- .../admittance_controller/admittance_rule.hpp | 26 +++++++++++++------ .../admittance_rule_impl.hpp | 8 +++--- .../src/admittance_controller.cpp | 7 ----- 4 files changed, 22 insertions(+), 24 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 3cf868142e..82599d5e4d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -90,7 +90,7 @@ namespace admittance_controller { bool on_set_chained_mode(bool chained_mode) override; - int num_joints_; + int num_joints_ = 0; // vectors to hold joint hardware interfaces std::vector> joint_position_command_interface_; @@ -122,9 +122,6 @@ namespace admittance_controller { realtime_tools::RealtimeBuffer> input_joint_command_; std::unique_ptr> state_publisher_; - // controller running state - bool controller_is_active_; - const std::set allowed_state_interface_types_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 54d438627f..c29f9d754b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -204,16 +204,30 @@ namespace admittance_controller { AdmittanceState &admittance_state); /** - * - * \param[in] current_joint_positions - */ + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement. The `wrench_world_` + * estimate includes gravity compensation + * \param[in] measured_wrench + * \param[in] sensor_rot + * \param[in] cog_rot + */ void process_wrench_measurements( const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &sensor_rot, const Eigen::Matrix &cog_rot ); + /** + * Returns the axis of rotation of a given rotation matrix + * \param[in] R + * \param[out] rotation_axis + */ Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d &R) const; + /** + * Normalizes given rotation matrix `R` + * \param[in] R + */ + static void normalize_rotation(Eigen::Matrix &R); + bool convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, const Eigen::Matrix &cartesian_delta, const std::string &link_name, @@ -223,11 +237,7 @@ namespace admittance_controller { const std::vector &joint_delta, const std::string &link_name, Eigen::Matrix &cartesian_delta); - /** - * Normalizes given rotation matrix `R` - * \param[in] R - */ - static void normalize_rotation(Eigen::Matrix &R); + /** * Calculates the transform from the specified link to the robot's base link at the given joint positions. If diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 3b22cae71f..888cc1f80b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -65,7 +65,7 @@ namespace admittance_controller { } controller_interface::return_type AdmittanceRule::reset(int num_joints) { - // reset all values back to zero + // reset all values back to default //allocate dynamic buffers joint_buffer_vec_.assign(num_joints, 0.0); @@ -258,8 +258,8 @@ namespace admittance_controller { // Compute admittance control law: F = M*a + D*v + S*(x - x_d) Eigen::Matrix admittance_acceleration_flat = admittance_state.mass_inv_.cwiseProduct(wrench_flat + - D*(desired_vel_flat-admittance_velocity_flat) + - K*pose_error); + D*(desired_vel_flat-admittance_velocity_flat) + K*pose_error); + // reshape admittance outputs admittance_state.admittance_acceleration_ = Eigen::Matrix(admittance_acceleration_flat.data()); @@ -278,8 +278,6 @@ namespace admittance_controller { normalize_rotation(admittance_rotation); admittance_state.admittance_position_.block<3, 3>(0, 0) = admittance_rotation; - // update admittance joint values - // calculate position drift due to integrating the joint positions auto admittance_position_base = ref_base_ft.block<3,1>(0, 3) + admittance_state.admittance_position_.block<3,1>(0, 3); Eigen::Matrix admittance_cur_position_base = base_ft.block<3,1>(0,3); diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 04294b9199..e0688c833b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -29,9 +29,6 @@ #include "rcutils/logging_macros.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -constexpr size_t - ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops - namespace admittance_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -176,9 +173,7 @@ namespace admittance_controller { // setup subscribers and publishers auto joint_command_callback = [this](const std::shared_ptr msg) { - if (controller_is_active_) { input_joint_command_.writeFromNonRT(msg); - } }; input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_commands", rclcpp::SystemDefaultsQoS(), joint_command_callback); @@ -213,7 +208,6 @@ namespace admittance_controller { return CallbackReturn::ERROR; } - controller_is_active_ = true; // update parameters if any have changed if (admittance_->parameter_handler_->is_old(admittance_->parameters_)){ admittance_->parameters_ = admittance_->parameter_handler_->get_params(); @@ -350,7 +344,6 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { - controller_is_active_ = false; force_torque_sensor_->release_interfaces(); return LifecycleNodeInterface::on_deactivate(previous_state); From bcac12992b7fe09968a1aa0b685393bd8161c0fd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 31 Jul 2022 14:44:05 -0600 Subject: [PATCH 040/111] added parameters for drift correction --- .../admittance_controller/admittance_rule.hpp | 10 +- .../admittance_rule_impl.hpp | 187 ++++++++++-------- .../admittance_controller_parameters.hpp | 57 ++++-- .../src/admittance_controller_parameters.yaml | 25 ++- .../src/joint_trajectory_controller.cpp | 2 + 5 files changed, 172 insertions(+), 109 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index c29f9d754b..9e3548208e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -37,7 +37,7 @@ namespace utility { // Numerical accuracy checks. Used as deadbands. - const double ROT_AXIS_EPSILON = 1e-6; + const double ROT_AXIS_EPSILON = 1e-12; } // utility namespace namespace admittance_controller { @@ -99,7 +99,6 @@ namespace admittance_controller { joint_pos_ = Eigen::Matrix::Zero(num_joints); joint_vel_ = Eigen::Matrix::Zero(num_joints); joint_acc_ = Eigen::Matrix::Zero(num_joints); - feedforward_vel_.setZero(); } Eigen::Matrix joint_pos_; @@ -110,12 +109,10 @@ namespace admittance_controller { Eigen::Matrix mass_inv_; Eigen::Matrix selected_axes_; Eigen::Matrix stiffness_; + Eigen::Matrix wrench_base; Eigen::Matrix admittance_acceleration_; Eigen::Matrix admittance_velocity_; Eigen::Matrix admittance_position_; - Eigen::Matrix wrench_base; - Eigen::Matrix feedforward_vel_; - }; class AdmittanceRule { @@ -256,6 +253,9 @@ namespace admittance_controller { template void vec_to_eigen(const std::vector& data, T2 &matrix); + template + void eigen_to_vec(const T2 &matrix, std::vector& data); + // number of robot joint int num_joints_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 888cc1f80b..ac88be6902 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -92,11 +92,14 @@ namespace admittance_controller { measured_wrench_.setZero(); ee_weight.setZero(); + // load/initialize Eigen types from parameters + apply_parameters_update(); + return controller_interface::return_type::OK; } - void AdmittanceRule::apply_parameters_update(){ - if (parameter_handler_->is_old(parameters_)){ + void AdmittanceRule::apply_parameters_update() { + if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); } // update param values @@ -108,16 +111,17 @@ namespace admittance_controller { for (auto i = 0ul; i < 6; ++i) { admittance_state_.mass_inv_[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping_[i] = parameters_.admittance.damping_ratio[i] * 2 * sqrt(admittance_state_.mass_[i] * admittance_state_.stiffness_[i]); + admittance_state_.damping_[i] = parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass_[i] * admittance_state_.stiffness_[i]); } } - bool AdmittanceRule::get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state){ + bool AdmittanceRule::get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state) { // get all reference transforms bool success = get_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_ref_.base_ft_); + parameters_.ft_sensor.frame.external, trans_ref_.base_ft_); success &= get_transform(reference_joint_state.positions, parameters_.kinematics.tip, false, trans_ref_.base_tip_); success &= get_transform(reference_joint_state.positions, parameters_.fixed_world_frame.frame.id, @@ -126,25 +130,30 @@ namespace admittance_controller { parameters_.ft_sensor.frame.external, trans_ref_.base_sensor_); success &= get_transform(reference_joint_state.positions, parameters_.gravity_compensation.frame.id, parameters_.gravity_compensation.frame.external, trans_ref_.base_cog_); + success &= get_transform(reference_joint_state.positions, parameters_.control.frame.id, + parameters_.control.frame.external, trans_ref_.base_control_); // get all current transforms success &= get_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_.base_ft_); + parameters_.ft_sensor.frame.external, trans_.base_ft_); success &= get_transform(current_joint_state.positions, parameters_.kinematics.tip, - false, trans_.base_tip_); + false, trans_.base_tip_); success &= get_transform(current_joint_state.positions, parameters_.fixed_world_frame.frame.id, - parameters_.fixed_world_frame.frame.external, trans_.world_base_); + parameters_.fixed_world_frame.frame.external, trans_.world_base_); success &= get_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_.base_sensor_); + parameters_.ft_sensor.frame.external, trans_.base_sensor_); success &= get_transform(current_joint_state.positions, parameters_.gravity_compensation.frame.id, - parameters_.gravity_compensation.frame.external, trans_.base_cog_); - rotations_.base_control_ = trans_.base_control_.block<3,3>(0,0); - rotations_.base_ft_ = trans_.base_ft_.block<3,3>(0,0); - rotations_.base_tip_ = trans_.base_tip_.block<3,3>(0,0); - rotations_.world_base_ = trans_.world_base_.block<3,3>(0,0); + parameters_.gravity_compensation.frame.external, trans_.base_cog_); + success &= get_transform(current_joint_state.positions, parameters_.control.frame.id, + parameters_.control.frame.external, trans_.base_control_); + + rotations_.base_control_ = trans_ref_.base_control_.block<3, 3>(0, 0); + rotations_.base_ft_ = trans_.base_ft_.block<3, 3>(0, 0); + rotations_.base_tip_ = trans_.base_tip_.block<3, 3>(0, 0); + rotations_.world_base_ = trans_.world_base_.block<3, 3>(0, 0); rotations_.base_world_ = rotations_.world_base_.transpose(); - rotations_.base_sensor_ = trans_.base_sensor_.block<3,3>(0,0); - rotations_.base_cog_ = trans_.base_cog_.block<3,3>(0,0); + rotations_.base_sensor_ = trans_.base_sensor_.block<3, 3>(0, 0); + rotations_.base_cog_ = trans_.base_cog_.block<3, 3>(0, 0); rotations_.world_sensor_ = rotations_.world_base_ * rotations_.base_sensor_; rotations_.world_cog_ = rotations_.world_base_ * rotations_.base_cog_; @@ -161,7 +170,7 @@ namespace admittance_controller { double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; - if (parameters_.enable_parameter_update_without_reactivation){ + if (parameters_.enable_parameter_update_without_reactivation) { apply_parameters_update(); } @@ -173,16 +182,9 @@ namespace admittance_controller { // transform wrench_world_ into base frame admittance_state_.wrench_base = rotations_.base_world_ * wrench_world_; - // calculate feedforward cartesian velocity in control frame - success &= convert_joint_deltas_to_cartesian_deltas(reference_joint_state.positions, - reference_joint_state.velocities, - parameters_.ft_sensor.frame.id, - admittance_state_.feedforward_vel_); - admittance_state_.feedforward_vel_ *= parameters_.use_feedforward_commanded_input; - // Compute admittance control law success &= calculate_admittance_rule(current_joint_state.positions, rotations_.base_control_, trans_.base_ft_, - trans_ref_.base_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); + trans_ref_.base_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference if (!success) { @@ -198,7 +200,8 @@ namespace admittance_controller { } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { desired_joint_state.velocities[i] = admittance_state_.joint_vel_[i]; - state_message_.error_joint_state.velocities[i] = reference_joint_state.velocities[i] - current_joint_state.velocities[i]; + state_message_.error_joint_state.velocities[i] = + reference_joint_state.velocities[i] - current_joint_state.velocities[i]; } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { desired_joint_state.accelerations[i] = admittance_state_.joint_acc_[i]; @@ -211,61 +214,70 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - bool AdmittanceRule::calculate_admittance_rule(const std::vector& current_joint_positions, - const Eigen::Matrix& base_control, - Eigen::Matrix base_ft, - Eigen::Matrix ref_base_ft, - const std::string & ft_sensor_frame, + bool AdmittanceRule::calculate_admittance_rule(const std::vector ¤t_joint_positions, + const Eigen::Matrix &base_control, + Eigen::Matrix base_ft, + Eigen::Matrix ref_base_ft, + const std::string &ft_sensor_frame, double dt, AdmittanceState &admittance_state) { // reshape inputs auto admittance_velocity_flat = Eigen::Matrix(admittance_state.admittance_velocity_.data()); auto wrench_flat = Eigen::Matrix(admittance_state.wrench_base.data()); - const auto desired_vel_flat = Eigen::Matrix(admittance_state.feedforward_vel_.data()); // create stiffness matrix Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); Eigen::Matrix K_rot = Eigen::Matrix::Zero(); - K_pos.diagonal() = admittance_state.stiffness_.block<3,1>(0,0); - K_rot.diagonal() = admittance_state.stiffness_.block<3,1>(3,0); - K_pos = base_control.transpose()*K_pos*base_control; - K_rot = base_control.transpose()*K_rot*base_control; - K.block<3,3>(0,0) = K_pos; - K.block<3,3>(3,3) = K_rot; + K_pos.diagonal() = admittance_state.stiffness_.block<3, 1>(0, 0); + K_rot.diagonal() = admittance_state.stiffness_.block<3, 1>(3, 0); + K_pos = base_control * K_pos * base_control.transpose(); + K_rot = base_control * K_rot * base_control.transpose(); + K.block<3, 3>(0, 0) = K_pos; + K.block<3, 3>(3, 3) = K_rot; // create damping matrix Eigen::Matrix D = Eigen::Matrix::Zero(); Eigen::Matrix D_pos = Eigen::Matrix::Zero(); Eigen::Matrix D_rot = Eigen::Matrix::Zero(); - D_pos.diagonal() = admittance_state.damping_.block<3,1>(0,0); - D_rot.diagonal() = admittance_state.damping_.block<3,1>(3,0); - D_pos = base_control.transpose()*D_pos*base_control; - D_rot = base_control.transpose()*D_rot*base_control; - D.block<3,3>(0,0) = D_pos; - D.block<3,3>(3,3) = D_rot; + D_pos.diagonal() = admittance_state.damping_.block<3, 1>(0, 0); + D_rot.diagonal() = admittance_state.damping_.block<3, 1>(3, 0); + D_pos = base_control * D_pos * base_control.transpose(); + D_rot = base_control * D_rot * base_control.transpose(); + D.block<3, 3>(0, 0) = D_pos; + D.block<3, 3>(3, 3) = D_rot; // calculate pose error Eigen::Matrix pose_error; - pose_error.block<3, 1>(0, 0) = -admittance_state.admittance_position_.block<3, 1>(0, 3); + pose_error.block<3, 1>(0, 0) = admittance_state.admittance_position_.block<3, 1>(0, 3); // calculate rotation error Eigen::Matrix admittance_rotation = admittance_state.admittance_position_.block<3, 3>(0, 0); Eigen::Vector3d V = get_rotation_axis(admittance_rotation); double theta = acos(std::clamp((1.0 / 2.0) * (admittance_rotation.trace() - 1), -1.0, 1.0)); - pose_error.block<3, 1>(3, 0) = theta * V; + pose_error.block<3, 1>(3, 0) = -theta * V; // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - Eigen::Matrix admittance_acceleration_flat = admittance_state.mass_inv_.cwiseProduct(wrench_flat + - D*(desired_vel_flat-admittance_velocity_flat) + K*pose_error); - - // reshape admittance outputs - admittance_state.admittance_acceleration_ = Eigen::Matrix(admittance_acceleration_flat.data()); + Eigen::Matrix admittance_acceleration_flat = admittance_state.mass_inv_.cwiseProduct(wrench_flat - + D * + admittance_velocity_flat - + K * pose_error); + + // project acceleration into controllable subspace + auto tmp = Eigen::Matrix(admittance_acceleration_flat.data()); + bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, tmp, + ft_sensor_frame, + admittance_state.joint_acc_); + eigen_to_vec(admittance_state.joint_acc_, joint_buffer_vec_); + success &= convert_joint_deltas_to_cartesian_deltas(current_joint_positions, joint_buffer_vec_, + ft_sensor_frame, + admittance_state.admittance_acceleration_); // integrate velocity - admittance_state.admittance_velocity_ += admittance_state.admittance_acceleration_*dt; - admittance_state.admittance_position_.block<3, 1>(0, 3) += admittance_state.admittance_velocity_.block<3,1>(0,0)*dt; + admittance_state.admittance_velocity_ += admittance_state.admittance_acceleration_ * dt; + admittance_state.admittance_position_.block<3, 1>(0, 3) += + admittance_state.admittance_velocity_.block<3, 1>(0, 0) * dt; // integrate rotational velocity Eigen::Matrix3d skew_symmetric; @@ -279,33 +291,29 @@ namespace admittance_controller { admittance_state.admittance_position_.block<3, 3>(0, 0) = admittance_rotation; // calculate position drift due to integrating the joint positions - auto admittance_position_base = ref_base_ft.block<3,1>(0, 3) + admittance_state.admittance_position_.block<3,1>(0, 3); - Eigen::Matrix admittance_cur_position_base = base_ft.block<3,1>(0,3); - Eigen::Matrix ft_rot = base_ft.block<3,3>(0,0); - Eigen::Matrix reference_ft_rot = ref_base_ft.block<3,3>(0,0); + auto admittance_position_base = + ref_base_ft.block<3, 1>(0, 3) + admittance_state.admittance_position_.block<3, 1>(0, 3); + Eigen::Matrix admittance_cur_position_base = base_ft.block<3, 1>(0, 3); + Eigen::Matrix ft_rot = base_ft.block<3, 3>(0, 0); + Eigen::Matrix reference_ft_rot = ref_base_ft.block<3, 3>(0, 0); // calculate rotation drift due to integrating the joint positions - Eigen::Matrix admittance_rot = admittance_state.admittance_position_.block<3,3>(0,0); - Eigen::Matrix R = admittance_rot.transpose()*(reference_ft_rot.transpose()*reference_ft_rot); + Eigen::Matrix R = (admittance_rotation*reference_ft_rot).transpose()*ft_rot; // calculate correction term - Eigen::Matrix correction_velocity; - correction_velocity.block<3, 1>(0, 0) = (admittance_position_base - admittance_cur_position_base); + Eigen::Matrix correction_term; + correction_term.block<3, 1>(0, 0) = parameters_.admittance.linear_correction*(admittance_position_base - admittance_cur_position_base); V = get_rotation_axis(R); theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); - correction_velocity.block<3, 1>(0, 1) = .1*theta*V; + correction_term.block<3, 1>(0, 1) = parameters_.admittance.angular_correction*theta * V; - // calculate joint velocities that correspondence to cartesian velocities - bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, admittance_state.admittance_velocity_ + correction_velocity, - ft_sensor_frame, - admittance_state.joint_vel_); // calculate joint velocities that correspondence to cartesian accelerations - success &= convert_cartesian_deltas_to_joint_deltas(current_joint_positions, admittance_state.admittance_acceleration_, - ft_sensor_frame, - admittance_state.joint_acc_); + success &= convert_cartesian_deltas_to_joint_deltas(current_joint_positions, admittance_state.admittance_velocity_ + correction_term, + ft_sensor_frame, + admittance_state.joint_vel_); for (auto i = 0ul; i < admittance_state.joint_pos_.size(); i++) { - admittance_state.joint_pos_[i] += admittance_state.joint_vel_[i]*dt; + admittance_state.joint_pos_[i] += admittance_state.joint_vel_[i] * dt; } return success; @@ -407,7 +415,7 @@ namespace admittance_controller { + V[2] * (R(0, 1) - R(1, 0)); double sign = (tmp >= 0) ? 1.0 : -1.0; - return sign*V; + return sign * V; } void AdmittanceRule::normalize_rotation(Eigen::Matrix3d &R) { @@ -435,9 +443,10 @@ namespace admittance_controller { } bool - AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, Eigen::Matrix& transform) { + AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, + Eigen::Matrix &transform) { bool success; - if (external) { + if (external) { transform.setIdentity(); try { auto transform_msg = tf_buffer_->lookupTransform(parameters_.kinematics.base, link_name, tf2::TimePointZero); @@ -451,7 +460,7 @@ namespace admittance_controller { success = false; } } else { - success = kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); + success = kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); vec_to_eigen(transform_buffer_vec_, transform); } return success; @@ -463,18 +472,18 @@ namespace admittance_controller { Eigen::Matrix &joint_delta) { memcpy(cart_buffer_vec_.data(), cartesian_delta.data(), 6 * sizeof(double)); bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec_, link_name, - joint_buffer_vec_); - joint_delta = Eigen::Map>(joint_buffer_vec_.data(), 6, 1); + joint_buffer_vec_); + joint_delta = Eigen::Map>(joint_buffer_vec_.data(), num_joints_, 1); return success; } bool AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, - const std::vector &joint_delta, - const std::string &link_name, - Eigen::Matrix& cartesian_delta) { + const std::vector &joint_delta, + const std::string &link_name, + Eigen::Matrix &cartesian_delta) { bool success = kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, link_name, - cart_buffer_vec_); + cart_buffer_vec_); vec_to_eigen(cart_buffer_vec_, cartesian_delta); return success; } @@ -482,7 +491,7 @@ namespace admittance_controller { void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { // TODO these fields are not used - eigen_to_msg(0*wrench_world_, parameters_.control.frame.id, state_message.input_wrench_command); + eigen_to_msg(0 * wrench_world_, parameters_.control.frame.id, state_message.input_wrench_command); state_message.input_pose_command.header.frame_id = parameters_.control.frame.id; eigen_to_msg(wrench_world_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench_filtered); @@ -515,10 +524,20 @@ namespace admittance_controller { } template - void AdmittanceRule::vec_to_eigen(const std::vector& data, T2 &matrix) { + void AdmittanceRule::vec_to_eigen(const std::vector &data, T2 &matrix) { + for (auto col = 0; col < matrix.cols(); col++) { + for (auto row = 0; row < matrix.rows(); row++) { + matrix(row, col) = data[row + col * matrix.rows()]; + } + } + + } + + template + void AdmittanceRule::eigen_to_vec(const T2 &matrix, std::vector &data) { for (auto col = 0; col < matrix.cols(); col++) { for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col*matrix.rows()]; + data[row + col * matrix.rows()] = matrix(row, col); } } diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp index 7c41a32b78..65bf09a619 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp +++ b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp @@ -209,7 +209,6 @@ namespace admittance_controller { std::vector chainable_command_interfaces; std::string robot_description; bool enable_parameter_update_without_reactivation = true; - bool use_feedforward_commanded_input = true; struct Kinematics { std::string plugin_name; std::string plugin_package; @@ -252,6 +251,8 @@ namespace admittance_controller { std::vector mass; std::vector damping_ratio; std::vector stiffness; + double linear_correction = 1.0; + double angular_correction = 0.1; } admittance; // for detecting if the parameter struct has been updated rclcpp::Time __stamp; @@ -398,15 +399,26 @@ namespace admittance_controller { } updated_params.admittance.stiffness = param.as_double_array(); } + if (param.get_name() == "admittance.linear_correction") { + if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); + !validation_result.success()) { + return validation_result; + } + updated_params.admittance.linear_correction = param.as_double(); + } + if (param.get_name() == "admittance.angular_correction") { + if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); + !validation_result.success()) { + return validation_result; + } + updated_params.admittance.angular_correction = param.as_double(); + } if (param.get_name() == "robot_description") { updated_params.robot_description = param.as_string(); } if (param.get_name() == "enable_parameter_update_without_reactivation") { updated_params.enable_parameter_update_without_reactivation = param.as_bool(); } - if (param.get_name() == "use_feedforward_commanded_input") { - updated_params.use_feedforward_commanded_input = param.as_bool(); - } } updated_params.__stamp = clock_.now(); @@ -591,6 +603,20 @@ namespace admittance_controller { auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; parameters_interface_->declare_parameter("admittance.stiffness", parameter, descriptor); } + if (!parameters_interface_->has_parameter("admittance.linear_correction")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the coefficient used in the linear drift correction calculation"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.admittance.linear_correction); + parameters_interface_->declare_parameter("admittance.linear_correction", parameter, descriptor); + } + if (!parameters_interface_->has_parameter("admittance.angular_correction")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "specifies the coefficient used in the angular drift correction calculation"; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(params_.admittance.angular_correction); + parameters_interface_->declare_parameter("admittance.angular_correction", parameter, descriptor); + } if (!parameters_interface_->has_parameter("robot_description")) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics."; @@ -605,13 +631,6 @@ namespace admittance_controller { auto parameter = rclcpp::ParameterValue(params_.enable_parameter_update_without_reactivation); parameters_interface_->declare_parameter("enable_parameter_update_without_reactivation", parameter, descriptor); } - if (!parameters_interface_->has_parameter("use_feedforward_commanded_input")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.use_feedforward_commanded_input); - parameters_interface_->declare_parameter("use_feedforward_commanded_input", parameter, descriptor); - } // get parameters and fill struct fields rclcpp::Parameter param; param = parameters_interface_->get_parameter("joints"); @@ -692,12 +711,22 @@ namespace admittance_controller { throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.stiffness': " + validation_result.error_msg())); } params_.admittance.stiffness = param.as_double_array(); + param = parameters_interface_->get_parameter("admittance.linear_correction"); + if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.linear_correction': " + validation_result.error_msg())); + } + params_.admittance.linear_correction = param.as_double(); + param = parameters_interface_->get_parameter("admittance.angular_correction"); + if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); + !validation_result.success()) { + throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.angular_correction': " + validation_result.error_msg())); + } + params_.admittance.angular_correction = param.as_double(); param = parameters_interface_->get_parameter("robot_description"); params_.robot_description = param.as_string(); param = parameters_interface_->get_parameter("enable_parameter_update_without_reactivation"); params_.enable_parameter_update_without_reactivation = param.as_bool(); - param = parameters_interface_->get_parameter("use_feedforward_commanded_input"); - params_.use_feedforward_commanded_input = param.as_bool(); params_.__stamp = clock_.now(); @@ -710,4 +739,4 @@ namespace admittance_controller { std::shared_ptr parameters_interface_; }; -} // namespace admittance_controller +} // namespace admittance_controller \ No newline at end of file diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 67814cef02..b4feb68ac6 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -156,6 +156,24 @@ admittance_controller: } } + linear_correction: { + type: double, + description: "specifies the coefficient used in the linear drift correction calculation", + default_value: 1.0, + validation: { + lower_bounds<>: [ 0.0] + } + } + + angular_correction: { + type: double, + description: "specifies the coefficient used in the angular drift correction calculation", + default_value: 0.1, + validation: { + lower_bounds<>: [ 0.0] + } + } + # general settings robot_description: { type: string, @@ -166,9 +184,4 @@ admittance_controller: type: bool, default_value: true, description: "if enabled, parameters will be dynamically updated in the control loop" - } - use_feedforward_commanded_input: { - type: bool, - default_value: true, - description: "if enabled, the velocity commanded to the admittance controller is added to its calculated admittance velocity" - } + } \ No newline at end of file diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index eaed8a398c..c95f762f19 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -60,6 +60,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = auto_declare>("state_interfaces", state_interface_types_); + command_joint_names_ = + auto_declare>("command_joints", command_interface_types_); allow_partial_joints_goal_ = auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); From 7b293860eb52c04bd9f81c298b7ed2a39321815e Mon Sep 17 00:00:00 2001 From: Schulze Date: Sat, 2 Jul 2022 19:33:15 -0300 Subject: [PATCH 041/111] Update controllers with new get_name hardware interfaces (#369) --- diff_drive_controller/src/diff_drive_controller.cpp | 7 +++---- .../gripper_action_controller_impl.hpp | 12 ++++++------ .../src/joint_state_broadcaster.cpp | 10 +++++----- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 9460849a75..bc2e7eed22 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -219,8 +219,7 @@ controller_interface::return_type DiffDriveController::update( else { odometry_.updateFromVelocity( - left_feedback_mean*period.seconds(), right_feedback_mean*period.seconds(), - time); + left_feedback_mean * period.seconds(), right_feedback_mean * period.seconds(), time); } } @@ -626,7 +625,7 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), [&wheel_name, &interface_name](const auto & interface) { - return interface.get_name() == wheel_name && + return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == interface_name; }); @@ -639,7 +638,7 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto & interface) { - return interface.get_name() == wheel_name && + return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == HW_IF_VELOCITY; }); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 2cd66776b5..748887f612 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -232,11 +232,11 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_name() != joint_name_) + if (position_command_interface_it->get_prefix_name() != joint_name_) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position command interface is different than joint name `" - << position_command_interface_it->get_name() << "` != `" + << position_command_interface_it->get_prefix_name() << "` != `" << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -250,11 +250,11 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_state_interface_it->get_name() != joint_name_) + if (position_state_interface_it->get_prefix_name() != joint_name_) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position state interface is different than joint name `" - << position_state_interface_it->get_name() << "` != `" + << position_state_interface_it->get_prefix_name() << "` != `" << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -268,11 +268,11 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); return controller_interface::CallbackReturn::ERROR; } - if (velocity_state_interface_it->get_name() != joint_name_) + if (velocity_state_interface_it->get_prefix_name() != joint_name_) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Velocity command interface is different than joint name `" - << velocity_state_interface_it->get_name() << "` != `" + << velocity_state_interface_it->get_prefix_name() << "` != `" << joint_name_ << "`"); return controller_interface::CallbackReturn::ERROR; } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a1ad76b092..65d9ec87c2 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -237,9 +237,9 @@ bool JointStateBroadcaster::init_joint_data() for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { // initialize map if name is new - if (name_if_value_mapping_.count(si->get_name()) == 0) + if (name_if_value_mapping_.count(si->get_prefix_name()) == 0) { - name_if_value_mapping_[si->get_name()] = {}; + name_if_value_mapping_[si->get_prefix_name()] = {}; } // add interface name std::string interface_name = si->get_interface_name(); @@ -247,7 +247,7 @@ bool JointStateBroadcaster::init_joint_data() { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[si->get_name()][interface_name] = kUninitializedValue; + name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; } // filter state interfaces that have at least one of the joint_states fields, @@ -345,10 +345,10 @@ controller_interface::return_type JointStateBroadcaster::update( { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[state_interface.get_name()][interface_name] = + name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] = state_interface.get_value(); RCLCPP_DEBUG( - get_node()->get_logger(), "%s: %f\n", state_interface.get_full_name().c_str(), + get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(), state_interface.get_value()); } From da52b32376d650a881a8b957408f067987eca214 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 3 Aug 2022 10:01:55 -0600 Subject: [PATCH 042/111] integrate motion in joint space to eliminate drift error --- .../admittance_controller.hpp | 13 +- .../admittance_controller/admittance_rule.hpp | 8 +- .../admittance_rule_impl.hpp | 119 ++++++--------- .../src/admittance_controller.cpp | 143 ++++++++++-------- 4 files changed, 134 insertions(+), 149 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 82599d5e4d..489432da7d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -100,8 +100,10 @@ namespace admittance_controller { std::vector> joint_position_state_interface_; std::vector> joint_velocity_state_interface_; std::vector> joint_acceleration_state_interface_; + std::vector> joint_effort_state_interface_; - // + // internal reference values + const std::vector reference_interfaces_types_ = {"position", "velocity"}; std::vector position_reference_; std::vector velocity_reference_; @@ -122,18 +124,15 @@ namespace admittance_controller { realtime_tools::RealtimeBuffer> input_joint_command_; std::unique_ptr> state_publisher_; - const std::set allowed_state_interface_types_ = { - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, - }; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; trajectory_msgs::msg::JointTrajectoryPoint last_state_reference_; trajectory_msgs::msg::JointTrajectoryPoint state_offset_; trajectory_msgs::msg::JointTrajectoryPoint prev_trajectory_point_; + // control loop data - trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, - state_error_; + trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, state_error_; trajectory_msgs::msg::JointTrajectory pre_admittance_point; + size_t loop_counter = 0; // helper methods void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 9e3548208e..7287db9bb8 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -50,10 +50,12 @@ namespace admittance_controller { world_base_.setIdentity(); base_sensor_.setIdentity(); base_cog_.setIdentity(); + base_desired_ft_.setIdentity(); } Eigen::Matrix base_control_; Eigen::Matrix base_ft_; + Eigen::Matrix base_desired_ft_; Eigen::Matrix base_tip_; Eigen::Matrix world_base_; Eigen::Matrix base_sensor_; @@ -64,6 +66,7 @@ namespace admittance_controller { Rotations() { base_control_.setIdentity(); base_ft_.setIdentity(); + base_desired_ft_.setIdentity(); base_tip_.setIdentity(); world_base_.setIdentity(); base_world_.setIdentity(); @@ -75,6 +78,7 @@ namespace admittance_controller { Eigen::Matrix base_control_; Eigen::Matrix base_ft_; + Eigen::Matrix base_desired_ft_; Eigen::Matrix base_tip_; Eigen::Matrix world_base_; Eigen::Matrix base_world_; @@ -140,7 +144,8 @@ namespace admittance_controller { * \param[in] success */ bool get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state); + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, + const AdmittanceState &admittance_state); /** * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field @@ -196,6 +201,7 @@ namespace admittance_controller { const Eigen::Matrix &base_control, Eigen::Matrix base_ft, Eigen::Matrix ref_base_ft, + Eigen::Matrix base_desired_ft, const std::string &ft_sensor_frame, double dt, AdmittanceState &admittance_state); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index ac88be6902..5efa73345d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -118,7 +118,8 @@ namespace admittance_controller { } bool AdmittanceRule::get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state) { + const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, + const AdmittanceState &admittance_state) { // get all reference transforms bool success = get_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, parameters_.ft_sensor.frame.external, trans_ref_.base_ft_); @@ -147,8 +148,17 @@ namespace admittance_controller { success &= get_transform(current_joint_state.positions, parameters_.control.frame.id, parameters_.control.frame.external, trans_.base_control_); + // get desired transforms (reference + admittance) + for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { + joint_buffer_vec_[i] = reference_joint_state.positions[i] + admittance_state.joint_pos_[i]; + } + success &= get_transform(joint_buffer_vec_, parameters_.ft_sensor.frame.id, + parameters_.ft_sensor.frame.external, trans_ref_.base_desired_ft_); + + rotations_.base_control_ = trans_ref_.base_control_.block<3, 3>(0, 0); rotations_.base_ft_ = trans_.base_ft_.block<3, 3>(0, 0); + rotations_.base_desired_ft_ = trans_.base_desired_ft_.block<3, 3>(0, 0); rotations_.base_tip_ = trans_.base_tip_.block<3, 3>(0, 0); rotations_.world_base_ = trans_.world_base_.block<3, 3>(0, 0); rotations_.base_world_ = rotations_.world_base_.transpose(); @@ -169,22 +179,24 @@ namespace admittance_controller { trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; +// double dt = 1.0/(1000.0); if (parameters_.enable_parameter_update_without_reactivation) { apply_parameters_update(); } - bool success = get_all_transforms(current_joint_state, reference_joint_state); + bool success = get_all_transforms(current_joint_state, reference_joint_state, admittance_state_); // apply filter and update wrench_world_ vector process_wrench_measurements(measured_wrench, rotations_.world_sensor_, rotations_.world_cog_); + // transform wrench_world_ into base frame // transform wrench_world_ into base frame admittance_state_.wrench_base = rotations_.base_world_ * wrench_world_; // Compute admittance control law success &= calculate_admittance_rule(current_joint_state.positions, rotations_.base_control_, trans_.base_ft_, - trans_ref_.base_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); + trans_ref_.base_ft_, trans_ref_.base_desired_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference if (!success) { @@ -218,14 +230,11 @@ namespace admittance_controller { const Eigen::Matrix &base_control, Eigen::Matrix base_ft, Eigen::Matrix ref_base_ft, + Eigen::Matrix base_desired_ft, const std::string &ft_sensor_frame, double dt, AdmittanceState &admittance_state) { - // reshape inputs - auto admittance_velocity_flat = Eigen::Matrix(admittance_state.admittance_velocity_.data()); - auto wrench_flat = Eigen::Matrix(admittance_state.wrench_base.data()); - // create stiffness matrix Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); @@ -248,73 +257,37 @@ namespace admittance_controller { D.block<3, 3>(0, 0) = D_pos; D.block<3, 3>(3, 3) = D_rot; - // calculate pose error - Eigen::Matrix pose_error; - pose_error.block<3, 1>(0, 0) = admittance_state.admittance_position_.block<3, 1>(0, 3); - - // calculate rotation error - Eigen::Matrix admittance_rotation = admittance_state.admittance_position_.block<3, 3>(0, 0); - Eigen::Vector3d V = get_rotation_axis(admittance_rotation); - double theta = acos(std::clamp((1.0 / 2.0) * (admittance_rotation.trace() - 1), -1.0, 1.0)); - pose_error.block<3, 1>(3, 0) = -theta * V; + // calculate admittance relative offset + Eigen::Matrix X; + X.block<3, 1>(0, 0) = base_desired_ft.block<3, 1>(0, 3) - ref_base_ft.block<3, 1>(0, 3); + Eigen::Matrix R_ref = ref_base_ft.block<3, 3>(0, 0); + Eigen::Matrix R_desired = base_desired_ft.block<3, 3>(0, 0); + Eigen::Matrix R = R_desired * R_ref.transpose(); + Eigen::Vector3d V = get_rotation_axis(R); + double theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); + X.block<3, 1>(3, 0) = -theta * V; + + // get admittance relative velocity + auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity_.data()); + + // get external force + auto F = Eigen::Matrix(admittance_state.wrench_base.data()); + + // Compute admittance control law: F = M*a + D*v + S*x + Eigen::Matrix X_ddot = admittance_state.mass_inv_.cwiseProduct(F - D * X_dot - K * X); + auto tmp = Eigen::Matrix(X_ddot.data()); + bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, tmp, + ft_sensor_frame, + admittance_state.joint_acc_); - // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - Eigen::Matrix admittance_acceleration_flat = admittance_state.mass_inv_.cwiseProduct(wrench_flat - - D * - admittance_velocity_flat - - K * pose_error); + // integrate motion in joint space + admittance_state.joint_vel_ += admittance_state.joint_acc_ * dt; + admittance_state.joint_pos_ += admittance_state.joint_vel_ * dt; - // project acceleration into controllable subspace - auto tmp = Eigen::Matrix(admittance_acceleration_flat.data()); - bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, tmp, - ft_sensor_frame, - admittance_state.joint_acc_); - eigen_to_vec(admittance_state.joint_acc_, joint_buffer_vec_); - success &= convert_joint_deltas_to_cartesian_deltas(current_joint_positions, joint_buffer_vec_, - ft_sensor_frame, - admittance_state.admittance_acceleration_); - - // integrate velocity - admittance_state.admittance_velocity_ += admittance_state.admittance_acceleration_ * dt; - admittance_state.admittance_position_.block<3, 1>(0, 3) += - admittance_state.admittance_velocity_.block<3, 1>(0, 0) * dt; - - // integrate rotational velocity - Eigen::Matrix3d skew_symmetric; - auto rot_vel = admittance_state.admittance_velocity_; - skew_symmetric << 0, -rot_vel(2, 1), rot_vel(1, 1), - rot_vel(2, 1), 0, -rot_vel(0, 1), - -rot_vel(1, 1), rot_vel(0, 1), 0; - Eigen::Matrix3d R_dot = skew_symmetric * admittance_rotation; - admittance_rotation += R_dot * dt; - normalize_rotation(admittance_rotation); - admittance_state.admittance_position_.block<3, 3>(0, 0) = admittance_rotation; - - // calculate position drift due to integrating the joint positions - auto admittance_position_base = - ref_base_ft.block<3, 1>(0, 3) + admittance_state.admittance_position_.block<3, 1>(0, 3); - Eigen::Matrix admittance_cur_position_base = base_ft.block<3, 1>(0, 3); - Eigen::Matrix ft_rot = base_ft.block<3, 3>(0, 0); - Eigen::Matrix reference_ft_rot = ref_base_ft.block<3, 3>(0, 0); - - // calculate rotation drift due to integrating the joint positions - Eigen::Matrix R = (admittance_rotation*reference_ft_rot).transpose()*ft_rot; - - // calculate correction term - Eigen::Matrix correction_term; - correction_term.block<3, 1>(0, 0) = parameters_.admittance.linear_correction*(admittance_position_base - admittance_cur_position_base); - V = get_rotation_axis(R); - theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); - correction_term.block<3, 1>(0, 1) = parameters_.admittance.angular_correction*theta * V; - - // calculate joint velocities that correspondence to cartesian accelerations - success &= convert_cartesian_deltas_to_joint_deltas(current_joint_positions, admittance_state.admittance_velocity_ + correction_term, - ft_sensor_frame, - admittance_state.joint_vel_); - - for (auto i = 0ul; i < admittance_state.joint_pos_.size(); i++) { - admittance_state.joint_pos_[i] += admittance_state.joint_vel_[i] * dt; - } + // calculate admittance velocity corresponding to joint velocity + eigen_to_vec(admittance_state.joint_vel_, joint_buffer_vec_); + success &= convert_joint_deltas_to_cartesian_deltas(current_joint_positions, joint_buffer_vec_, ft_sensor_frame, + admittance_state.admittance_velocity_); return success; @@ -530,7 +503,6 @@ namespace admittance_controller { matrix(row, col) = data[row + col * matrix.rows()]; } } - } template @@ -540,7 +512,6 @@ namespace admittance_controller { data[row + col * matrix.rows()] = matrix(row, col); } } - } } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index e0688c833b..478485f4f8 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -40,17 +40,26 @@ namespace admittance_controller { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } - // allocate dynamic memory num_joints_ = admittance_->parameters_.joints.size(); + + // allocate dynamic memory last_state_reference_.positions.assign(num_joints_, 0.0); last_state_reference_.velocities.assign(num_joints_, 0.0); last_state_reference_.accelerations.assign(num_joints_, 0.0); - last_commanded_state_ = last_state_reference_; state_reference_ = last_state_reference_; state_desired_ = last_state_reference_; state_current_ = last_state_reference_; + joint_position_command_interface_.reserve(num_joints_); + joint_velocity_command_interface_.reserve(num_joints_); + joint_acceleration_command_interface_.reserve(num_joints_); + joint_effort_command_interface_.reserve(num_joints_); + joint_position_state_interface_.reserve(num_joints_); + joint_velocity_state_interface_.reserve(num_joints_); + joint_acceleration_state_interface_.reserve(num_joints_); + joint_effort_state_interface_.reserve(num_joints_); + return CallbackReturn::SUCCESS; } @@ -59,8 +68,8 @@ namespace admittance_controller { // claim the joint + interface combinations for state interfaces from the resource manager. Finally, // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported - if (!admittance_){ - return {controller_interface::interface_configuration_type::INDIVIDUAL,{}}; + if (!admittance_) { + return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; } std::vector state_interfaces_config_names; @@ -82,8 +91,8 @@ namespace admittance_controller { // claim the joint + interface combinations for command interfaces from the resource manager. Finally, // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported - if (!admittance_){ - return {controller_interface::interface_configuration_type::INDIVIDUAL,{}}; + if (!admittance_) { + return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; } std::vector command_interfaces_config_names; @@ -99,26 +108,29 @@ namespace admittance_controller { std::vector AdmittanceController::on_export_reference_interfaces() { // create CommandInterface interfaces that other controllers will be able to chain with - if (!admittance_){ + if (!admittance_) { return {}; } std::vector chainable_command_interfaces; auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); - reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); - chainable_command_interfaces.reserve(num_chainable_interfaces); - position_reference_.clear(); - velocity_reference_.clear(); - std::unordered_map *> chainable_interface_map = { - {hardware_interface::HW_IF_POSITION, &position_reference_}, - {hardware_interface::HW_IF_VELOCITY, &velocity_reference_}}; + //allocate dynamic memory + chainable_command_interfaces.reserve(num_chainable_interfaces); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + position_reference_.reserve(num_joints_); + velocity_reference_.reserve(num_joints_); + // assign reference interfaces auto index = 0ul; - for (const auto &interface: admittance_->parameters_.chainable_command_interfaces) { - for (const auto &joint: admittance_->parameters_.joints) { - chainable_interface_map[interface]->push_back(reference_interfaces_.data() + index); + for (const auto& interface : reference_interfaces_types_) { + for (const auto& joint : admittance_->parameters_.joints) { + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_.data() + index); + else if (hardware_interface::HW_IF_VELOCITY == interface) { + velocity_reference_.emplace_back(reference_interfaces_.data() + index); + } chainable_command_interfaces.emplace_back( hardware_interface::CommandInterface(std::string(get_node()->get_name()), joint + "/" + interface, @@ -173,7 +185,7 @@ namespace admittance_controller { // setup subscribers and publishers auto joint_command_callback = [this](const std::shared_ptr msg) { - input_joint_command_.writeFromNonRT(msg); + input_joint_command_.writeFromNonRT(msg); }; input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_commands", rclcpp::SystemDefaultsQoS(), joint_command_callback); @@ -204,48 +216,42 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { // on_activate is called when the lifecycle node activates. - if (!admittance_){ + if (!admittance_) { return CallbackReturn::ERROR; } // update parameters if any have changed - if (admittance_->parameter_handler_->is_old(admittance_->parameters_)){ + if (admittance_->parameter_handler_->is_old(admittance_->parameters_)) { admittance_->parameters_ = admittance_->parameter_handler_->get_params(); } - // Initialize Admittance Rule and update admittance config - admittance_->reset(num_joints_); - - // clear out vectors in case of restart - joint_position_command_interface_.clear(); - joint_velocity_command_interface_.clear(); - joint_position_state_interface_.clear(); - joint_velocity_state_interface_.clear(); - // assign command interfaces - std::unordered_map> *> - command_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_command_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_command_interface_} - }; for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) { for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { - command_interface_map[admittance_->parameters_.command_interfaces[i]]->emplace_back( - command_interfaces_[i * num_joints_ + j]); + if (hardware_interface::HW_IF_POSITION == admittance_->parameters_.command_interfaces[i]) { + joint_position_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); + } else if (hardware_interface::HW_IF_VELOCITY == admittance_->parameters_.command_interfaces[i]) { + joint_velocity_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); + } else if (hardware_interface::HW_IF_ACCELERATION == admittance_->parameters_.command_interfaces[i]) { + joint_acceleration_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); + } else if (hardware_interface::HW_IF_EFFORT == admittance_->parameters_.command_interfaces[i]) { + joint_effort_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); + } } } // assign state interfaces - std::unordered_map> *> state_interface_map = { - {hardware_interface::HW_IF_POSITION, &joint_position_state_interface_}, - {hardware_interface::HW_IF_VELOCITY, &joint_velocity_state_interface_} - }; for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { - state_interface_map[admittance_->parameters_.state_interfaces[i]]->emplace_back( - state_interfaces_[i * num_joints_ + j]); + if (hardware_interface::HW_IF_POSITION == admittance_->parameters_.state_interfaces[i]) { + joint_position_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); + } else if (hardware_interface::HW_IF_VELOCITY == admittance_->parameters_.state_interfaces[i]) { + joint_velocity_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); + } else if (hardware_interface::HW_IF_ACCELERATION == admittance_->parameters_.state_interfaces[i]) { + joint_acceleration_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); + } else if (hardware_interface::HW_IF_EFFORT == admittance_->parameters_.state_interfaces[i]) { + joint_effort_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); + } } } @@ -254,32 +260,29 @@ namespace admittance_controller { // if joint_position_state_interface_ is empty, we have no information about the state, continuing is dangerous if (joint_position_state_interface_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), - "No joint position state interface exist."); + RCLCPP_ERROR(get_node()->get_logger(), "No joint position state interface exist."); return CallbackReturn::ERROR; } - // handle state after restart or initial startup - read_state_from_hardware(state_reference_); + // handle state after restart or initial startups + read_state_from_hardware(state_current_); + // initialize states from last read state reference + last_commanded_state_ = state_current_; + last_state_reference_ = state_current_; + + read_state_reference_interfaces(state_reference_); // reset dynamic fields in case non-zero state_reference_.velocities.assign(num_joints_, 0.0); state_reference_.accelerations.assign(num_joints_, 0.0); - - // initialize last_commanded_state_ last read state reference - last_commanded_state_ = state_reference_; - last_state_reference_ = state_reference_; - - // initialize state reference state_desired_ = state_reference_; - state_current_ = state_reference_; return CallbackReturn::SUCCESS; } controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { // update input reference from ros subscriber message - if (!admittance_){ + if (!admittance_) { return controller_interface::return_type::ERROR; } @@ -301,9 +304,10 @@ namespace admittance_controller { controller_interface::return_type AdmittanceController::update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) { // Realtime constraints are required in this function - if (!admittance_){ + if (!admittance_) { return controller_interface::return_type::ERROR; } + loop_counter++; // update input reference from chainable interfaces read_state_reference_interfaces(state_reference_); @@ -319,7 +323,6 @@ namespace admittance_controller { // write calculated values to joint interfaces for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { joint_position_command_interface_[i].get().set_value(state_desired_.positions[i]); - // if in open loop, feed the commanded state back into the input (open_loop_buffer) last_commanded_state_.positions[i] = state_desired_.positions[i]; } for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { @@ -345,6 +348,14 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { force_torque_sensor_->release_interfaces(); + joint_position_command_interface_.clear(); + joint_velocity_command_interface_.clear(); + joint_acceleration_command_interface_.clear(); + joint_effort_command_interface_.clear(); + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + joint_acceleration_state_interface_.clear(); + joint_effort_state_interface_.clear(); return LifecycleNodeInterface::on_deactivate(previous_state); } @@ -354,7 +365,7 @@ namespace admittance_controller { } CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { - if (!admittance_){ + if (!admittance_) { return CallbackReturn::ERROR; } admittance_->reset(num_joints_); @@ -397,20 +408,18 @@ namespace admittance_controller { // the values are nan, the corresponding field will be set to empty for (auto i = 0ul; i < position_reference_.size(); i++) { - state_reference.positions[i] = *position_reference_[i]; - if (std::isnan(state_reference.positions[i])) { - state_reference.positions = last_state_reference_.positions; - break; + if (std::isnan(*position_reference_[i])) { + *position_reference_[i] = last_state_reference_.positions[i]; } + state_reference.positions[i] = *position_reference_[i]; } last_state_reference_.positions = state_reference.positions; for (auto i = 0ul; i < velocity_reference_.size(); i++) { - state_reference.velocities[i] = *velocity_reference_[i]; - if (std::isnan(state_reference.velocities[i])) { - state_reference.velocities = last_state_reference_.velocities; - break; + if (std::isnan(*velocity_reference_[i])) { + *velocity_reference_[i] = last_state_reference_.velocities[i]; } + state_reference.velocities[i] = *velocity_reference_[i]; } last_state_reference_.velocities = state_reference.velocities; From 09516055027b76a9b1aaf68e64dad8539c750647 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 3 Aug 2022 10:06:06 -0600 Subject: [PATCH 043/111] remove drift correction parameters --- .../admittance_controller_parameters.hpp | 42 ------------------- .../src/admittance_controller_parameters.yaml | 18 -------- 2 files changed, 60 deletions(-) diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp index 65bf09a619..60f5a5dce6 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp +++ b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp @@ -251,8 +251,6 @@ namespace admittance_controller { std::vector mass; std::vector damping_ratio; std::vector stiffness; - double linear_correction = 1.0; - double angular_correction = 0.1; } admittance; // for detecting if the parameter struct has been updated rclcpp::Time __stamp; @@ -399,20 +397,6 @@ namespace admittance_controller { } updated_params.admittance.stiffness = param.as_double_array(); } - if (param.get_name() == "admittance.linear_correction") { - if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); - !validation_result.success()) { - return validation_result; - } - updated_params.admittance.linear_correction = param.as_double(); - } - if (param.get_name() == "admittance.angular_correction") { - if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); - !validation_result.success()) { - return validation_result; - } - updated_params.admittance.angular_correction = param.as_double(); - } if (param.get_name() == "robot_description") { updated_params.robot_description = param.as_string(); } @@ -603,20 +587,6 @@ namespace admittance_controller { auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; parameters_interface_->declare_parameter("admittance.stiffness", parameter, descriptor); } - if (!parameters_interface_->has_parameter("admittance.linear_correction")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the coefficient used in the linear drift correction calculation"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.admittance.linear_correction); - parameters_interface_->declare_parameter("admittance.linear_correction", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("admittance.angular_correction")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the coefficient used in the angular drift correction calculation"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.admittance.angular_correction); - parameters_interface_->declare_parameter("admittance.angular_correction", parameter, descriptor); - } if (!parameters_interface_->has_parameter("robot_description")) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics."; @@ -711,18 +681,6 @@ namespace admittance_controller { throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.stiffness': " + validation_result.error_msg())); } params_.admittance.stiffness = param.as_double_array(); - param = parameters_interface_->get_parameter("admittance.linear_correction"); - if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.linear_correction': " + validation_result.error_msg())); - } - params_.admittance.linear_correction = param.as_double(); - param = parameters_interface_->get_parameter("admittance.angular_correction"); - if(auto validation_result = gen_param_struct_validators::lower_bounds(param, 0.0); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.angular_correction': " + validation_result.error_msg())); - } - params_.admittance.angular_correction = param.as_double(); param = parameters_interface_->get_parameter("robot_description"); params_.robot_description = param.as_string(); param = parameters_interface_->get_parameter("enable_parameter_update_without_reactivation"); diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index b4feb68ac6..b43b2db24d 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -156,24 +156,6 @@ admittance_controller: } } - linear_correction: { - type: double, - description: "specifies the coefficient used in the linear drift correction calculation", - default_value: 1.0, - validation: { - lower_bounds<>: [ 0.0] - } - } - - angular_correction: { - type: double, - description: "specifies the coefficient used in the angular drift correction calculation", - default_value: 0.1, - validation: { - lower_bounds<>: [ 0.0] - } - } - # general settings robot_description: { type: string, From 98854329ee567119b2a668f1e6a1615b98f8000c Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 3 Aug 2022 15:47:30 -0600 Subject: [PATCH 044/111] move interface vector reset to on_activate --- .../src/admittance_controller.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 478485f4f8..acf2826cc9 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -225,6 +225,16 @@ namespace admittance_controller { admittance_->parameters_ = admittance_->parameter_handler_->get_params(); } + // reset all interfaces in case of restart + joint_position_command_interface_.clear(); + joint_velocity_command_interface_.clear(); + joint_acceleration_command_interface_.clear(); + joint_effort_command_interface_.clear(); + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + joint_acceleration_state_interface_.clear(); + joint_effort_state_interface_.clear(); + // assign command interfaces for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) { for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { @@ -348,14 +358,6 @@ namespace admittance_controller { CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { force_torque_sensor_->release_interfaces(); - joint_position_command_interface_.clear(); - joint_velocity_command_interface_.clear(); - joint_acceleration_command_interface_.clear(); - joint_effort_command_interface_.clear(); - joint_position_state_interface_.clear(); - joint_velocity_state_interface_.clear(); - joint_acceleration_state_interface_.clear(); - joint_effort_state_interface_.clear(); return LifecycleNodeInterface::on_deactivate(previous_state); } From 7d5fcf20585f73a52138e002bb53df5f4b640848 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 3 Aug 2022 20:07:02 -0600 Subject: [PATCH 045/111] remove interface maps --- .../admittance_controller.hpp | 24 ++- .../src/admittance_controller.cpp | 154 ++++++++---------- .../src/joint_trajectory_controller.cpp | 4 +- 3 files changed, 88 insertions(+), 94 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 489432da7d..2ea88586b2 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -93,14 +93,21 @@ namespace admittance_controller { int num_joints_ = 0; // vectors to hold joint hardware interfaces - std::vector> joint_position_command_interface_; - std::vector> joint_velocity_command_interface_; - std::vector> joint_acceleration_command_interface_; - std::vector> joint_effort_command_interface_; - std::vector> joint_position_state_interface_; - std::vector> joint_velocity_state_interface_; - std::vector> joint_acceleration_state_interface_; - std::vector> joint_effort_state_interface_; +// std::vector joint_position_command_interface_; +// std::vector joint_velocity_command_interface_; +// std::vector joint_acceleration_command_interface_; +// std::vector joint_effort_command_interface_; +// std::vector joint_position_state_interface_; +// std::vector joint_velocity_state_interface_; +// std::vector joint_acceleration_state_interface_; +// std::vector joint_effort_state_interface_; + size_t state_pos_ind = -1; + size_t state_vel_ind = -1; + size_t state_acc_ind = -1; + size_t command_pos_ind = -1; + size_t command_vel_ind = -1; + size_t command_acc_ind = -1; + // internal reference values const std::vector reference_interfaces_types_ = {"position", "velocity"}; @@ -137,6 +144,7 @@ namespace admittance_controller { // helper methods void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); + void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_commanded); }; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index acf2826cc9..5a95b98962 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -51,15 +51,6 @@ namespace admittance_controller { state_desired_ = last_state_reference_; state_current_ = last_state_reference_; - joint_position_command_interface_.reserve(num_joints_); - joint_velocity_command_interface_.reserve(num_joints_); - joint_acceleration_command_interface_.reserve(num_joints_); - joint_effort_command_interface_.reserve(num_joints_); - joint_position_state_interface_.reserve(num_joints_); - joint_velocity_state_interface_.reserve(num_joints_); - joint_acceleration_state_interface_.reserve(num_joints_); - joint_effort_state_interface_.reserve(num_joints_); - return CallbackReturn::SUCCESS; } @@ -73,11 +64,13 @@ namespace admittance_controller { } std::vector state_interfaces_config_names; - for (const auto &interface: admittance_->parameters_.state_interfaces) { + for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { + const auto &interface = admittance_->parameters_.state_interfaces[i]; for (const auto &joint: admittance_->parameters_.joints) { state_interfaces_config_names.push_back(joint + "/" + interface); } } + auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); state_interfaces_config_names.insert(state_interfaces_config_names.end(), ft_interfaces.begin(), ft_interfaces.end()); @@ -124,8 +117,8 @@ namespace admittance_controller { // assign reference interfaces auto index = 0ul; - for (const auto& interface : reference_interfaces_types_) { - for (const auto& joint : admittance_->parameters_.joints) { + for (const auto &interface: reference_interfaces_types_) { + for (const auto &joint: admittance_->parameters_.joints) { if (hardware_interface::HW_IF_POSITION == interface) position_reference_.emplace_back(reference_interfaces_.data() + index); else if (hardware_interface::HW_IF_VELOCITY == interface) { @@ -225,55 +218,34 @@ namespace admittance_controller { admittance_->parameters_ = admittance_->parameter_handler_->get_params(); } - // reset all interfaces in case of restart - joint_position_command_interface_.clear(); - joint_velocity_command_interface_.clear(); - joint_acceleration_command_interface_.clear(); - joint_effort_command_interface_.clear(); - joint_position_state_interface_.clear(); - joint_velocity_state_interface_.clear(); - joint_acceleration_state_interface_.clear(); - joint_effort_state_interface_.clear(); - - // assign command interfaces + // get state interface inds + std::unordered_map inter_to_ind = {{hardware_interface::HW_IF_POSITION, -1}, + {hardware_interface::HW_IF_VELOCITY, -1}, + {hardware_interface::HW_IF_ACCELERATION, -1}}; + for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { + const auto &interface = admittance_->parameters_.state_interfaces[i]; + inter_to_ind[interface] = i; + } + state_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; + state_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; + state_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; + + // get state interface inds + inter_to_ind = {{hardware_interface::HW_IF_POSITION, -1}, + {hardware_interface::HW_IF_VELOCITY, -1}, + {hardware_interface::HW_IF_ACCELERATION, -1}}; for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) { - for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { - if (hardware_interface::HW_IF_POSITION == admittance_->parameters_.command_interfaces[i]) { - joint_position_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); - } else if (hardware_interface::HW_IF_VELOCITY == admittance_->parameters_.command_interfaces[i]) { - joint_velocity_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); - } else if (hardware_interface::HW_IF_ACCELERATION == admittance_->parameters_.command_interfaces[i]) { - joint_acceleration_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); - } else if (hardware_interface::HW_IF_EFFORT == admittance_->parameters_.command_interfaces[i]) { - joint_effort_command_interface_.emplace_back(command_interfaces_[i * num_joints_ + j]); - } - } + const auto &interface = admittance_->parameters_.command_interfaces[i]; + inter_to_ind[interface] = i; } + command_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; + command_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; + command_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; - // assign state interfaces - for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { - for (auto j = 0ul; j < admittance_->parameters_.joints.size(); j++) { - if (hardware_interface::HW_IF_POSITION == admittance_->parameters_.state_interfaces[i]) { - joint_position_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); - } else if (hardware_interface::HW_IF_VELOCITY == admittance_->parameters_.state_interfaces[i]) { - joint_velocity_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); - } else if (hardware_interface::HW_IF_ACCELERATION == admittance_->parameters_.state_interfaces[i]) { - joint_acceleration_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); - } else if (hardware_interface::HW_IF_EFFORT == admittance_->parameters_.state_interfaces[i]) { - joint_effort_state_interface_.emplace_back(state_interfaces_[i * num_joints_ + j]); - } - } - } // initialize interface of the FTS semantic semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); - // if joint_position_state_interface_ is empty, we have no information about the state, continuing is dangerous - if (joint_position_state_interface_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "No joint position state interface exist."); - return CallbackReturn::ERROR; - } - // handle state after restart or initial startups read_state_from_hardware(state_current_); @@ -331,18 +303,7 @@ namespace admittance_controller { admittance_->update(state_current_, ft_values, state_reference_, period, state_desired_); // write calculated values to joint interfaces - for (auto i = 0ul; i < joint_position_command_interface_.size(); i++) { - joint_position_command_interface_[i].get().set_value(state_desired_.positions[i]); - last_commanded_state_.positions[i] = state_desired_.positions[i]; - } - for (auto i = 0ul; i < joint_velocity_command_interface_.size(); i++) { - joint_velocity_command_interface_[i].get().set_value(state_desired_.velocities[i]); - last_commanded_state_.velocities[i] = state_desired_.velocities[i]; - } - for (auto i = 0ul; i < joint_acceleration_command_interface_.size(); i++) { - joint_acceleration_command_interface_[i].get().set_value(state_desired_.accelerations[i]); - last_commanded_state_.accelerations[i] = state_desired_.accelerations[i]; - } + write_state_to_hardware(state_desired_); // Publish controller state state_publisher_->lock(); @@ -375,32 +336,57 @@ namespace admittance_controller { } void AdmittanceController::read_state_from_hardware( - trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { + trajectory_msgs::msg::JointTrajectoryPoint &state_current) { // Fill fields of state_reference argument from hardware state interfaces. If the hardware does not exist or // the values are nan, the corresponding state field will be set to empty. // if any interface has nan values, assume state_reference is the last command state - for (auto i = 0ul; i < joint_position_state_interface_.size(); i++) { - state_reference.positions[i] = joint_position_state_interface_[i].get().get_value(); - if (std::isnan(state_reference.positions[i])) { - state_reference.positions = last_commanded_state_.positions; - break; - } - } - for (auto i = 0ul; i < joint_velocity_state_interface_.size(); i++) { - state_reference.velocities[i] = joint_velocity_state_interface_[i].get().get_value(); - if (std::isnan(state_reference.velocities[i])) { - state_reference.velocities = last_commanded_state_.velocities; - break; + for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) { + for (auto inter_ind = 0; inter_ind < 3; inter_ind++) { + auto ind = joint_ind + num_joints_ * inter_ind; + if (inter_ind == state_pos_ind) { + state_current.positions[joint_ind] = state_interfaces_[ind].get_value(); + if (std::isnan(state_current.positions[joint_ind])) { + state_current.positions = last_commanded_state_.positions; + break; + } + } else if (inter_ind == state_vel_ind) { + state_current.velocities[joint_ind] = state_interfaces_[ind].get_value(); + if (std::isnan(state_current.positions[joint_ind])) { + state_current.velocities = last_commanded_state_.velocities; + break; + } + } else if (inter_ind == state_acc_ind) { + state_current.accelerations[joint_ind] = state_interfaces_[ind].get_value(); + if (std::isnan(state_current.positions[joint_ind])) { + state_current.accelerations = last_commanded_state_.accelerations; + break; + } + } } } - for (auto i = 0ul; i < joint_acceleration_state_interface_.size(); i++) { - state_reference.accelerations[i] = joint_acceleration_state_interface_[i].get().get_value(); - if (std::isnan(state_reference.accelerations[i])) { - state_reference.accelerations = last_commanded_state_.accelerations; - break; + + } + + void + AdmittanceController::write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_commanded) { + // Fill fields of state_reference argument from hardware state interfaces. If the hardware does not exist or + // the values are nan, the corresponding state field will be set to empty. + + // if any interface has nan values, assume state_reference is the last command state + for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) { + for (auto inter_ind = 0; inter_ind < 3; inter_ind++) { + auto ind = joint_ind + num_joints_ * inter_ind; + if (inter_ind == command_pos_ind) { + command_interfaces_[ind].set_value(state_commanded.positions[joint_ind]); + } else if (inter_ind == command_vel_ind) { + command_interfaces_[ind].set_value(state_commanded.velocities[joint_ind]); + } else if (inter_ind == command_acc_ind) { + command_interfaces_[ind].set_value(state_commanded.accelerations[joint_ind]); + } } } + last_commanded_state_ = state_desired_; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c95f762f19..dabd570567 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -895,8 +895,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { // go home - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; +// traj_home_point_ptr_->update(traj_msg_home_ptr_); +// traj_point_active_ptr_ = &traj_home_point_ptr_; return CallbackReturn::SUCCESS; } From cf7031f101a807d3ec6ed890e620e0ae404d4963 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 4 Aug 2022 07:00:59 -0600 Subject: [PATCH 046/111] fix JTC with newest ros_control --- .../joint_trajectory_controller.hpp | 425 ++-- .../src/joint_trajectory_controller.cpp | 1865 +++++++++-------- 2 files changed, 1168 insertions(+), 1122 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 79fe6bccf4..59bcdac3c0 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -27,6 +27,7 @@ #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +//#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/visibility_control.h" @@ -48,226 +49,226 @@ using namespace std::chrono_literals; // NOLINT namespace rclcpp_action { -template -class ServerGoalHandle; + template + class ServerGoalHandle; } // namespace rclcpp_action namespace rclcpp_lifecycle { -class State; + class State; } // namespace rclcpp_lifecycle namespace joint_trajectory_controller { -class Trajectory; - -class JointTrajectoryController : public controller_interface::ControllerInterface -{ -public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - JointTrajectoryController(); - - /** - * @brief command_interface_configuration This controller requires the position command - * interfaces for the controlled joints - */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - /** - * @brief command_interface_configuration This controller requires the position and velocity - * state interfaces for the controlled joints - */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_error( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - -protected: - std::vector joint_names_; - std::vector command_joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - - // To reduce number of variables and to make the code shorter the interfaces are ordered in types - // as the following constants - const std::vector allowed_interface_types_ = { - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, - hardware_interface::HW_IF_EFFORT, + class Trajectory; + + class JointTrajectoryController : public controller_interface::ControllerInterface + { + public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + JointTrajectoryController(); + + /** + * @brief command_interface_configuration This controller requires the position command + * interfaces for the controlled joints + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration This controller requires the position and velocity + * state interfaces for the controlled joints + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + protected: + std::vector joint_names_; + std::vector command_joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, + hardware_interface::HW_IF_EFFORT, + }; + + // Preallocate variables used in the realtime update() function + trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint state_desired_; + trajectory_msgs::msg::JointTrajectoryPoint state_error_; + + // Degrees of freedom + size_t dof_; + + // Parameters for some special cases, e.g. hydraulics powered robots + // Run the controller in open-loop, i.e., read hardware states only when starting controller. + // This is useful when robot is not exactly following the commanded trajectory. + bool open_loop_control_ = false; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + /// Allow integration in goal trajectories to accept goals without position or velocity specified + bool allow_integration_in_goal_trajectories_ = false; + /// Specify interpolation method. Default to splines. + interpolation_methods::InterpolationMethod interpolation_method_{ + interpolation_methods::DEFAULT_INTERPOLATION}; + + double state_publish_rate_; + double action_monitor_rate_; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_state_interface_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + /// If true, a velocity feedforward term plus corrective PID term is used + bool use_closed_loop_pid_adapter_ = false; + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + // reserved storage for result of the command when closed loop pid adapter is used + std::vector tmp_command_; + + // TODO(karsten1987): eventually activate and deactivate subscriber directly when it's supported + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr joint_command_subscriber_ = + nullptr; + + std::shared_ptr * traj_point_active_ptr_ = nullptr; + std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr traj_home_point_ptr_ = nullptr; + std::shared_ptr traj_msg_home_ptr_ = nullptr; + realtime_tools::RealtimeBuffer> + traj_msg_external_point_ptr_; + + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; + using StatePublisher = realtime_tools::RealtimePublisher; + using StatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr publisher_; + StatePublisherPtr state_publisher_; + + rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); + rclcpp::Time last_state_publish_time_; + + using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + rclcpp_action::Server::SharedPtr action_server_; + bool allow_partial_joints_goal_ = false; + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + // joint limits for JTC +// std::vector joint_limits_; + + // callbacks for action_server_ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_action::CancelResponse cancel_callback( + const std::shared_ptr> goal_handle); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void feedback_setup_callback( + std::shared_ptr> goal_handle); + + // fill trajectory_msg so it matches joints controlled by this controller + // positions set to current position, velocities, accelerations and efforts to 0.0 + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void fill_partial_goal( + std::shared_ptr trajectory_msg) const; + // sorts the joints of the incoming message to our local order + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void sort_to_local_joint_order( + std::shared_ptr trajectory_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void add_new_trajectory_msg( + const std::shared_ptr & traj_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + + SegmentTolerances default_tolerances_; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void preempt_active_goal(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void set_hold_position(); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool reset(); + + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void publish_state( + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error); + + void read_state_from_hardware(JointTrajectoryPoint & state); + + bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + + private: + bool contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type); + + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); }; - // Preallocate variables used in the realtime update() function - trajectory_msgs::msg::JointTrajectoryPoint state_current_; - trajectory_msgs::msg::JointTrajectoryPoint state_desired_; - trajectory_msgs::msg::JointTrajectoryPoint state_error_; - - // Degrees of freedom - size_t dof_; - - // Parameters for some special cases, e.g. hydraulics powered robots - // Run the controller in open-loop, i.e., read hardware states only when starting controller. - // This is useful when robot is not exactly following the commanded trajectory. - bool open_loop_control_ = false; - trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; - /// Allow integration in goal trajectories to accept goals without position or velocity specified - bool allow_integration_in_goal_trajectories_ = false; - /// Specify interpolation method. Default to splines. - interpolation_methods::InterpolationMethod interpolation_method_{ - interpolation_methods::DEFAULT_INTERPOLATION}; - - double state_publish_rate_; - double action_monitor_rate_; - - // The interfaces are defined as the types in 'allowed_interface_types_' member. - // For convenience, for each type the interfaces are ordered so that i-th position - // matches i-th index in joint_names_ - template - using InterfaceReferences = std::vector>>; - - InterfaceReferences joint_command_interface_; - InterfaceReferences joint_state_interface_; - - bool has_position_state_interface_ = false; - bool has_velocity_state_interface_ = false; - bool has_acceleration_state_interface_ = false; - bool has_position_command_interface_ = false; - bool has_velocity_command_interface_ = false; - bool has_acceleration_command_interface_ = false; - bool has_effort_command_interface_ = false; - - /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_pid_adapter_ = false; - using PidPtr = std::shared_ptr; - std::vector pids_; - // Feed-forward velocity weight factor when calculating closed loop pid adapter's command - std::vector ff_velocity_scale_; - // reserved storage for result of the command when closed loop pid adapter is used - std::vector tmp_command_; - - // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported - bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr joint_command_subscriber_ = - nullptr; - - std::shared_ptr * traj_point_active_ptr_ = nullptr; - std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; - realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; - - using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; - using StatePublisher = realtime_tools::RealtimePublisher; - using StatePublisherPtr = std::unique_ptr; - rclcpp::Publisher::SharedPtr publisher_; - StatePublisherPtr state_publisher_; - - rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); - rclcpp::Time last_state_publish_time_; - - using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; - using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; - using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; - - rclcpp_action::Server::SharedPtr action_server_; - bool allow_partial_joints_goal_ = false; - RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. - rclcpp::TimerBase::SharedPtr goal_handle_timer_; - rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); - - // callback for topic interface - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void topic_callback(const std::shared_ptr msg); - - // callbacks for action_server_ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::GoalResponse goal_received_callback( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::CancelResponse goal_cancelled_callback( - const std::shared_ptr> goal_handle); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void goal_accepted_callback( - std::shared_ptr> goal_handle); - - // fill trajectory_msg so it matches joints controlled by this controller - // positions set to current position, velocities, accelerations and efforts to 0.0 - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void fill_partial_goal( - std::shared_ptr trajectory_msg) const; - // sorts the joints of the incoming message to our local order - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( - const std::shared_ptr & traj_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const; - - SegmentTolerances default_tolerances_; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void preempt_active_goal(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_hold_position(); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool reset(); - - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error); - - void read_state_from_hardware(JointTrajectoryPoint & state); - - bool read_state_from_command_interfaces(JointTrajectoryPoint & state); - -private: - bool contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type); - - void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); -}; - } // namespace joint_trajectory_controller -#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ +#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ \ No newline at end of file diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dabd570567..753d836ef1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -29,6 +29,7 @@ #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +//#include "joint_limits/joint_limits_rosparam.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/logging.hpp" @@ -45,152 +46,172 @@ namespace joint_trajectory_controller { -JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), joint_names_({}) -{ -} - -controller_interface::CallbackReturn JointTrajectoryController::on_init() -{ - try + JointTrajectoryController::JointTrajectoryController() + : controller_interface::ControllerInterface(), joint_names_({}), dof_(0) { - // with the lifecycle node being initialized, we can declare parameters - joint_names_ = auto_declare>("joints", joint_names_); - command_interface_types_ = - auto_declare>("command_interfaces", command_interface_types_); - state_interface_types_ = - auto_declare>("state_interfaces", state_interface_types_); - command_joint_names_ = - auto_declare>("command_joints", command_interface_types_); - allow_partial_joints_goal_ = - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); - allow_integration_in_goal_trajectories_ = auto_declare( - "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); - state_publish_rate_ = auto_declare("state_publish_rate", 50.0); - action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return CallbackReturn::SUCCESS; -} -controller_interface::InterfaceConfiguration -JointTrajectoryController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : command_joint_names_) + controller_interface::CallbackReturn JointTrajectoryController::on_init() { - for (const auto & interface_type : command_interface_types_) + try { - conf.names.push_back(joint_name + "/" + interface_type); + // with the lifecycle node being initialized, we can declare parameters + joint_names_ = auto_declare>("joints", joint_names_); + command_joint_names_ = + auto_declare>("command_joints", command_interface_types_); + command_interface_types_ = + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("state_interfaces", state_interface_types_); + allow_partial_joints_goal_ = + auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); + open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); + allow_integration_in_goal_trajectories_ = auto_declare( + "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); + state_publish_rate_ = auto_declare("state_publish_rate", 50.0); + action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); + + const std::string interpolation_string = auto_declare( + "interpolation_method", interpolation_methods::InterpolationMethodMap.at( + interpolation_methods::DEFAULT_INTERPOLATION)); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + auto_declare("constraints.stopped_velocity_tolerance", 0.01); + auto_declare("constraints.goal_time", 0.0); } - } - return conf; -} - -controller_interface::InterfaceConfiguration -JointTrajectoryController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint_name : joint_names_) - { - for (const auto & interface_type : state_interface_types_) + catch (const std::exception & e) { - conf.names.push_back(joint_name + "/" + interface_type); + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } - } - return conf; -} -controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) { - // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); - if (has_velocity_state_interface_ && has_velocity_command_interface_) + controller_interface::InterfaceConfiguration + JointTrajectoryController::command_interface_configuration() const + { + if (dof_ == 0) { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; + fprintf( + stderr, + "During ros2_control interface configuration, degrees of freedom is not valid;" + " it should be positive. Actual DOF is %zu\n", + dof_); + std::exit(EXIT_FAILURE); } - if (has_acceleration_state_interface_ && has_acceleration_command_interface_) + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint_name : command_joint_names_) { - error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + for (const auto & interface_type : command_interface_types_) + { + conf.names.push_back(joint_name + "/" + interface_type); + } } - }; + return conf; + } - // Check if a new external message has been received from nonRT threads - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) + controller_interface::InterfaceConfiguration + JointTrajectoryController::state_interface_configuration() const { - fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); - // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(dof_ * state_interface_types_.size()); + for (const auto & joint_name : joint_names_) + { + for (const auto & interface_type : state_interface_types_) + { + conf.names.push_back(joint_name + "/" + interface_type); + } + } + return conf; } - JointTrajectoryPoint state_current, state_desired, state_error; - const auto joint_num = joint_names_.size(); - resize_joint_trajectory_point(state_current, joint_num); + controller_interface::return_type JointTrajectoryController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + return controller_interface::return_type::OK; + } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&, joint_num](auto & joint_interface, const std::vector & trajectory_point_interface) { - for (size_t index = 0; index < joint_num; ++index) + auto compute_error_for_joint = [&]( + JointTrajectoryPoint & error, int index, + const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) { + // error defined as the difference between current and desired + if (has_position_state_interface_ && has_position_command_interface_) + { + error.positions[index] = + angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + } + if (has_velocity_state_interface_ && has_velocity_command_interface_) + { + error.velocities[index] = desired.velocities[index] - current.velocities[index]; + } + if (has_acceleration_state_interface_ && has_acceleration_command_interface_) { - joint_interface[index].get().set_value(trajectory_point_interface[index]); + error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; } }; - // current state update - state_current.time_from_start.set__sec(0); - read_state_from_hardware(state_current); + // Check if a new external message has been received from nonRT threads + auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); + auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + if (current_external_msg != *new_external_msg) + { + fill_partial_goal(*new_external_msg); + sort_to_local_joint_order(*new_external_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(*new_external_msg); + } - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) - { - bool first_sample = false; - // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) + // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not + // changed, but its value only? + auto assign_interface_from_point = + [&](auto & joint_interface, const std::vector & trajectory_point_interface) { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; + + // current state update + state_current_.time_from_start.set__sec(0); + read_state_from_hardware(state_current_); + + // currently carrying out a trajectory + if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) { - first_sample = true; - if (open_loop_control_) + bool first_sample = false; + // if sampling the first time, set the point before you sample + if (!(*traj_point_active_ptr_)->is_sampled_already()) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + first_sample = true; + + + if (open_loop_control_) + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); + } + else + { + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); + } } - else + + // find segment for current timestamp + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + const bool valid_point = + (*traj_point_active_ptr_) + ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + + if (!valid_point) { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current); + return controller_interface::return_type::ERROR; } - } - resize_joint_trajectory_point(state_error, joint_num); - - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = - (*traj_point_active_ptr_)->sample(time, state_desired, start_segment_itr, end_segment_itr); - if (valid_point) - { bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; @@ -198,24 +219,24 @@ controller_interface::return_type JointTrajectoryController::update( const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); // Check state/goal tolerance - for (size_t index = 0; index < joint_num; ++index) + for (size_t index = 0; index < dof_; ++index) { - compute_error_for_joint(state_error, index, state_current, state_desired); + compute_error_for_joint(state_error_, index, state_current_, state_desired_); // Always check the state tolerance on the first sample in case the first sample // is the last point if ( - (before_last_point || first_sample) && - !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.state_tolerance[index], false)) + (before_last_point || first_sample) && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.state_tolerance[index], false)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && - !check_state_tolerance_per_joint( - state_error, index, default_tolerances_.goal_state_tolerance[index], false)) + !before_last_point && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) { outside_goal_tolerance = true; @@ -241,20 +262,20 @@ controller_interface::return_type JointTrajectoryController::update( if (use_closed_loop_pid_adapter_) { // Update PIDs - for (auto i = 0ul; i < joint_num; ++i) + for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( - state_desired.positions[i] - state_current.positions[i], - state_desired.velocities[i] - state_current.velocities[i], - (uint64_t)period.nanoseconds()); + state_desired_.positions[i] - state_current_.positions[i], + state_desired_.velocities[i] - state_current_.velocities[i], + (uint64_t)period.nanoseconds()); } } // set values for next hardware write() if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired.positions); + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); } if (has_velocity_command_interface_) { @@ -264,12 +285,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); } } if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); } if (has_effort_command_interface_) { @@ -279,12 +300,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[3], state_desired.effort); + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); } } - // store command as state when hardware state has tracking offset - last_commanded_state_ = state_desired; + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; } const auto active_goal = *rt_active_goal_.readFromRT(); @@ -295,9 +316,9 @@ controller_interface::return_type JointTrajectoryController::update( feedback->header.stamp = time; feedback->joint_names = joint_names_; - feedback->actual = state_current; - feedback->desired = state_desired; - feedback->error = state_error; + feedback->actual = state_current_; + feedback->desired = state_desired_; + feedback->error = state_error_; active_goal->setFeedback(feedback); // check abort @@ -305,7 +326,6 @@ controller_interface::return_type JointTrajectoryController::update( { set_hold_position(); auto result = std::make_shared(); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); active_goal->setAborted(result); @@ -338,1001 +358,1026 @@ controller_interface::return_type JointTrajectoryController::update( // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + time_difference); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied or violated within the goal_time_tolerance } } } + + publish_state(state_desired_, state_current_, state_error_); + return controller_interface::return_type::OK; } - publish_state(state_desired, state_current, state_error); - return controller_interface::return_type::OK; -} + void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) + { + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) -{ - const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < joint_num; ++index) + // Assign values from the hardware + // Position states always exist + assign_point_from_interface(state.positions, joint_state_interface_[0]); + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + assign_point_from_interface(state.velocities, joint_state_interface_[1]); + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + assign_point_from_interface(state.accelerations, joint_state_interface_[2]); + } + else + { + // Make empty so the property is ignored during interpolation + state.accelerations.clear(); } - }; - - // Assign values from the hardware - // Position states always exist - assign_point_from_interface(state.positions, joint_state_interface_[0]); - // velocity and acceleration states are optional - if (has_velocity_state_interface_) - { - assign_point_from_interface(state.velocities, joint_state_interface_[1]); - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) - { - assign_point_from_interface(state.accelerations, joint_state_interface_[2]); } else { // Make empty so the property is ignored during interpolation + state.velocities.clear(); state.accelerations.clear(); } } - else + + bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) { - // Make empty so the property is ignored during interpolation - state.velocities.clear(); - state.accelerations.clear(); - } -} + bool has_values = true; -bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) -{ - bool has_values = true; + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; - const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = - [&, joint_num](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < joint_num; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } + auto interface_has_values = [](const auto & joint_interface) { + return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { + return std::isnan(interface.get().get_value()); + }) == joint_interface.end(); }; - auto interface_has_values = [](const auto & joint_interface) { - return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { - return std::isnan(interface.get().get_value()); - }) == joint_interface.end(); - }; - - // Assign values from the command interfaces as state. Therefore needs check for both. - // Position state interface has to exist always - if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) - { - assign_point_from_interface(state.positions, joint_command_interface_[0]); - } - else - { - state.positions.clear(); - has_values = false; - } - // velocity and acceleration states are optional - if (has_velocity_state_interface_) - { - if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) + // Assign values from the command interfaces as state. Therefore needs check for both. + // Position state interface has to exist always + if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) { - assign_point_from_interface(state.velocities, joint_command_interface_[1]); + assign_point_from_interface(state.positions, joint_command_interface_[0]); } else { - state.velocities.clear(); + state.positions.clear(); has_values = false; } - } - else - { - state.velocities.clear(); - } - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) - { - if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) + // velocity and acceleration states are optional + if (has_velocity_state_interface_) { - assign_point_from_interface(state.accelerations, joint_command_interface_[2]); + if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) + { + assign_point_from_interface(state.velocities, joint_command_interface_[1]); + } + else + { + state.velocities.clear(); + has_values = false; + } + } + else + { + state.velocities.clear(); + } + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) + { + if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) + { + assign_point_from_interface(state.accelerations, joint_command_interface_[2]); + } + else + { + state.accelerations.clear(); + has_values = false; + } } else { state.accelerations.clear(); - has_values = false; } - } - else - { - state.accelerations.clear(); - } - - return has_values; -} -controller_interface::CallbackReturn JointTrajectoryController::on_configure( - const rclcpp_lifecycle::State &) -{ - const auto logger = get_node()->get_logger(); - - // update parameters - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - // TODO(destogl): why is this here? Add comment or move - if (!reset()) - { - return CallbackReturn::FAILURE; - } - - if (joint_names_.empty()) - { - // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? - RCLCPP_WARN(logger, "'joints' parameter is empty."); - } - - command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); - - if (command_joint_names_.empty()) - { - command_joint_names_ = joint_names_; - RCLCPP_INFO( - logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); - } - else if (command_joint_names_.size() != joint_names_.size()) - { - RCLCPP_ERROR( - logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); - return CallbackReturn::FAILURE; + return has_values; } - // Specialized, child controllers set interfaces before calling configure function. - if (command_interface_types_.empty()) + controller_interface::CallbackReturn JointTrajectoryController::on_configure( + const rclcpp_lifecycle::State &) { - command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); - } - - if (command_interface_types_.empty()) - { - RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); - return CallbackReturn::FAILURE; - } + const auto logger = get_node()->get_logger(); - // Check if only allowed interface types are used and initialize storage to avoid memory - // allocation during activation - joint_command_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) + // update parameters + joint_names_ = get_node()->get_parameter("joints").as_string_array(); + if ((dof_ > 0) && (joint_names_.size() != dof_)) { - RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); + RCLCPP_ERROR( + logger, + "The JointTrajectoryController does not support restarting with a different number of DOF"); + // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we + // can continue return CallbackReturn::FAILURE; } - } - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 2. position [velocity, [acceleration]] - - has_position_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); - has_velocity_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); - has_acceleration_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); - has_effort_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); - - if (has_velocity_command_interface_) - { - // if there is only velocity then use also PID adapter - if (command_interface_types_.size() == 1) + dof_ = joint_names_.size(); + + // TODO(destogl): why is this here? Add comment or move + if (!reset()) { - use_closed_loop_pid_adapter_ = true; + return CallbackReturn::FAILURE; } - else if (!has_position_command_interface_) + + if (joint_names_.empty()) { - RCLCPP_ERROR( - logger, - "'velocity' command interface can be used either alone or 'position' " - "interface has to be present."); - return CallbackReturn::FAILURE; + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? + RCLCPP_WARN(logger, "'joints' parameter is empty."); } - // invalid: acceleration is defined and no velocity - } - else if (has_acceleration_command_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); - return CallbackReturn::FAILURE; - } - // effort can't be combined with other interfaces - if (has_effort_command_interface_) - { - if (command_interface_types_.size() == 1) + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) { - use_closed_loop_pid_adapter_ = true; + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); } - else + else if (command_joint_names_.size() != joint_names_.size()) { - RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); return CallbackReturn::FAILURE; } - } - if (use_closed_loop_pid_adapter_) - { - size_t num_joints = joint_names_.size(); - pids_.resize(num_joints); - ff_velocity_scale_.resize(num_joints); - tmp_command_.resize(num_joints, 0.0); - - // Init PID gains from ROS parameter server - for (size_t i = 0; i < pids_.size(); ++i) - { - const std::string prefix = "gains." + joint_names_[i]; - const auto k_p = auto_declare(prefix + ".p", 0.0); - const auto k_i = auto_declare(prefix + ".i", 0.0); - const auto k_d = auto_declare(prefix + ".d", 0.0); - const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + joint_names_[i], 0.0); - // Initialize PID - pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); + // Specialized, child controllers set interfaces before calling configure function. + if (command_interface_types_.empty()) + { + command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); } - } - - // Read always state interfaces from the parameter because they can be used - // independently from the controller's type. - // Specialized, child controllers should set its default value. - state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); - - if (state_interface_types_.empty()) - { - RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); - return CallbackReturn::FAILURE; - } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) - { - RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); - return CallbackReturn::FAILURE; - } - - // Check if only allowed interface types are used and initialize storage to avoid memory - // allocation during activation - // Note: 'effort' storage is also here, but never used. Still, for this is OK. - joint_state_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : state_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) + if (command_interface_types_.empty()) { - RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); + RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); return CallbackReturn::FAILURE; } - } - - has_position_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); - has_velocity_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); - has_acceleration_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); - if (has_velocity_state_interface_) - { - if (!has_position_state_interface_) + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : command_interface_types_) { - RCLCPP_ERROR( - logger, - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - return CallbackReturn::FAILURE; + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } } - } - else - { - if (has_acceleration_state_interface_) + + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 2. position [velocity, [acceleration]] + + has_position_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); + + if (has_velocity_command_interface_) { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; + // if there is only velocity then use also PID adapter + if (command_interface_types_.size() == 1) + { + use_closed_loop_pid_adapter_ = true; + } + else if (!has_position_command_interface_) + { + RCLCPP_ERROR( + logger, + "'velocity' command interface can be used either alone or 'position' " + "interface has to be present."); + return CallbackReturn::FAILURE; + } + // invalid: acceleration is defined and no velocity } - if (has_velocity_command_interface_ && command_interface_types_.size() == 1) + else if (has_acceleration_command_interface_) { RCLCPP_ERROR( - logger, - "'velocity' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); + logger, + "'acceleration' command interface can only be used if 'velocity' and " + "'position' interfaces are present"); return CallbackReturn::FAILURE; } - // effort is always used alone so no need for size check + + // effort can't be combined with other interfaces if (has_effort_command_interface_) { - RCLCPP_ERROR( - logger, - "'effort' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; + if (command_interface_types_.size() == 1) + { + use_closed_loop_pid_adapter_ = true; + } + else + { + RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); + return CallbackReturn::FAILURE; + } } - } - auto get_interface_list = [](const std::vector & interface_types) { - std::stringstream ss_interfaces; - for (size_t index = 0; index < interface_types.size(); ++index) + if (use_closed_loop_pid_adapter_) { - if (index != 0) + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + tmp_command_.resize(dof_, 0.0); + + // Init PID gains from ROS parameter server + for (size_t i = 0; i < pids_.size(); ++i) { - ss_interfaces << " "; + const std::string prefix = "gains." + command_joint_names_[i]; + const auto k_p = auto_declare(prefix + ".p", 0.0); + const auto k_i = auto_declare(prefix + ".i", 0.0); + const auto k_d = auto_declare(prefix + ".d", 0.0); + const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); + ff_velocity_scale_[i] = + auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); + // Initialize PID + pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); } - ss_interfaces << interface_types[index]; } - return ss_interfaces.str(); - }; - // Print output so users can be sure the interface setup is correct - RCLCPP_INFO( - logger, "Command interfaces are [%s] and state interfaces are [%s].", - get_interface_list(command_interface_types_).c_str(), - get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_); + // Read always state interfaces from the parameter because they can be used + // independently from the controller's type. + // Specialized, child controllers should set its default value. + state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); - // Read parameters customizing controller for special cases - open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); - allow_integration_in_goal_trajectories_ = - get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + if (state_interface_types_.empty()) + { + RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); + return CallbackReturn::FAILURE; + } - // subscriber callback - // non realtime - // TODO(karsten): check if traj msg and point time are valid - auto callback = [this](const std::shared_ptr msg) -> void { - if (!validate_trajectory_msg(*msg)) + if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) { - return; + RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); + return CallbackReturn::FAILURE; } - // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement - // always replace old msg with new one for now - if (subscriber_is_active_) + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + // Note: 'effort' storage is also here, but never used. Still, for this is OK. + joint_state_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : state_interface_types_) { - add_new_trajectory_msg(msg); + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } } - }; - // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = - get_node()->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); + has_position_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); - // TODO(karsten1987): no lifecycle for subscriber yet - // joint_command_subscriber_->on_activate(); + if (has_velocity_state_interface_) + { + if (!has_position_state_interface_) + { + RCLCPP_ERROR( + logger, + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + return CallbackReturn::FAILURE; + } + } + else + { + if (has_acceleration_state_interface_) + { + RCLCPP_ERROR( + logger, + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + return CallbackReturn::FAILURE; + } + if (has_velocity_command_interface_ && command_interface_types_.size() == 1) + { + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } + // effort is always used alone so no need for size check + if (has_effort_command_interface_) + { + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } + } - // State publisher - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); - if (state_publish_rate_ > 0.0) - { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); - } - else - { - state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); - } + auto get_interface_list = [](const std::vector & interface_types) { + std::stringstream ss_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) + { + if (index != 0) + { + ss_interfaces << " "; + } + ss_interfaces << interface_types[index]; + } + return ss_interfaces.str(); + }; - publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = std::make_unique(publisher_); + // Print output so users can be sure the interface setup is correct + RCLCPP_INFO( + logger, "Command interfaces are [%s] and state interfaces are [%s].", + get_interface_list(command_interface_types_).c_str(), + get_interface_list(state_interface_types_).c_str()); - const auto n_joints = joint_names_.size(); + default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); - state_publisher_->lock(); - state_publisher_->msg_.joint_names = joint_names_; - state_publisher_->msg_.desired.positions.resize(n_joints); - state_publisher_->msg_.desired.velocities.resize(n_joints); - state_publisher_->msg_.desired.accelerations.resize(n_joints); - state_publisher_->msg_.actual.positions.resize(n_joints); - state_publisher_->msg_.error.positions.resize(n_joints); - if (has_velocity_state_interface_) - { - state_publisher_->msg_.actual.velocities.resize(n_joints); - state_publisher_->msg_.error.velocities.resize(n_joints); - } - if (has_acceleration_state_interface_) - { - state_publisher_->msg_.actual.accelerations.resize(n_joints); - state_publisher_->msg_.error.accelerations.resize(n_joints); - } - state_publisher_->unlock(); + // Read parameters customizing controller for special cases + open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); + allow_integration_in_goal_trajectories_ = + get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); - last_state_publish_time_ = get_node()->now(); + const std::string interpolation_string = + get_node()->get_parameter("interpolation_method").as_string(); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + RCLCPP_INFO( + logger, "Using '%s' interpolation method.", + interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + + // subscriber callback + // non realtime + // TODO(karsten): check if traj msg and point time are valid + auto callback = [this](const std::shared_ptr msg) -> void { + if (!validate_trajectory_msg(*msg)) + { + return; + } - // action server configuration - allow_partial_joints_goal_ = - get_node()->get_parameter("allow_partial_joints_goal").get_value(); - if (allow_partial_joints_goal_) - { - RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); - } + // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement + // always replace old msg with new one for now + if (subscriber_is_active_) + { + add_new_trajectory_msg(msg); + } + }; - RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); + // TODO(karsten1987): create subscriber with subscription deactivated + joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); - using namespace std::placeholders; - action_server_ = rclcpp_action::create_server( - get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), - get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), - std::string(get_node()->get_name()) + "/follow_joint_trajectory", - std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), - std::bind(&JointTrajectoryController::cancel_callback, this, _1), - std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); + // TODO(karsten1987): no lifecycle for subscriber yet + // joint_command_subscriber_->on_activate(); - return CallbackReturn::SUCCESS; -} + // State publisher + state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); + if (state_publish_rate_ > 0.0) + { + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); + } + else + { + state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); + } -controller_interface::CallbackReturn JointTrajectoryController::on_activate( - const rclcpp_lifecycle::State &) -{ - // order all joints in the storage - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); - if (!controller_interface::get_ordered_interfaces( - command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(publisher_); + + state_publisher_->lock(); + state_publisher_->msg_.joint_names = command_joint_names_; + state_publisher_->msg_.desired.positions.resize(dof_); + state_publisher_->msg_.desired.velocities.resize(dof_); + state_publisher_->msg_.desired.accelerations.resize(dof_); + state_publisher_->msg_.actual.positions.resize(dof_); + state_publisher_->msg_.error.positions.resize(dof_); + if (has_velocity_state_interface_) { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", - command_joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); - return controller_interface::CallbackReturn::ERROR; + state_publisher_->msg_.actual.velocities.resize(dof_); + state_publisher_->msg_.error.velocities.resize(dof_); } - } - for (const auto & interface : state_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); - if (!controller_interface::get_ordered_interfaces( - state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + if (has_acceleration_state_interface_) { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); - return controller_interface::CallbackReturn::ERROR; + state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.error.accelerations.resize(dof_); } - } + state_publisher_->unlock(); - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) - { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); - } + last_state_publish_time_ = get_node()->now(); - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); - - subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = get_node()->now(); - - // Initialize current state storage if hardware state has tracking offset - resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading last_commanded_state_ from commands is - // those are not nan - trajectory_msgs::msg::JointTrajectoryPoint state; - resize_joint_trajectory_point(state, joint_names_.size()); - if (read_state_from_command_interfaces(state)) - { - last_commanded_state_ = state; - } + // action server configuration + allow_partial_joints_goal_ = + get_node()->get_parameter("allow_partial_joints_goal").get_value(); + if (allow_partial_joints_goal_) + { + RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); + } - // TODO(karsten1987): activate subscriptions of subscriber - return CallbackReturn::SUCCESS; -} + action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); + RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); -controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( - const rclcpp_lifecycle::State &) -{ - // TODO(anyone): How to halt when using effort commands? - for (size_t index = 0; index < joint_names_.size(); ++index) + using namespace std::placeholders; + action_server_ = rclcpp_action::create_server( + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string(get_node()->get_name()) + "/follow_joint_trajectory", + std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), + std::bind(&JointTrajectoryController::cancel_callback, this, _1), + std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); + + resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point(state_desired_, dof_); + resize_joint_trajectory_point(state_error_, dof_); + resize_joint_trajectory_point(last_commanded_state_, dof_); + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn JointTrajectoryController::on_activate( + const rclcpp_lifecycle::State &) { - if (has_position_command_interface_) + // order all joints in the storage + for (const auto & interface : command_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; + } + } + for (const auto & interface : state_interface_types_) { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + interface.c_str(), joint_state_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; + } } - if (has_velocity_command_interface_) + // Store 'home' pose + traj_msg_home_ptr_ = std::make_shared(); + traj_msg_home_ptr_->header.stamp.sec = 0; + traj_msg_home_ptr_->header.stamp.nanosec = 0; + traj_msg_home_ptr_->points.resize(1); + traj_msg_home_ptr_->points[0].time_from_start.sec = 0; + traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; + traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); + for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) { - joint_command_interface_[1][index].get().set_value(0.0); + traj_msg_home_ptr_->points[0].positions[index] = + joint_state_interface_[0][index].get().get_value(); } - if (has_effort_command_interface_) + traj_external_point_ptr_ = std::make_shared(); + traj_home_point_ptr_ = std::make_shared(); + traj_msg_external_point_ptr_.writeFromNonRT( + std::shared_ptr()); + + subscriber_is_active_ = true; + traj_point_active_ptr_ = &traj_external_point_ptr_; + last_state_publish_time_ = get_node()->now(); + + // Initialize current state storage if hardware state has tracking offset + read_state_from_hardware(state_current_); + read_state_from_hardware(state_desired_); + read_state_from_hardware(last_commanded_state_); + // Handle restart of controller by reading from commands if + // those are not nan + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + if (read_state_from_command_interfaces(state)) { - joint_command_interface_[3][index].get().set_value(0.0); + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; } + + // TODO(karsten1987): activate subscriptions of subscriber + return CallbackReturn::SUCCESS; } - for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( + const rclcpp_lifecycle::State &) { - joint_command_interface_[index].clear(); - joint_state_interface_[index].clear(); - } - release_interfaces(); + // TODO(anyone): How to halt when using effort commands? + for (size_t index = 0; index < dof_; ++index) + { + if (has_position_command_interface_) + { + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); + } - subscriber_is_active_ = false; + if (has_velocity_command_interface_) + { + joint_command_interface_[1][index].get().set_value(0.0); + } - return CallbackReturn::SUCCESS; -} + if (has_effort_command_interface_) + { + joint_command_interface_[3][index].get().set_value(0.0); + } + } -controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( - const rclcpp_lifecycle::State &) -{ - // go home -// traj_home_point_ptr_->update(traj_msg_home_ptr_); -// traj_point_active_ptr_ = &traj_home_point_ptr_; + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); - return CallbackReturn::SUCCESS; -} + subscriber_is_active_ = false; -controller_interface::CallbackReturn JointTrajectoryController::on_error( - const rclcpp_lifecycle::State &) -{ - if (!reset()) - { - return CallbackReturn::ERROR; + return CallbackReturn::SUCCESS; } - return CallbackReturn::SUCCESS; -} - -bool JointTrajectoryController::reset() -{ - subscriber_is_active_ = false; - joint_command_subscriber_.reset(); - for (const auto & pid : pids_) + controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( + const rclcpp_lifecycle::State &) { - pid->reset(); - } + // go home + traj_home_point_ptr_->update(traj_msg_home_ptr_); + traj_point_active_ptr_ = &traj_home_point_ptr_; - // iterator has no default value - // prev_traj_point_ptr_; - traj_point_active_ptr_ = nullptr; - traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); + return CallbackReturn::SUCCESS; + } - // reset pids - for (const auto & pid : pids_) + controller_interface::CallbackReturn JointTrajectoryController::on_error( + const rclcpp_lifecycle::State &) { - pid->reset(); + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; } - return true; -} + bool JointTrajectoryController::reset() + { + subscriber_is_active_ = false; + joint_command_subscriber_.reset(); -controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( - const rclcpp_lifecycle::State &) -{ - // TODO(karsten1987): what to do? + for (const auto & pid : pids_) + { + pid->reset(); + } - return CallbackReturn::SUCCESS; -} + // iterator has no default value + // prev_traj_point_ptr_; + traj_point_active_ptr_ = nullptr; + traj_external_point_ptr_.reset(); + traj_home_point_ptr_.reset(); + traj_msg_home_ptr_.reset(); -void JointTrajectoryController::publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error) -{ - if (state_publisher_period_.seconds() <= 0.0) - { - return; + // reset pids + for (const auto & pid : pids_) + { + pid->reset(); + } + + return true; } - if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) + controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( + const rclcpp_lifecycle::State &) { - return; + // TODO(karsten1987): what to do? + + return CallbackReturn::SUCCESS; } - if (state_publisher_ && state_publisher_->trylock()) + void JointTrajectoryController::publish_state( + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error) { - last_state_publish_time_ = get_node()->now(); - state_publisher_->msg_.header.stamp = last_state_publish_time_; - state_publisher_->msg_.desired.positions = desired_state.positions; - state_publisher_->msg_.desired.velocities = desired_state.velocities; - state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - state_publisher_->msg_.actual.positions = current_state.positions; - state_publisher_->msg_.error.positions = state_error.positions; - if (has_velocity_state_interface_) + if (state_publisher_period_.seconds() <= 0.0) { - state_publisher_->msg_.actual.velocities = current_state.velocities; - state_publisher_->msg_.error.velocities = state_error.velocities; + return; } - if (has_acceleration_state_interface_) + + if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) { - state_publisher_->msg_.actual.accelerations = current_state.accelerations; - state_publisher_->msg_.error.accelerations = state_error.accelerations; + return; } - state_publisher_->unlockAndPublish(); - } -} - -rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( - const rclcpp_action::GoalUUID &, std::shared_ptr goal) -{ - RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); + if (state_publisher_ && state_publisher_->trylock()) + { + last_state_publish_time_ = get_node()->now(); + state_publisher_->msg_.header.stamp = last_state_publish_time_; + state_publisher_->msg_.desired.positions = desired_state.positions; + state_publisher_->msg_.desired.velocities = desired_state.velocities; + state_publisher_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_->msg_.actual.positions = current_state.positions; + state_publisher_->msg_.error.positions = state_error.positions; + if (has_velocity_state_interface_) + { + state_publisher_->msg_.actual.velocities = current_state.velocities; + state_publisher_->msg_.error.velocities = state_error.velocities; + } + if (has_acceleration_state_interface_) + { + state_publisher_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_->msg_.error.accelerations = state_error.accelerations; + } - // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); - return rclcpp_action::GoalResponse::REJECT; + state_publisher_->unlockAndPublish(); + } } - if (!validate_trajectory_msg(goal->trajectory)) + rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal) { - return rclcpp_action::GoalResponse::REJECT; - } + RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); - RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} + // Precondition: Running controller + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } -rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( - const std::shared_ptr> goal_handle) -{ - RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + if (!validate_trajectory_msg(goal->trajectory)) + { + return rclcpp_action::GoalResponse::REJECT; + } - // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal && active_goal->gh_ == goal_handle) + RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( + const std::shared_ptr> goal_handle) { - // Controller uptime - // Enter hold current position mode - set_hold_position(); + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + // Controller uptime + // Enter hold current position mode + set_hold_position(); - RCLCPP_DEBUG( - get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + RCLCPP_DEBUG( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); - // Mark the current goal as canceled - auto action_res = std::make_shared(); - active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; } - return rclcpp_action::CancelResponse::ACCEPT; -} -void JointTrajectoryController::feedback_setup_callback( - std::shared_ptr> goal_handle) -{ - // Update new trajectory + void JointTrajectoryController::feedback_setup_callback( + std::shared_ptr> goal_handle) { - preempt_active_goal(); - auto traj_msg = - std::make_shared(goal_handle->get_goal()->trajectory); + // Update new trajectory + { + preempt_active_goal(); + auto traj_msg = + std::make_shared(goal_handle->get_goal()->trajectory); - add_new_trajectory_msg(traj_msg); - } + add_new_trajectory_msg(traj_msg); + } - // Update the active goal - RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); - rt_goal->preallocated_feedback_->joint_names = joint_names_; - rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); + // Update the active goal + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->preallocated_feedback_->joint_names = joint_names_; + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); - // Setup goal status checking timer - goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); -} + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + } -void JointTrajectoryController::fill_partial_goal( - std::shared_ptr trajectory_msg) const -{ - // joint names in the goal are a subset of existing joints, as checked in goal_callback - // so if the size matches, the goal contains all controller joints - if (joint_names_.size() == trajectory_msg->joint_names.size()) + void JointTrajectoryController::fill_partial_goal( + std::shared_ptr trajectory_msg) const { - return; - } + // joint names in the goal are a subset of existing joints, as checked in goal_callback + // so if the size matches, the goal contains all controller joints + if (dof_ == trajectory_msg->joint_names.size()) + { + return; + } - trajectory_msg->joint_names.reserve(joint_names_.size()); + trajectory_msg->joint_names.reserve(dof_); - for (size_t index = 0; index < joint_names_.size(); ++index) - { + for (size_t index = 0; index < dof_; ++index) { - if ( - std::find( - trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), - joint_names_[index]) != trajectory_msg->joint_names.end()) { - // joint found on msg - continue; - } - trajectory_msg->joint_names.push_back(joint_names_[index]); + if ( + std::find( + trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), + joint_names_[index]) != trajectory_msg->joint_names.end()) + { + // joint found on msg + continue; + } + trajectory_msg->joint_names.push_back(joint_names_[index]); - for (auto & it : trajectory_msg->points) - { - // Assume hold position with 0 velocity and acceleration for missing joints - if (!it.positions.empty()) + for (auto & it : trajectory_msg->points) { - if ( - has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) + // Assume hold position with 0 velocity and acceleration for missing joints + if (!it.positions.empty()) { - // copy last command if cmd interface exists - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + if ( + has_position_command_interface_ && + !std::isnan(joint_command_interface_[0][index].get().get_value())) + { + // copy last command if cmd interface exists + it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + } + else if (has_position_state_interface_) + { + // copy current state if state interface exists + it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + } } - else if (has_position_state_interface_) + if (!it.velocities.empty()) { - // copy current state if state interface exists - it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + it.velocities.push_back(0.0); + } + if (!it.accelerations.empty()) + { + it.accelerations.push_back(0.0); + } + if (!it.effort.empty()) + { + it.effort.push_back(0.0); } - } - if (!it.velocities.empty()) - { - it.velocities.push_back(0.0); - } - if (!it.accelerations.empty()) - { - it.accelerations.push_back(0.0); - } - if (!it.effort.empty()) - { - it.effort.push_back(0.0); } } } } -} - -void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) -{ - // rearrange all points in the trajectory message based on mapping - std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); - auto remap = [this]( - const std::vector & to_remap, - const std::vector & mapping) -> std::vector { - if (to_remap.empty()) - { - return to_remap; - } - if (to_remap.size() != mapping.size()) - { - RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); - return to_remap; - } - std::vector output; - output.resize(mapping.size(), 0.0); - for (size_t index = 0; index < mapping.size(); ++index) - { - auto map_index = mapping[index]; - output[map_index] = to_remap[index]; - } - return output; - }; - for (size_t index = 0; index < trajectory_msg->points.size(); ++index) + void JointTrajectoryController::sort_to_local_joint_order( + std::shared_ptr trajectory_msg) { - trajectory_msg->points[index].positions = - remap(trajectory_msg->points[index].positions, mapping_vector); - - trajectory_msg->points[index].velocities = - remap(trajectory_msg->points[index].velocities, mapping_vector); + // rearrange all points in the trajectory message based on mapping + std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); + auto remap = [this]( + const std::vector & to_remap, + const std::vector & mapping) -> std::vector { + if (to_remap.empty()) + { + return to_remap; + } + if (to_remap.size() != mapping.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + return to_remap; + } + std::vector output; + output.resize(mapping.size(), 0.0); + for (size_t index = 0; index < mapping.size(); ++index) + { + auto map_index = mapping[index]; + output[map_index] = to_remap[index]; + } + return output; + }; - trajectory_msg->points[index].accelerations = - remap(trajectory_msg->points[index].accelerations, mapping_vector); + for (size_t index = 0; index < trajectory_msg->points.size(); ++index) + { + trajectory_msg->points[index].positions = + remap(trajectory_msg->points[index].positions, mapping_vector); - trajectory_msg->points[index].effort = - remap(trajectory_msg->points[index].effort, mapping_vector); - } -} + trajectory_msg->points[index].velocities = + remap(trajectory_msg->points[index].velocities, mapping_vector); -bool JointTrajectoryController::validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const -{ - if (allow_empty && vector_field.empty()) - { - return true; - } - if (joint_names_size != vector_field.size()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); - return false; - } - return true; -} + trajectory_msg->points[index].accelerations = + remap(trajectory_msg->points[index].accelerations, mapping_vector); -bool JointTrajectoryController::validate_trajectory_msg( - const trajectory_msgs::msg::JointTrajectory & trajectory) const -{ - // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) - { - if (trajectory.joint_names.size() != joint_names_.size()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Joints on incoming trajectory don't match the controller joints."); - return false; + trajectory_msg->points[index].effort = + remap(trajectory_msg->points[index].effort, mapping_vector); } } - if (trajectory.joint_names.empty()) + bool JointTrajectoryController::validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const { - RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); - return false; - } - - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) - { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) + if (allow_empty && vector_field.empty()) { - trajectory_end_time += p.time_from_start; + return true; } - if (trajectory_end_time < get_node()->now()) + if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non zero time start time (%f) that ends on the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); + get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; } + return true; } - for (size_t i = 0; i < trajectory.joint_names.size(); ++i) + bool JointTrajectoryController::validate_trajectory_msg( + const trajectory_msgs::msg::JointTrajectory & trajectory) const { - const std::string & incoming_joint_name = trajectory.joint_names[i]; - - auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); - if (it == joint_names_.end()) + // If partial joints goals are not allowed, goal should specify all controller joints + if (!allow_partial_joints_goal_) { - RCLCPP_ERROR( - get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", - incoming_joint_name.c_str()); - return false; + if (trajectory.joint_names.size() != dof_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Joints on incoming trajectory don't match the controller joints."); + return false; + } } - } - rclcpp::Duration previous_traj_time(0ms); - for (size_t i = 0; i < trajectory.points.size(); ++i) - { - if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) + if (trajectory.joint_names.empty()) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", - i - 1, i, previous_traj_time.seconds(), - rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); + RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); return false; } - previous_traj_time = trajectory.points[i].time_from_start; - - const size_t joint_count = trajectory.joint_names.size(); - const auto & points = trajectory.points; - // This currently supports only position, velocity and acceleration inputs - if (allow_integration_in_goal_trajectories_) - { - const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && - points[i].accelerations.empty(); - const bool position_error = - !points[i].positions.empty() && - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); - const bool velocity_error = - !points[i].velocities.empty() && - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); - const bool acceleration_error = - !points[i].accelerations.empty() && - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, false); - if (all_empty || position_error || velocity_error || acceleration_error) + + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) { + trajectory_end_time += p.time_from_start; + } + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non zero time start time (%f) that ends on the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; } } - else if ( - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true) || - !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) + +// for (size_t i = 0; i < trajectory.joint_names.size(); ++i) +// { +// const std::string & incoming_joint_name = trajectory.joint_names[i]; +// +// auto it = +// std::find(command_joint_names_.begin(), command_joint_names_.end(), incoming_joint_name); +// if (it == command_joint_names_.end()) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", +// incoming_joint_name.c_str()); +// return false; +// } +// } + + rclcpp::Duration previous_traj_time(0ms); + for (size_t i = 0; i < trajectory.points.size(); ++i) { - return false; + if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", + i - 1, i, previous_traj_time.seconds(), + rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); + return false; + } + previous_traj_time = trajectory.points[i].time_from_start; + + const size_t joint_count = trajectory.joint_names.size(); + const auto & points = trajectory.points; + // This currently supports only position, velocity and acceleration inputs + if (allow_integration_in_goal_trajectories_) + { + const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty(); + const bool position_error = + !points[i].positions.empty() && + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); + const bool velocity_error = + !points[i].velocities.empty() && + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); + const bool acceleration_error = + !points[i].accelerations.empty() && + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, false); + if (all_empty || position_error || velocity_error || acceleration_error) + { + return false; + } + } + else if ( + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, true) || + !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) + { + return false; + } } + return true; } - return true; -} -void JointTrajectoryController::add_new_trajectory_msg( - const std::shared_ptr & traj_msg) -{ - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); -} - -void JointTrajectoryController::preempt_active_goal() -{ - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal) + void JointTrajectoryController::add_new_trajectory_msg( + const std::shared_ptr & traj_msg) { - set_hold_position(); - auto action_res = std::make_shared(); - action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); - action_res->set__error_string("Current goal cancelled due to new incoming action."); - active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); } -} -void JointTrajectoryController::set_hold_position() -{ - trajectory_msgs::msg::JointTrajectory empty_msg; - empty_msg.header.stamp = rclcpp::Time(0); + void JointTrajectoryController::preempt_active_goal() + { + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + set_hold_position(); + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled due to new incoming action."); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + } - auto traj_msg = std::make_shared(empty_msg); - add_new_trajectory_msg(traj_msg); -} + void JointTrajectoryController::set_hold_position() + { + trajectory_msgs::msg::JointTrajectory empty_msg; + empty_msg.header.stamp = rclcpp::Time(0); -bool JointTrajectoryController::contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type) -{ - return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != - interface_type_list.end(); -} + auto traj_msg = std::make_shared(empty_msg); + add_new_trajectory_msg(traj_msg); + } -void JointTrajectoryController::resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) -{ - point.positions.resize(size); - if (has_velocity_state_interface_) + bool JointTrajectoryController::contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type) { - point.velocities.resize(size); + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); } - if (has_acceleration_state_interface_) + + void JointTrajectoryController::resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) { - point.accelerations.resize(size); + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_state_interface_) + { + point.accelerations.resize(size, 0.0); + } } -} } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) + joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) \ No newline at end of file From bdb32b444ef2ef188d182dfc903cd7da34c512af Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 5 Aug 2022 10:03:57 -0600 Subject: [PATCH 047/111] mend --- .../include/admittance_controller/admittance_controller.hpp | 4 +++- .../config/admittance_controller_parameters.hpp | 2 +- .../src/admittance_controller_parameters.yaml | 2 +- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 2 +- tricycle_controller/config/tricycle_drive_controller.yaml | 4 ++-- tricycle_controller/doc/userdoc.rst | 6 +++--- .../include/tricycle_controller/tricycle_controller.hpp | 2 +- tricycle_controller/package.xml | 2 +- tricycle_controller/src/tricycle_controller.cpp | 6 +++--- .../test/config/test_tricycle_controller.yaml | 2 +- 11 files changed, 18 insertions(+), 16 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 2ea88586b2..40cfeb2461 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -52,7 +52,9 @@ namespace admittance_controller { class AdmittanceController : public controller_interface::ChainableControllerInterface { public: ADMITTANCE_CONTROLLER_PUBLIC - AdmittanceController() = default; + AdmittanceController() { + int o =0; + }; ADMITTANCE_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp index 60f5a5dce6..b801463bac 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp +++ b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp @@ -697,4 +697,4 @@ namespace admittance_controller { std::shared_ptr parameters_interface_; }; -} // namespace admittance_controller \ No newline at end of file +} // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index b43b2db24d..31a74b8565 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -166,4 +166,4 @@ admittance_controller: type: bool, default_value: true, description: "if enabled, parameters will be dynamically updated in the control loop" - } \ No newline at end of file + } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 59bcdac3c0..f7cd5b388b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -271,4 +271,4 @@ namespace joint_trajectory_controller } // namespace joint_trajectory_controller -#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ \ No newline at end of file +#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 753d836ef1..ea3d6b4668 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1380,4 +1380,4 @@ namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) \ No newline at end of file + joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index 4eabf615ef..c1077b13c3 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -1,4 +1,4 @@ -tricycle_controller: +tricycle_controller: ros__parameters: # Model traction_joint_name: traction_joint # Name of traction joint in URDF @@ -16,7 +16,7 @@ tricycle_controller: pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom - + # Rate Limiting traction: # All values should be positive # min_velocity: 0.0 diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 2271b8c9cf..a248bbec96 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,8 +10,8 @@ commands for the tricycle drive base. Odometry is computed from hardware feedbac Velocity commands ----------------- -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. @@ -21,4 +21,4 @@ Other features Realtime-safe implementation. Odometry publishing Velocity, acceleration and jerk limits - Automatic stop after command timeout \ No newline at end of file + Automatic stop after command timeout diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index c5ba1c62e3..959dcc9e69 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -125,7 +125,7 @@ class TricycleController : public controller_interface::ControllerInterface { bool open_loop = false; bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in seperate node + bool odom_only_twist = false; // for doing the pose integration in separate node std::string base_frame_id = "base_link"; std::string odom_frame_id = "odom"; std::array pose_covariance_diagonal; diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9dec765d60..df07cd4afe 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -32,4 +32,4 @@ ament_cmake - \ No newline at end of file + diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 7cec107a91..bfae724aef 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -151,7 +151,7 @@ controller_interface::return_type TricycleController::update( double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - + if (odom_params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); @@ -160,7 +160,7 @@ controller_interface::return_type TricycleController::update( { if (std::isnan(Ws_read) || std::isnan(alpha_read)) { - RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); return controller_interface::return_type::ERROR; } odometry_.update(Ws_read, alpha_read, period); @@ -208,7 +208,7 @@ controller_interface::return_type TricycleController::update( // Compute wheel velocity and angle auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); - // Reduce wheel speed until the target angle has been reached + // Reduce wheel speed until the target angle has been reached double alpha_delta = abs(alpha_write - alpha_read); double scale; if (alpha_delta < M_PI / 6) diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index d81d722219..efecfe968f 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -17,4 +17,4 @@ test_tricycle_controller: max_deceleration: 8.0 steering: max_position: 1.57 # pi/2 - max_velocity: 1.0 \ No newline at end of file + max_velocity: 1.0 From ed370619a958d3f48ae865130c75b4fe42267e71 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 5 Aug 2022 10:13:04 -0600 Subject: [PATCH 048/111] revert forward command controller changes --- forward_command_controller/CMakeLists.txt | 27 ---- .../forward_command_plugin.xml | 54 +------ .../chainable_forward_controller.hpp | 85 ----------- .../forward_command_controller.hpp | 84 ++--------- .../forward_controller.hpp | 67 --------- .../forward_controllers_base.hpp | 37 +++-- ...i_interface_forward_command_controller.hpp | 86 ++--------- .../src/chainable_forward_controller.cpp | 139 ------------------ .../src/forward_command_controller.cpp | 57 ++++++- .../src/forward_controller.cpp | 80 ---------- .../src/forward_controllers_base.cpp | 84 ++++++----- ...i_interface_forward_command_controller.cpp | 51 ++++++- .../test/test_forward_command_controller.cpp | 1 - .../test/test_forward_command_controller.hpp | 1 - ...oad_chained_forward_command_controller.cpp | 42 ------ ...i_interface_forward_command_controller.cpp | 41 ------ .../test_load_forward_command_controller.cpp | 3 - ...i_interface_forward_command_controller.cpp | 1 - ...i_interface_forward_command_controller.hpp | 1 - 19 files changed, 188 insertions(+), 753 deletions(-) delete mode 100644 forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp delete mode 100644 forward_command_controller/include/forward_command_controller/forward_controller.hpp delete mode 100644 forward_command_controller/src/chainable_forward_controller.cpp delete mode 100644 forward_command_controller/src/forward_controller.cpp delete mode 100644 forward_command_controller/test/test_load_chained_forward_command_controller.cpp delete mode 100644 forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index d0b763898a..f1d7a18328 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -28,8 +28,6 @@ endforeach() add_library(${PROJECT_NAME} SHARED - src/chainable_forward_controller.cpp - src/forward_controller.cpp src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp @@ -100,31 +98,6 @@ if(BUILD_TESTING) target_link_libraries(test_multi_interface_forward_command_controller ${PROJECT_NAME} ) - - ament_add_gmock( - test_load_chained_forward_command_controller - test/test_load_chained_forward_command_controller.cpp - ) - target_include_directories(test_load_chained_forward_command_controller PRIVATE include) - ament_target_dependencies( - test_load_chained_forward_command_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - - ament_add_gmock( - test_load_chained_multi_interface_forward_command_controller - test/test_load_chained_multi_interface_forward_command_controller.cpp - ) - target_include_directories(test_load_chained_multi_interface_forward_command_controller PRIVATE include) - ament_target_dependencies( - test_load_chained_multi_interface_forward_command_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - endif() ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 47d661d603..48fa29936d 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,61 +1,11 @@ - - - + The forward command controller commands a group of joints in a given interface - - MultiInterfaceForwardController ros2_control controller. - - - - - - The forward command controller commands a group of joints in a given interface - - - + type="forward_command_controller::MultiInterfaceForwardCommandController" base_class_type="controller_interface::ControllerInterface"> MultiInterfaceForwardController ros2_control controller. diff --git a/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp b/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp deleted file mode 100644 index e62b39237b..0000000000 --- a/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) -// -// 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 FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ -#define FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ - -#include -#include -#include - -#include "controller_interface/chainable_controller_interface.hpp" -#include "forward_command_controller/forward_controllers_base.hpp" -#include "forward_command_controller/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" - -namespace forward_command_controller -{ -using CmdType = std_msgs::msg::Float64MultiArray; - -/** - * \brief Forward command controller for a set of joints and interfaces. - * - * This class forwards the command signal down to a set of joints or interfaces. - * - * Subscribes to: - * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. - */ -class ChainableForwardController : public ForwardControllersBase, - public controller_interface::ChainableControllerInterface -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ChainableForwardController(); - - FORWARD_COMMAND_CONTROLLER_PUBLIC - ~ChainableForwardController() = default; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - -protected: - std::vector on_export_reference_interfaces() override; - - bool on_set_chained_mode(bool chained_mode) override; - - controller_interface::return_type update_reference_from_subscribers() override; - - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - std::vector reference_interface_names_; -}; - -} // namespace forward_command_controller - -#endif // FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 1b3c997a2c..f50af328d8 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -18,11 +18,8 @@ #include #include -#include "forward_command_controller/chainable_forward_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" -#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace forward_command_controller { @@ -37,78 +34,19 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ - template < - typename T, - typename std::enable_if< - std::is_convertible::value, - T>::type * = nullptr, - typename std::enable_if< - std::is_convertible::value, T>::type * = - nullptr> - class BaseForwardCommandController : public T - { - public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - BaseForwardCommandController() : T() {} - - protected: - void declare_parameters() override - { - controller_interface::ControllerInterfaceBase::auto_declare>( - "joints", std::vector()); - controller_interface::ControllerInterfaceBase::auto_declare("interface_name", ""); - }; - - controller_interface::CallbackReturn read_parameters() override - { - joint_names_ = T::get_node()->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) - { - RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) - { - interface_name_ = T::get_node()->get_parameter("interface_name").as_string(); - } - - if (interface_name_.empty()) - { - RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & joint : joint_names_) - { - T::command_interface_names_.push_back(joint + "/" + interface_name_); - } - - return controller_interface::CallbackReturn::SUCCESS; - }; - - std::vector joint_names_; - std::string interface_name_; - }; +class ForwardCommandController : public ForwardControllersBase +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardCommandController(); - class ForwardCommandController : public BaseForwardCommandController - { - public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ForwardCommandController() : BaseForwardCommandController() {} - }; +protected: + void declare_parameters() override; + controller_interface::CallbackReturn read_parameters() override; - class ChainableForwardCommandController - : public BaseForwardCommandController - { - public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ChainableForwardCommandController() : BaseForwardCommandController() - { - } - }; + std::vector joint_names_; + std::string interface_name_; +}; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/forward_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_controller.hpp deleted file mode 100644 index 5bf2d1f831..0000000000 --- a/forward_command_controller/include/forward_command_controller/forward_controller.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) -// -// 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 FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ -#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ - -#include "controller_interface/controller_interface.hpp" -#include "forward_command_controller/forward_controllers_base.hpp" -#include "forward_command_controller/visibility_control.h" - -namespace forward_command_controller -{ -using CmdType = std_msgs::msg::Float64MultiArray; - -/** - * \brief Forward command controller for a set of joints and interfaces. - */ -class ForwardController : public ForwardControllersBase, - public controller_interface::ControllerInterface -{ -public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ForwardController(); - - FORWARD_COMMAND_CONTROLLER_PUBLIC - ~ForwardController() = default; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; -}; - -} // namespace forward_command_controller - -#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index c04ed7b985..3e153d7e2e 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -32,14 +32,14 @@ namespace forward_command_controller using CmdType = std_msgs::msg::Float64MultiArray; /** - * \brief Forward command controller base class for shared implementation. + * \brief Forward command controller for a set of joints and interfaces. * * This class forwards the command signal down to a set of joints or interfaces. * * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class ForwardControllersBase +class ForwardControllersBase : public controller_interface::ControllerInterface { public: FORWARD_COMMAND_CONTROLLER_PUBLIC @@ -49,27 +49,29 @@ class ForwardControllersBase ~ForwardControllersBase() = default; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration get_command_interface_configuration() const; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration get_state_interface_configuration() const; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn execute_init( - const std::shared_ptr & node); + controller_interface::CallbackReturn on_init() override; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn execute_configure( - const rclcpp_lifecycle::State & previous_state, - std::vector & command_interfaces); + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn execute_activate( - const rclcpp_lifecycle::State & previous_state, - std::vector & command_interfaces); + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - controller_interface::CallbackReturn execute_deactivate( - const rclcpp_lifecycle::State & previous_state); + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: /** @@ -79,7 +81,7 @@ class ForwardControllersBase virtual void declare_parameters() = 0; /** - * Derived controllers have to read parameters in this method and set `command_interface_names_` + * Derived controllers have to read parameters in this method and set `command_interface_types_` * variable. The variable is then used to propagate the command interface configuration to * controller manager. The method is called from `on_configure`-method of this class. * @@ -93,13 +95,10 @@ class ForwardControllersBase std::vector joint_names_; std::string interface_name_; - std::vector command_interface_names_; + std::vector command_interface_types_; realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; - -private: - std::shared_ptr node_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index ddf26fe1c3..8acec36c5c 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -18,11 +18,8 @@ #include #include -#include "forward_command_controller/chainable_forward_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace forward_command_controller { @@ -37,79 +34,20 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ - template < - typename T, - typename std::enable_if< - std::is_convertible::value, - T>::type * = nullptr, - typename std::enable_if< - std::is_convertible::value, T>::type * = - nullptr> - class BaseMultiInterfaceForwardCommandController : public T - { - public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - BaseMultiInterfaceForwardCommandController() : T() {} - - protected: - void declare_parameters() override - { - controller_interface::ControllerInterfaceBase::auto_declare("joint", joint_name_); - controller_interface::ControllerInterfaceBase::auto_declare>( - "interface_names", interface_names_); - }; - - controller_interface::CallbackReturn read_parameters() override - { - joint_name_ = T::get_node()->get_parameter("joint").as_string(); - interface_names_ = T::get_node()->get_parameter("interface_names").as_string_array(); - - if (joint_name_.empty()) - { - RCLCPP_ERROR(T::get_node()->get_logger(), "'joint' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - if (interface_names_.empty()) - { - RCLCPP_ERROR(T::get_node()->get_logger(), "'interfaces' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & interface : interface_names_) - - { - T::command_interface_names_.push_back(joint_name_ + "/" + interface); - } - - return controller_interface::CallbackReturn::SUCCESS; - }; - - std::string joint_name_; - std::vector interface_names_; - }; +class MultiInterfaceForwardCommandController +: public forward_command_controller::ForwardControllersBase +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + MultiInterfaceForwardCommandController(); - class MultiInterfaceForwardCommandController - : public BaseMultiInterfaceForwardCommandController - { - public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - MultiInterfaceForwardCommandController() - : BaseMultiInterfaceForwardCommandController() - { - } - }; +protected: + void declare_parameters() override; + controller_interface::CallbackReturn read_parameters() override; - class ChainableMultiInterfaceForwardCommandController - : public BaseMultiInterfaceForwardCommandController - { - public: - FORWARD_COMMAND_CONTROLLER_PUBLIC - ChainableMultiInterfaceForwardCommandController() - : BaseMultiInterfaceForwardCommandController() - { - } - }; + std::string joint_name_; + std::vector interface_names_; +}; } // namespace forward_command_controller diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp deleted file mode 100644 index 25a247dd2e..0000000000 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) -// -// 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 "forward_command_controller/chainable_forward_controller.hpp" - -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" - -namespace forward_command_controller -{ -ChainableForwardController::ChainableForwardController() -: ForwardControllersBase(), controller_interface::ChainableControllerInterface() -{ -} - -controller_interface::CallbackReturn ChainableForwardController::on_init() -{ - return execute_init(get_node()); -} - -controller_interface::CallbackReturn ChainableForwardController::on_configure( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = execute_configure(previous_state, command_interfaces_); - if (ret != CallbackReturn::SUCCESS) - { - return ret; - } - - // The names should be in the same order as for command interfaces for easier matching - reference_interface_names_ = command_interface_names_; - // for any case make reference interfaces size of command interfaces - reference_interfaces_.resize( - reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); - - return CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration -ChainableForwardController::command_interface_configuration() const -{ - return get_command_interface_configuration(); -} - -controller_interface::InterfaceConfiguration -ChainableForwardController::state_interface_configuration() const -{ - return get_state_interface_configuration(); -} - -std::vector -ChainableForwardController::on_export_reference_interfaces() -{ - std::vector reference_interfaces; - - for (size_t i = 0; i < reference_interface_names_.size(); ++i) - { - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); - } - - return reference_interfaces; -} - -bool ChainableForwardController::on_set_chained_mode(bool chained_mode) -{ - // we can set chained mode in any situation - (void)chained_mode; - return true; -} - -controller_interface::CallbackReturn ChainableForwardController::on_activate( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = execute_activate(previous_state, command_interfaces_); - if (ret != CallbackReturn::SUCCESS) - { - return ret; - } - - std::fill( - reference_interfaces_.begin(), reference_interfaces_.end(), - std::numeric_limits::quiet_NaN()); - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ChainableForwardController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - return execute_deactivate(previous_state); -} - -controller_interface::return_type ChainableForwardController::update_reference_from_subscribers() -{ - auto joint_commands = rt_command_ptr_.readFromRT(); - // message is valid - if (!(!joint_commands || !(*joint_commands))) - { - reference_interfaces_ = (*joint_commands)->data; - } - - return controller_interface::return_type::OK; -} - -controller_interface::return_type ChainableForwardController::update_and_write_commands( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - if (!std::isnan(reference_interfaces_[i])) - { - command_interfaces_[i].set_value(reference_interfaces_[i]); - } - } - - return controller_interface::return_type::OK; -} - -} // namespace forward_command_controller diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index b003782416..06c6b15120 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,13 +14,58 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include "forward_command_controller/chainable_forward_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" +#include +#include +#include +#include +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace forward_command_controller +{ +ForwardCommandController::ForwardCommandController() : ForwardControllersBase() {} + +void ForwardCommandController::declare_parameters() +{ + auto_declare("joints", std::vector()); + auto_declare("interface_name", std::string()); +} + +controller_interface::CallbackReturn ForwardCommandController::read_parameters() +{ + joint_names_ = get_node()->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + // Specialized, child controllers set interfaces before calling configure function. + if (interface_name_.empty()) + { + interface_name_ = get_node()->get_parameter("interface_name").as_string(); + } + + if (interface_name_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : joint_names_) + { + command_interface_types_.push_back(joint + "/" + interface_name_); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace forward_command_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) -PLUGINLIB_EXPORT_CLASS( - forward_command_controller::ChainableForwardCommandController, - controller_interface::ChainableControllerInterface) + forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp deleted file mode 100644 index 790738a9be..0000000000 --- a/forward_command_controller/src/forward_controller.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt) -// -// 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 "forward_command_controller/forward_controller.hpp" - -#include "forward_command_controller/visibility_control.h" - -namespace forward_command_controller -{ -ForwardController::ForwardController() -: ForwardControllersBase(), controller_interface::ControllerInterface() -{ -} - -controller_interface::InterfaceConfiguration ForwardController::command_interface_configuration() - const -{ - return get_command_interface_configuration(); -} - -controller_interface::InterfaceConfiguration ForwardController::state_interface_configuration() - const -{ - return get_state_interface_configuration(); -} - -controller_interface::CallbackReturn ForwardController::on_init() -{ - return execute_init(get_node()); -} - -controller_interface::CallbackReturn ForwardController::on_configure( - const rclcpp_lifecycle::State & previous_state) -{ - return execute_configure(previous_state, command_interfaces_); -} - -controller_interface::CallbackReturn ForwardController::on_activate( - const rclcpp_lifecycle::State & previous_state) -{ - return execute_activate(previous_state, command_interfaces_); -} - -controller_interface::CallbackReturn ForwardController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - return execute_deactivate(previous_state); -} - -controller_interface::return_type ForwardController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - auto joint_commands = rt_command_ptr_.readFromRT(); - - // no command received yet - if (!joint_commands || !(*joint_commands)) - { - return controller_interface::return_type::OK; - } - - for (auto index = 0ul; index < command_interfaces_.size(); ++index) - { - command_interfaces_[index].set_value((*joint_commands)->data[index]); - } - - return controller_interface::return_type::OK; -} - -} // namespace forward_command_controller diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 3aa76d4423..e4ea46fcc5 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -28,15 +28,14 @@ namespace forward_command_controller { ForwardControllersBase::ForwardControllersBase() -: rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) +: controller_interface::ControllerInterface(), + rt_command_ptr_(nullptr), + joints_command_subscriber_(nullptr) { } -controller_interface::CallbackReturn ForwardControllersBase::execute_init( - const std::shared_ptr & node) +controller_interface::CallbackReturn ForwardControllersBase::on_init() { - node_ = node; - try { declare_parameters(); @@ -50,53 +49,42 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_init( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardControllersBase::execute_configure( - const rclcpp_lifecycle::State & /*previous_state*/, - std::vector & command_interfaces) +controller_interface::CallbackReturn ForwardControllersBase::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { auto ret = this->read_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(node_->get_logger(), "Error when reading parameters."); return ret; } - joints_command_subscriber_ = node_->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { - // check if message is correct size, if not ignore - if (msg->data.size() == command_interface_names_.size()) - { - rt_command_ptr_.writeFromNonRT(msg); - } - }); - - // pre-reserve command interfaces - command_interfaces.reserve(command_interface_names_.size()); + joints_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - RCLCPP_INFO(node_->get_logger(), "configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration -ForwardControllersBase::get_command_interface_configuration() const +ForwardControllersBase::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names = command_interface_names_; + command_interfaces_config.names = command_interface_types_; return command_interfaces_config; } -controller_interface::InterfaceConfiguration -ForwardControllersBase::get_state_interface_configuration() const +controller_interface::InterfaceConfiguration ForwardControllersBase::state_interface_configuration() + const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn ForwardControllersBase::execute_activate( - const rclcpp_lifecycle::State & /*previous_state*/, - std::vector & command_interfaces) +controller_interface::CallbackReturn ForwardControllersBase::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { // check if we have all resources defined in the "points" parameter // also verify that we *only* have the resources defined in the "points" parameter @@ -105,23 +93,23 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_activate( ordered_interfaces; if ( !controller_interface::get_ordered_interfaces( - command_interfaces, command_interface_names_, std::string(""), ordered_interfaces) || - command_interface_names_.size() != ordered_interfaces.size()) + command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || + command_interface_types_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu command interfaces, got %zu", - command_interface_names_.size(), ordered_interfaces.size()); + get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + command_interface_types_.size(), ordered_interfaces.size()); return controller_interface::CallbackReturn::ERROR; } // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); - RCLCPP_INFO(node_->get_logger(), "activate successful"); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardControllersBase::execute_deactivate( +controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer @@ -129,4 +117,32 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_deactivate( return controller_interface::CallbackReturn::SUCCESS; } +controller_interface::return_type ForwardControllersBase::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!joint_commands || !(*joint_commands)) + { + return controller_interface::return_type::OK; + } + + if ((*joint_commands)->data.size() != command_interfaces_.size()) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + (*joint_commands)->data.size(), command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + + for (auto index = 0ul; index < command_interfaces_.size(); ++index) + { + command_interfaces_[index].set_value((*joint_commands)->data[index]); + } + + return controller_interface::return_type::OK; +} + } // namespace forward_command_controller diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 328907ec78..8faacd0776 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -14,14 +14,51 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" -#include "forward_command_controller/chainable_forward_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" +#include +#include + +namespace forward_command_controller +{ +MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() +: ForwardControllersBase() +{ +} + +void MultiInterfaceForwardCommandController::declare_parameters() +{ + auto_declare("joint", joint_name_); + auto_declare("interface_names", interface_names_); +} + +controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() +{ + joint_name_ = get_node()->get_parameter("joint").as_string(); + interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); + + if (joint_name_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (interface_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & interface : interface_names_) + { + command_interface_types_.push_back(joint_name_ + "/" + interface); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace forward_command_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - forward_command_controller::MultiInterfaceForwardCommandController, - controller_interface::ControllerInterface) -PLUGINLIB_EXPORT_CLASS( - forward_command_controller::ChainableMultiInterfaceForwardCommandController, - controller_interface::ChainableControllerInterface) + forward_command_controller::MultiInterfaceForwardCommandController, + controller_interface::ControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 0240dbb4e0..697e42d671 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -23,7 +23,6 @@ #include "test_forward_command_controller.hpp" #include "forward_command_controller/forward_command_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index d9acbdf6c4..9c6bd2a352 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -22,7 +22,6 @@ #include "gmock/gmock.h" #include "forward_command_controller/forward_command_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/forward_command_controller/test/test_load_chained_forward_command_controller.cpp b/forward_command_controller/test/test_load_chained_forward_command_controller.cpp deleted file mode 100644 index 9c93fc8d7e..0000000000 --- a/forward_command_controller/test/test_load_chained_forward_command_controller.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// 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 "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 "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadForwardCommandController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NO_THROW(cm.load_controller( - "test_forward_command_controller", - "forward_command_controller/ChainedForwardCommandController")); - - rclcpp::shutdown(); -} diff --git a/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp deleted file mode 100644 index f73ba042de..0000000000 --- a/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) 2021, PickNik, Inc. -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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 "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 "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadMultiInterfaceForwardController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NO_THROW(cm.load_controller( - "test_forward_command_controller", - "forward_command_controller/ChainedMultiInterfaceForwardCommandController")); -} diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index 03d735fb1c..464b57b69d 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -34,9 +34,6 @@ TEST(TestLoadForwardCommandController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - cm.load_controller( - "test_forward_command_controller", "forward_command_controller/ForwardCommandController"); - ASSERT_NO_THROW(cm.load_controller( "test_forward_command_controller", "forward_command_controller/ForwardCommandController")); diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index bb4ded549e..0cada04859 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -24,7 +24,6 @@ #include "test_multi_interface_forward_command_controller.hpp" -#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 8846b2bcdc..62a4d4e981 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -23,7 +23,6 @@ #include "gmock/gmock.h" -#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" From c7c731210faa7da6e7756214c2d5355e4bd0915c Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 5 Aug 2022 14:38:41 -0600 Subject: [PATCH 049/111] update force reading in hardware read function --- .../admittance_controller.hpp | 4 +++- .../config/admittance_controller_parameters.hpp | 2 +- .../src/admittance_controller.cpp | 16 ++++++++++------ .../src/admittance_controller_parameters.yaml | 2 +- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 2 +- .../config/tricycle_drive_controller.yaml | 4 ++-- tricycle_controller/doc/userdoc.rst | 6 +++--- .../tricycle_controller/tricycle_controller.hpp | 2 +- tricycle_controller/package.xml | 2 +- tricycle_controller/src/tricycle_controller.cpp | 6 +++--- .../test/config/test_tricycle_controller.yaml | 2 +- 12 files changed, 28 insertions(+), 22 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 2ea88586b2..ce99d8ba7f 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -138,11 +138,13 @@ namespace admittance_controller { // control loop data trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, state_error_; + geometry_msgs::msg::Wrench ft_values_; trajectory_msgs::msg::JointTrajectory pre_admittance_point; size_t loop_counter = 0; // helper methods - void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state); + void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state_current, + geometry_msgs::msg::Wrench &ft_values); void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_commanded); diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp index 60f5a5dce6..b801463bac 100644 --- a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp +++ b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp @@ -697,4 +697,4 @@ namespace admittance_controller { std::shared_ptr parameters_interface_; }; -} // namespace admittance_controller \ No newline at end of file +} // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5a95b98962..06523958ba 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -247,7 +247,7 @@ namespace admittance_controller { force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); // handle state after restart or initial startups - read_state_from_hardware(state_current_); + read_state_from_hardware(state_current_, ft_values_); // initialize states from last read state reference last_commanded_state_ = state_current_; @@ -295,12 +295,10 @@ namespace admittance_controller { read_state_reference_interfaces(state_reference_); // get all controller inputs - geometry_msgs::msg::Wrench ft_values; - force_torque_sensor_->get_values_as_message(ft_values); - read_state_from_hardware(state_current_); + read_state_from_hardware(state_current_, ft_values_); // apply admittance control to reference to determine desired state - admittance_->update(state_current_, ft_values, state_reference_, period, state_desired_); + admittance_->update(state_current_, ft_values_, state_reference_, period, state_desired_); // write calculated values to joint interfaces write_state_to_hardware(state_desired_); @@ -336,7 +334,8 @@ namespace admittance_controller { } void AdmittanceController::read_state_from_hardware( - trajectory_msgs::msg::JointTrajectoryPoint &state_current) { + trajectory_msgs::msg::JointTrajectoryPoint &state_current, + geometry_msgs::msg::Wrench &ft_values) { // Fill fields of state_reference argument from hardware state interfaces. If the hardware does not exist or // the values are nan, the corresponding state field will be set to empty. @@ -365,6 +364,11 @@ namespace admittance_controller { } } } + force_torque_sensor_->get_values_as_message(ft_values); + if (std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || std::isnan(ft_values.force.z) + || std::isnan(ft_values.torque.x) || std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)){ + ft_values = geometry_msgs::msg::Wrench(); + } } diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index b43b2db24d..31a74b8565 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -166,4 +166,4 @@ admittance_controller: type: bool, default_value: true, description: "if enabled, parameters will be dynamically updated in the control loop" - } \ No newline at end of file + } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 59bcdac3c0..f7cd5b388b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -271,4 +271,4 @@ namespace joint_trajectory_controller } // namespace joint_trajectory_controller -#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ \ No newline at end of file +#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 753d836ef1..ea3d6b4668 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1380,4 +1380,4 @@ namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) \ No newline at end of file + joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index 4eabf615ef..c1077b13c3 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -1,4 +1,4 @@ -tricycle_controller: +tricycle_controller: ros__parameters: # Model traction_joint_name: traction_joint # Name of traction joint in URDF @@ -16,7 +16,7 @@ tricycle_controller: pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom - + # Rate Limiting traction: # All values should be positive # min_velocity: 0.0 diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 2271b8c9cf..a248bbec96 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,8 +10,8 @@ commands for the tricycle drive base. Odometry is computed from hardware feedbac Velocity commands ----------------- -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. @@ -21,4 +21,4 @@ Other features Realtime-safe implementation. Odometry publishing Velocity, acceleration and jerk limits - Automatic stop after command timeout \ No newline at end of file + Automatic stop after command timeout diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index c5ba1c62e3..959dcc9e69 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -125,7 +125,7 @@ class TricycleController : public controller_interface::ControllerInterface { bool open_loop = false; bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in seperate node + bool odom_only_twist = false; // for doing the pose integration in separate node std::string base_frame_id = "base_link"; std::string odom_frame_id = "odom"; std::array pose_covariance_diagonal; diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9dec765d60..df07cd4afe 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -32,4 +32,4 @@ ament_cmake - \ No newline at end of file + diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 7cec107a91..bfae724aef 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -151,7 +151,7 @@ controller_interface::return_type TricycleController::update( double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - + if (odom_params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); @@ -160,7 +160,7 @@ controller_interface::return_type TricycleController::update( { if (std::isnan(Ws_read) || std::isnan(alpha_read)) { - RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); return controller_interface::return_type::ERROR; } odometry_.update(Ws_read, alpha_read, period); @@ -208,7 +208,7 @@ controller_interface::return_type TricycleController::update( // Compute wheel velocity and angle auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); - // Reduce wheel speed until the target angle has been reached + // Reduce wheel speed until the target angle has been reached double alpha_delta = abs(alpha_write - alpha_read); double scale; if (alpha_delta < M_PI / 6) diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index d81d722219..efecfe968f 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -17,4 +17,4 @@ test_tricycle_controller: max_deceleration: 8.0 steering: max_position: 1.57 # pi/2 - max_velocity: 1.0 \ No newline at end of file + max_velocity: 1.0 From 45b8afdc468991453ee53500ebcb036f8db0d8cd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sat, 6 Aug 2022 13:57:41 -0600 Subject: [PATCH 050/111] initialize reference vectors properly --- .../include/admittance_controller/admittance_rule_impl.hpp | 7 +++++-- admittance_controller/src/admittance_controller.cpp | 4 ++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 5efa73345d..a6756bf58b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -178,8 +178,7 @@ namespace admittance_controller { const rclcpp::Duration &period, trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { - double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; -// double dt = 1.0/(1000.0); + double dt = period.seconds(); if (parameters_.enable_parameter_update_without_reactivation) { apply_parameters_update(); @@ -279,6 +278,10 @@ namespace admittance_controller { bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, tmp, ft_sensor_frame, admittance_state.joint_acc_); +// +// std::cout << "joint_acc_" << admittance_state.joint_acc_ << std::endl; +// std::cout << "admittance_velocity_" << admittance_state.admittance_velocity_ << std::endl; +// std::cout << "dt" << std::to_string(dt) << std::endl; // integrate motion in joint space admittance_state.joint_vel_ += admittance_state.joint_acc_ * dt; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 06523958ba..f054e0e497 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -112,8 +112,8 @@ namespace admittance_controller { //allocate dynamic memory chainable_command_interfaces.reserve(num_chainable_interfaces); reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); - position_reference_.reserve(num_joints_); - velocity_reference_.reserve(num_joints_); + position_reference_ = {}; + velocity_reference_ = {}; // assign reference interfaces auto index = 0ul; From 893cea631cb5b1f1d7ec4ea67cfd8d9f18c85538 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sat, 6 Aug 2022 14:36:41 -0600 Subject: [PATCH 051/111] resize states in activate --- .../src/joint_trajectory_controller.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ea3d6b4668..438ebe46cd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -817,10 +817,7 @@ namespace joint_trajectory_controller std::bind(&JointTrajectoryController::cancel_callback, this, _1), std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); - resize_joint_trajectory_point(state_current_, dof_); - resize_joint_trajectory_point(state_desired_, dof_); - resize_joint_trajectory_point(state_error_, dof_); - resize_joint_trajectory_point(last_commanded_state_, dof_); + RCLCPP_ERROR(get_node()->get_logger(), "configure called!!!"); return CallbackReturn::SUCCESS; } @@ -882,6 +879,12 @@ namespace joint_trajectory_controller last_state_publish_time_ = get_node()->now(); // Initialize current state storage if hardware state has tracking offset + resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point(state_desired_, dof_); + resize_joint_trajectory_point(state_error_, dof_); + resize_joint_trajectory_point(last_commanded_state_, dof_); + + RCLCPP_ERROR(get_node()->get_logger(), "activate called!!!"); read_state_from_hardware(state_current_); read_state_from_hardware(state_desired_); read_state_from_hardware(last_commanded_state_); From 911ecdd7805403dd7dfa7c98f1c11fa7dc67e7a9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 8 Aug 2022 16:29:34 -0600 Subject: [PATCH 052/111] use reference wrapper instead of double pointer --- .../admittance_controller.hpp | 4 ++-- .../src/admittance_controller.cpp | 20 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 6068be77da..b24f9117d8 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -113,8 +113,8 @@ namespace admittance_controller { // internal reference values const std::vector reference_interfaces_types_ = {"position", "velocity"}; - std::vector position_reference_; - std::vector velocity_reference_; + std::vector> position_reference_; + std::vector> velocity_reference_; // Admittance rule and dependent variables; std::unique_ptr admittance_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f054e0e497..6d9d136a67 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -120,9 +120,9 @@ namespace admittance_controller { for (const auto &interface: reference_interfaces_types_) { for (const auto &joint: admittance_->parameters_.joints) { if (hardware_interface::HW_IF_POSITION == interface) - position_reference_.emplace_back(reference_interfaces_.data() + index); + position_reference_.emplace_back(reference_interfaces_[index]); else if (hardware_interface::HW_IF_VELOCITY == interface) { - velocity_reference_.emplace_back(reference_interfaces_.data() + index); + velocity_reference_.emplace_back(reference_interfaces_[index]); } chainable_command_interfaces.emplace_back( hardware_interface::CommandInterface(std::string(get_node()->get_name()), @@ -273,10 +273,10 @@ namespace admittance_controller { // if message exists, load values into references if (joint_command_msg.get()) { for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { - *position_reference_[i] = joint_command_msg->positions[i]; + position_reference_[i].get() = joint_command_msg->positions[i]; } for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { - *velocity_reference_[i] = joint_command_msg->velocities[i]; + velocity_reference_[i].get() = joint_command_msg->velocities[i]; } } @@ -400,18 +400,18 @@ namespace admittance_controller { // the values are nan, the corresponding field will be set to empty for (auto i = 0ul; i < position_reference_.size(); i++) { - if (std::isnan(*position_reference_[i])) { - *position_reference_[i] = last_state_reference_.positions[i]; + if (std::isnan(position_reference_[i])) { + position_reference_[i].get() = last_state_reference_.positions[i]; } - state_reference.positions[i] = *position_reference_[i]; + state_reference.positions[i] = position_reference_[i]; } last_state_reference_.positions = state_reference.positions; for (auto i = 0ul; i < velocity_reference_.size(); i++) { - if (std::isnan(*velocity_reference_[i])) { - *velocity_reference_[i] = last_state_reference_.velocities[i]; + if (std::isnan(velocity_reference_[i])) { + velocity_reference_[i].get() = last_state_reference_.velocities[i]; } - state_reference.velocities[i] = *velocity_reference_[i]; + state_reference.velocities[i] = velocity_reference_[i]; } last_state_reference_.velocities = state_reference.velocities; From d8999a5486e3443862437ba1323b0647ef7379eb Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 8 Aug 2022 16:44:30 -0600 Subject: [PATCH 053/111] remove rotation struct --- .../admittance_controller/admittance_rule.hpp | 29 ------------------- .../admittance_rule_impl.hpp | 28 +++++++----------- .../src/admittance_controller.cpp | 1 + 3 files changed, 12 insertions(+), 46 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7287db9bb8..9cc64840c1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -62,32 +62,6 @@ namespace admittance_controller { Eigen::Matrix base_cog_; }; - struct Rotations { - Rotations() { - base_control_.setIdentity(); - base_ft_.setIdentity(); - base_desired_ft_.setIdentity(); - base_tip_.setIdentity(); - world_base_.setIdentity(); - base_world_.setIdentity(); - base_sensor_.setIdentity(); - base_cog_.setIdentity(); - world_sensor_.setIdentity(); - world_cog_.setIdentity(); - } - - Eigen::Matrix base_control_; - Eigen::Matrix base_ft_; - Eigen::Matrix base_desired_ft_; - Eigen::Matrix base_tip_; - Eigen::Matrix world_base_; - Eigen::Matrix base_world_; - Eigen::Matrix base_sensor_; - Eigen::Matrix base_cog_; - Eigen::Matrix world_sensor_; - Eigen::Matrix world_cog_; - }; - struct AdmittanceState { AdmittanceState() = default; @@ -286,9 +260,6 @@ namespace admittance_controller { Transforms trans_; Transforms trans_ref_; - // rotations needed for admittance update - Rotations rotations_; - // position of center of gravity in cog_frame Eigen::Vector3d cog_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 5efa73345d..2914ea1169 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -85,7 +85,6 @@ namespace admittance_controller { // reset transforms and rotations trans_ = Transforms(); trans_ref_ = Transforms(); - rotations_ = Rotations(); // reset forces wrench_world_.setZero(); @@ -155,18 +154,6 @@ namespace admittance_controller { success &= get_transform(joint_buffer_vec_, parameters_.ft_sensor.frame.id, parameters_.ft_sensor.frame.external, trans_ref_.base_desired_ft_); - - rotations_.base_control_ = trans_ref_.base_control_.block<3, 3>(0, 0); - rotations_.base_ft_ = trans_.base_ft_.block<3, 3>(0, 0); - rotations_.base_desired_ft_ = trans_.base_desired_ft_.block<3, 3>(0, 0); - rotations_.base_tip_ = trans_.base_tip_.block<3, 3>(0, 0); - rotations_.world_base_ = trans_.world_base_.block<3, 3>(0, 0); - rotations_.base_world_ = rotations_.world_base_.transpose(); - rotations_.base_sensor_ = trans_.base_sensor_.block<3, 3>(0, 0); - rotations_.base_cog_ = trans_.base_cog_.block<3, 3>(0, 0); - rotations_.world_sensor_ = rotations_.world_base_ * rotations_.base_sensor_; - rotations_.world_cog_ = rotations_.world_base_ * rotations_.base_cog_; - return success; } @@ -179,7 +166,6 @@ namespace admittance_controller { trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { double dt = period.seconds() + ((double) period.nanoseconds()) * 1E-9; -// double dt = 1.0/(1000.0); if (parameters_.enable_parameter_update_without_reactivation) { apply_parameters_update(); @@ -187,15 +173,23 @@ namespace admittance_controller { bool success = get_all_transforms(current_joint_state, reference_joint_state, admittance_state_); + // calculate needed rotations + Eigen::Matrix rot_base_sensor = trans_.base_sensor_.block<3,3>(0,0); + Eigen::Matrix rot_world_base = trans_.world_base_.block<3,3>(0,0); + Eigen::Matrix rot_base_cog = trans_.base_cog_.block<3,3>(0,0); + Eigen::Matrix rot_base_control = trans_.base_control_.block<3,3>(0,0); + // apply filter and update wrench_world_ vector - process_wrench_measurements(measured_wrench, rotations_.world_sensor_, rotations_.world_cog_); + Eigen::Matrix rot_world_sensor = rot_world_base*rot_base_sensor; + Eigen::Matrix rot_world_cog = rot_world_base*rot_base_cog; + process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); // transform wrench_world_ into base frame // transform wrench_world_ into base frame - admittance_state_.wrench_base = rotations_.base_world_ * wrench_world_; + admittance_state_.wrench_base = rot_world_base.transpose() * wrench_world_; // Compute admittance control law - success &= calculate_admittance_rule(current_joint_state.positions, rotations_.base_control_, trans_.base_ft_, + success &= calculate_admittance_rule(current_joint_state.positions, rot_base_control, trans_.base_ft_, trans_ref_.base_ft_, trans_ref_.base_desired_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6d9d136a67..0dbacbde80 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -254,6 +254,7 @@ namespace admittance_controller { last_state_reference_ = state_current_; read_state_reference_interfaces(state_reference_); + // reset dynamic fields in case non-zero state_reference_.velocities.assign(num_joints_, 0.0); state_reference_.accelerations.assign(num_joints_, 0.0); From 4833f4a3a60d098952be6805fc4f4eb0e48260c2 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 8 Aug 2022 17:11:55 -0600 Subject: [PATCH 054/111] update test --- .../admittance_controller.hpp | 3 -- .../test/test_admittance_controller.cpp | 41 ------------------- 2 files changed, 44 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index b24f9117d8..3ae10cbdf0 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -52,9 +52,6 @@ namespace admittance_controller { class AdmittanceController : public controller_interface::ChainableControllerInterface { public: ADMITTANCE_CONTROLLER_PUBLIC - AdmittanceController() { - int o =0; - }; ADMITTANCE_CONTROLLER_PUBLIC CallbackReturn on_init() override; diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index ed91813403..b74a4d46ee 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -22,18 +22,6 @@ #include -//// Test on_configure returns ERROR when a parameter is invalid -//TEST_P(AdmittanceControllerTestParameterizedInvalidParameters, one_parameter_is_invalid) -//{ -// SetUpController(); -// auto name = std::get<0>(GetParam()); -// auto val = std::get<1>(GetParam()); -// rclcpp::Parameter parameter(name, val); -// controller_->get_node()->set_parameter(parameter); -// -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -//} - // Test on_configure returns ERROR when a required parameter is missing TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) { @@ -45,26 +33,18 @@ INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringConfiguration, AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( -// "admittance.damping_ratio", optional "admittance.mass", "admittance.selected_axes", "admittance.stiffness", "chainable_command_interfaces", "command_interfaces", -// "control.frame.external", optional "control.frame.id", -// "fixed_world_frame.frame.external", optional "fixed_world_frame.frame.id", -// "ft_sensor.filter_coefficient", optional -// "ft_sensor.frame.external", optional "ft_sensor.frame.id", "ft_sensor.name", -// "gravity_compensation.CoG.force", optional "gravity_compensation.CoG.pos", -// "gravity_compensation.frame.external", optional "gravity_compensation.frame.id", "joints", -// "kinematics.alpha", optional "kinematics.base", "kinematics.plugin_name", "kinematics.plugin_package", @@ -204,27 +184,6 @@ TEST_F(AdmittanceControllerTest, activate_success) ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - // Check command interfaces size - std::unordered_map < std::string, - std::vector < std::reference_wrapper < hardware_interface::LoanedCommandInterface >> * > - command_interface_map = { - {hardware_interface::HW_IF_POSITION, &controller_->joint_position_command_interface_}, - {hardware_interface::HW_IF_VELOCITY, &controller_->joint_velocity_command_interface_} - }; - for (const auto & interface : command_interface_types_) { - ASSERT_TRUE(command_interface_map[interface]->size() == joint_names_.size()); - } - // Check state interfaces size - // assign state interfaces - std::unordered_map < std::string, - std::vector < std::reference_wrapper < hardware_interface::LoanedStateInterface >> * > state_interface_map = { - {hardware_interface::HW_IF_POSITION, &controller_->joint_position_state_interface_}, - {hardware_interface::HW_IF_VELOCITY, &controller_->joint_velocity_state_interface_} - }; - for (const auto & interface : command_interface_types_) { - ASSERT_TRUE(state_interface_map[interface]->size() == joint_names_.size()); - } - } TEST_F(AdmittanceControllerTest, update_success) From 7f22790c2ac6103dba351cfed232fdbf81a25a28 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 8 Aug 2022 17:28:05 -0600 Subject: [PATCH 055/111] remove generated code --- admittance_controller/CMakeLists.txt | 11 +- .../admittance_controller/admittance_rule.hpp | 2 +- .../admittance_controller_parameters.hpp | 700 ------------------ admittance_controller/package.xml | 3 +- 4 files changed, 8 insertions(+), 708 deletions(-) delete mode 100644 admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index e86d15f3f8..760c5f3da3 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(trajectory_msgs REQUIRED) find_package(angles REQUIRED) find_package(rcutils REQUIRED) find_package(tf2_kdl REQUIRED) +find_package(generate_parameter_library REQUIRED) # The admittance controller @@ -45,11 +46,11 @@ target_include_directories( include ) -# uncomment to generate parameter listener code -#find_package(generate_parameter_library REQUIRED) -#generate_parameter_library(admittance_controller_parameters -# src/admittance_controller_parameters.yaml -# ) +generate_parameter_library(admittance_controller_parameters + src/admittance_controller_parameters.yaml + ) + +target_link_libraries(admittance_controller admittance_controller_parameters) ament_target_dependencies( diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 9cc64840c1..54ee6e081e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -28,7 +28,7 @@ #include "controller_interface/controller_interface.hpp" #include "control_toolbox/filters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "config/admittance_controller_parameters.hpp" +#include "admittance_controller_parameters.hpp" // kinematics plugins #include "kinematics_interface/kinematics_interface_base.hpp" diff --git a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp b/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp deleted file mode 100644 index b801463bac..0000000000 --- a/admittance_controller/include/admittance_controller/config/admittance_controller_parameters.hpp +++ /dev/null @@ -1,700 +0,0 @@ -// auto-generated DO NOT EDIT - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace gen_param_struct_validators { - class Result { - public: - template - Result(const std::string& format, Args... args) { - msg_ = fmt::format(format, args...); - success_ = false; - } - - Result() = default; - - operator rcl_interfaces::msg::SetParametersResult() const { - rcl_interfaces::msg::SetParametersResult result; - result.successful = success_; - result.reason = msg_; - return result; - } - - bool success() { return success_; } - - std::string error_msg() { return msg_; } - - private: - std::string msg_; - bool success_ = true; - }; - - auto OK = Result(); - using ERROR = Result; - - template - bool contains(std::vector const& vec, T const& val) { - return std::find(vec.cbegin(), vec.cend(), val) != vec.cend(); - } - - template - bool is_unique(std::vector const& x) { - auto vec = x; - std::sort(vec.begin(), vec.end()); - return std::adjacent_find(vec.cbegin(), vec.cend()) == vec.cend(); - } - - template - Result unique(rclcpp::Parameter const& parameter) { - if (!is_unique(parameter.get_value>())) { - return ERROR("Parameter '{}' must only contain unique values", - parameter.get_name()); - } - return OK; - } - - template - Result subset_of(rclcpp::Parameter const& parameter, - std::vector valid_values) { - auto const& input_values = parameter.get_value>(); - - for (auto const& value : input_values) { - if (!contains(valid_values, value)) { - return ERROR("Invalid entry '{}' for parameter '{}'. Not in set: {}", - value, parameter.get_name(), valid_values); - } - } - - return OK; - } - - template - Result fixed_size(const rclcpp::Parameter& parameter, size_t size) { - auto param_value = parameter.get_value>(); - if (param_value.size() != size) { - return ERROR("Invalid length '{}' for parameter '{}'. Required length: {}", - param_value.size(), parameter.get_name().c_str(), size); - } - return OK; - } - - template - Result size_gt(rclcpp::Parameter const& parameter, size_t size) { - auto const& values = parameter.get_value>(); - if (values.size() > size) { - return OK; - } - - return ERROR( - "Invalid length '{}' for parameter '{}'. Required greater than: {}", - values.size(), parameter.get_name(), size); - } - - template - Result size_lt(rclcpp::Parameter const& parameter, size_t size) { - auto const& values = parameter.get_value>(); - if (values.size() < size) { - return OK; - } - - return ERROR("Invalid length '{}' for parameter '{}'. Required less than: {}", - values.size(), parameter.get_name(), size); - } - - template - Result element_bounds(const rclcpp::Parameter& parameter, T lower, T upper) { - auto param_value = parameter.get_value>(); - for (auto val : param_value) { - if (val < lower || val > upper) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", - val, parameter.get_name(), lower, upper); - } - } - return OK; - } - - template - Result lower_element_bounds(const rclcpp::Parameter& parameter, T lower) { - auto param_value = parameter.get_value>(); - for (auto val : param_value) { - if (val < lower) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", - val, parameter.get_name(), lower); - } - } - return OK; - } - - template - Result upper_element_bounds(const rclcpp::Parameter& parameter, T upper) { - auto param_value = parameter.get_value>(); - for (auto val : param_value) { - if (val > upper) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", - val, parameter.get_name(), upper); - } - } - return OK; - } - - template - Result bounds(const rclcpp::Parameter& parameter, T lower, T upper) { - auto param_value = parameter.get_value(); - if (param_value < lower || param_value > upper) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required bounds: [{}, {}]", - param_value, parameter.get_name(), lower, upper); - } - return OK; - } - - template - Result lower_bounds(const rclcpp::Parameter& parameter, T lower) { - auto param_value = parameter.get_value(); - if (param_value < lower) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required lower bounds: {}", - param_value, parameter.get_name(), lower); - } - return OK; - } - - template - Result upper_bounds(const rclcpp::Parameter& parameter, T upper) { - auto param_value = parameter.get_value(); - if (param_value > upper) { - return ERROR( - "Invalid value '{}' for parameter '{}'. Required upper bounds: {}", - param_value, parameter.get_name(), upper); - } - return OK; - } - - template - Result one_of(rclcpp::Parameter const& parameter, std::vector collection) { - auto param_value = parameter.get_value(); - - if (std::find(collection.cbegin(), collection.cend(), param_value) == - collection.end()) { - return ERROR("The parameter '{}' with the value '{}' not in the set: {}", - parameter.get_name(), param_value, - fmt::format("{}", fmt::join(collection, ", "))); - } - - return OK; - } - -} // namespace gen_param_struct_validators - -namespace admittance_controller { - struct Params { - std::vector joints; - std::vector command_interfaces; - std::vector state_interfaces; - std::vector chainable_command_interfaces; - std::string robot_description; - bool enable_parameter_update_without_reactivation = true; - struct Kinematics { - std::string plugin_name; - std::string plugin_package; - std::string base; - std::string tip; - double alpha = 0.0005; - } kinematics; - struct FtSensor { - std::string name; - double filter_coefficient = 0.005; - struct Frame { - std::string id; - bool external = false; - } frame; - } ft_sensor; - struct Control { - struct Frame { - std::string id; - bool external = false; - } frame; - } control; - struct FixedWorldFrame { - struct Frame { - std::string id; - bool external = false; - } frame; - } fixed_world_frame; - struct GravityCompensation { - struct Frame { - std::string id; - bool external = false; - } frame; - struct Cog { - std::vector pos; - double force = 0.0; - } CoG; - } gravity_compensation; - struct Admittance { - std::vector selected_axes; - std::vector mass; - std::vector damping_ratio; - std::vector stiffness; - } admittance; - // for detecting if the parameter struct has been updated - rclcpp::Time __stamp; - }; - - class ParamListener{ - public: - // throws rclcpp::exceptions::InvalidParameterValueException on initialization if invalid parameter are loaded - ParamListener(rclcpp::Node::SharedPtr node) - : ParamListener(node->get_node_parameters_interface()) {} - - ParamListener(rclcpp_lifecycle::LifecycleNode::SharedPtr node) - : ParamListener(node->get_node_parameters_interface()) {} - - ParamListener(const std::shared_ptr& parameters_interface){ - parameters_interface_ = parameters_interface; - declare_params(); - auto update_param_cb = [this](const std::vector ¶meters){return this->update(parameters);}; - handle_ = parameters_interface_->add_on_set_parameters_callback(update_param_cb); - clock_ = rclcpp::Clock(); - } - - Params get_params() const{ - return params_; - } - - bool is_old(Params const& other) const { - return params_.__stamp != other.__stamp; - } - - void refresh_dynamic_parameters() { - // TODO remove any destroyed dynamic parameters - - // declare any new dynamic parameters - rclcpp::Parameter param; - - } - - rcl_interfaces::msg::SetParametersResult update(const std::vector ¶meters) { - // Copy params_ so we only update it if all validation succeeds - Params updated_params = params_; - - for (const auto ¶m: parameters) { - if (param.get_name() == "joints") { - updated_params.joints = param.as_string_array(); - } - if (param.get_name() == "command_interfaces") { - updated_params.command_interfaces = param.as_string_array(); - } - if (param.get_name() == "state_interfaces") { - updated_params.state_interfaces = param.as_string_array(); - } - if (param.get_name() == "chainable_command_interfaces") { - updated_params.chainable_command_interfaces = param.as_string_array(); - } - if (param.get_name() == "kinematics.plugin_name") { - updated_params.kinematics.plugin_name = param.as_string(); - } - if (param.get_name() == "kinematics.plugin_package") { - updated_params.kinematics.plugin_package = param.as_string(); - } - if (param.get_name() == "kinematics.base") { - updated_params.kinematics.base = param.as_string(); - } - if (param.get_name() == "kinematics.tip") { - updated_params.kinematics.tip = param.as_string(); - } - if (param.get_name() == "kinematics.alpha") { - updated_params.kinematics.alpha = param.as_double(); - } - if (param.get_name() == "ft_sensor.name") { - updated_params.ft_sensor.name = param.as_string(); - } - if (param.get_name() == "ft_sensor.frame.id") { - updated_params.ft_sensor.frame.id = param.as_string(); - } - if (param.get_name() == "ft_sensor.frame.external") { - updated_params.ft_sensor.frame.external = param.as_bool(); - } - if (param.get_name() == "ft_sensor.filter_coefficient") { - updated_params.ft_sensor.filter_coefficient = param.as_double(); - } - if (param.get_name() == "control.frame.id") { - updated_params.control.frame.id = param.as_string(); - } - if (param.get_name() == "control.frame.external") { - updated_params.control.frame.external = param.as_bool(); - } - if (param.get_name() == "fixed_world_frame.frame.id") { - updated_params.fixed_world_frame.frame.id = param.as_string(); - } - if (param.get_name() == "fixed_world_frame.frame.external") { - updated_params.fixed_world_frame.frame.external = param.as_bool(); - } - if (param.get_name() == "gravity_compensation.frame.id") { - updated_params.gravity_compensation.frame.id = param.as_string(); - } - if (param.get_name() == "gravity_compensation.frame.external") { - updated_params.gravity_compensation.frame.external = param.as_bool(); - } - if (param.get_name() == "gravity_compensation.CoG.pos") { - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 3); - !validation_result.success()) { - return validation_result; - } - updated_params.gravity_compensation.CoG.pos = param.as_double_array(); - } - if (param.get_name() == "gravity_compensation.CoG.force") { - updated_params.gravity_compensation.CoG.force = param.as_double(); - } - if (param.get_name() == "admittance.selected_axes") { - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - return validation_result; - } - updated_params.admittance.selected_axes = param.as_bool_array(); - } - if (param.get_name() == "admittance.mass") { - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - return validation_result; - } - if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0001, 1000000.0); - !validation_result.success()) { - return validation_result; - } - updated_params.admittance.mass = param.as_double_array(); - } - if (param.get_name() == "admittance.damping_ratio") { - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - return validation_result; - } - updated_params.admittance.damping_ratio = param.as_double_array(); - } - if (param.get_name() == "admittance.stiffness") { - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - return validation_result; - } - if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0, 100000000.0); - !validation_result.success()) { - return validation_result; - } - updated_params.admittance.stiffness = param.as_double_array(); - } - if (param.get_name() == "robot_description") { - updated_params.robot_description = param.as_string(); - } - if (param.get_name() == "enable_parameter_update_without_reactivation") { - updated_params.enable_parameter_update_without_reactivation = param.as_bool(); - } - } - - updated_params.__stamp = clock_.now(); - params_ = updated_params; - return gen_param_struct_validators::OK; - } - - void declare_params(){ - // declare all parameters and give default values to non-required ones - if (!parameters_interface_->has_parameter("joints")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which joints will be used by the controller"; - descriptor.read_only = true; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface_->declare_parameter("joints", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("command_interfaces")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which command interfaces to claim"; - descriptor.read_only = true; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface_->declare_parameter("command_interfaces", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("state_interfaces")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which state interfaces to claim"; - descriptor.read_only = true; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface_->declare_parameter("state_interfaces", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("chainable_command_interfaces")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which chainable interfaces to claim"; - descriptor.read_only = true; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING_ARRAY; - parameters_interface_->declare_parameter("chainable_command_interfaces", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("kinematics.plugin_name")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies which kinematics plugin to load"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("kinematics.plugin_name", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("kinematics.plugin_package")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the package to load the kinematics plugin from"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("kinematics.plugin_package", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("kinematics.base")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the base link of the robot description used by the kinematics plugin"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("kinematics.base", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("kinematics.tip")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the end effector link of the robot description used by the kinematics plugin"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("kinematics.tip", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("kinematics.alpha")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the damping coefficient for the Jacobian pseudo inverse"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.kinematics.alpha); - parameters_interface_->declare_parameter("kinematics.alpha", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("ft_sensor.name")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "name of the force torque sensor in the robot description"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("ft_sensor.name", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("ft_sensor.frame.id")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "frame of the force torque sensor"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("ft_sensor.frame.id", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("ft_sensor.frame.external")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.ft_sensor.frame.external); - parameters_interface_->declare_parameter("ft_sensor.frame.external", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("ft_sensor.filter_coefficient")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies the coefficient for the sensor's exponential filter"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.ft_sensor.filter_coefficient); - parameters_interface_->declare_parameter("ft_sensor.filter_coefficient", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("control.frame.id")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "control frame used for admittance control"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("control.frame.id", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("control.frame.external")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the control frame is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.control.frame.external); - parameters_interface_->declare_parameter("control.frame.external", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("fixed_world_frame.frame.id")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "world frame, gravity points down (neg. Z) in this frame"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("fixed_world_frame.frame.id", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("fixed_world_frame.frame.external")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the world frame is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.fixed_world_frame.frame.external); - parameters_interface_->declare_parameter("fixed_world_frame.frame.external", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("gravity_compensation.frame.id")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "frame which center of gravity (CoG) is defined in"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("gravity_compensation.frame.id", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("gravity_compensation.frame.external")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.gravity_compensation.frame.external); - parameters_interface_->declare_parameter("gravity_compensation.frame.external", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("gravity_compensation.CoG.pos")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "position of the center of gravity (CoG) in its frame"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface_->declare_parameter("gravity_compensation.CoG.pos", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("gravity_compensation.CoG.force")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "weight of the end effector, e.g mass * 9.81"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.gravity_compensation.CoG.force); - parameters_interface_->declare_parameter("gravity_compensation.CoG.force", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("admittance.selected_axes")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies if the axes x, y, z, rx, ry, and rz are enabled"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_BOOL_ARRAY; - parameters_interface_->declare_parameter("admittance.selected_axes", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("admittance.mass")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface_->declare_parameter("admittance.mass", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("admittance.damping_ratio")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface_->declare_parameter("admittance.damping_ratio", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("admittance.stiffness")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; - parameters_interface_->declare_parameter("admittance.stiffness", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("robot_description")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics."; - descriptor.read_only = true; - auto parameter = rclcpp::ParameterType::PARAMETER_STRING; - parameters_interface_->declare_parameter("robot_description", parameter, descriptor); - } - if (!parameters_interface_->has_parameter("enable_parameter_update_without_reactivation")) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = "if enabled, parameters will be dynamically updated in the control loop"; - descriptor.read_only = false; - auto parameter = rclcpp::ParameterValue(params_.enable_parameter_update_without_reactivation); - parameters_interface_->declare_parameter("enable_parameter_update_without_reactivation", parameter, descriptor); - } - // get parameters and fill struct fields - rclcpp::Parameter param; - param = parameters_interface_->get_parameter("joints"); - params_.joints = param.as_string_array(); - param = parameters_interface_->get_parameter("command_interfaces"); - params_.command_interfaces = param.as_string_array(); - param = parameters_interface_->get_parameter("state_interfaces"); - params_.state_interfaces = param.as_string_array(); - param = parameters_interface_->get_parameter("chainable_command_interfaces"); - params_.chainable_command_interfaces = param.as_string_array(); - param = parameters_interface_->get_parameter("kinematics.plugin_name"); - params_.kinematics.plugin_name = param.as_string(); - param = parameters_interface_->get_parameter("kinematics.plugin_package"); - params_.kinematics.plugin_package = param.as_string(); - param = parameters_interface_->get_parameter("kinematics.base"); - params_.kinematics.base = param.as_string(); - param = parameters_interface_->get_parameter("kinematics.tip"); - params_.kinematics.tip = param.as_string(); - param = parameters_interface_->get_parameter("kinematics.alpha"); - params_.kinematics.alpha = param.as_double(); - param = parameters_interface_->get_parameter("ft_sensor.name"); - params_.ft_sensor.name = param.as_string(); - param = parameters_interface_->get_parameter("ft_sensor.frame.id"); - params_.ft_sensor.frame.id = param.as_string(); - param = parameters_interface_->get_parameter("ft_sensor.frame.external"); - params_.ft_sensor.frame.external = param.as_bool(); - param = parameters_interface_->get_parameter("ft_sensor.filter_coefficient"); - params_.ft_sensor.filter_coefficient = param.as_double(); - param = parameters_interface_->get_parameter("control.frame.id"); - params_.control.frame.id = param.as_string(); - param = parameters_interface_->get_parameter("control.frame.external"); - params_.control.frame.external = param.as_bool(); - param = parameters_interface_->get_parameter("fixed_world_frame.frame.id"); - params_.fixed_world_frame.frame.id = param.as_string(); - param = parameters_interface_->get_parameter("fixed_world_frame.frame.external"); - params_.fixed_world_frame.frame.external = param.as_bool(); - param = parameters_interface_->get_parameter("gravity_compensation.frame.id"); - params_.gravity_compensation.frame.id = param.as_string(); - param = parameters_interface_->get_parameter("gravity_compensation.frame.external"); - params_.gravity_compensation.frame.external = param.as_bool(); - param = parameters_interface_->get_parameter("gravity_compensation.CoG.pos"); - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 3); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'gravity_compensation.CoG.pos': " + validation_result.error_msg())); - } - params_.gravity_compensation.CoG.pos = param.as_double_array(); - param = parameters_interface_->get_parameter("gravity_compensation.CoG.force"); - params_.gravity_compensation.CoG.force = param.as_double(); - param = parameters_interface_->get_parameter("admittance.selected_axes"); - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.selected_axes': " + validation_result.error_msg())); - } - params_.admittance.selected_axes = param.as_bool_array(); - param = parameters_interface_->get_parameter("admittance.mass"); - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.mass': " + validation_result.error_msg())); - } - if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0001, 1000000.0); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.mass': " + validation_result.error_msg())); - } - params_.admittance.mass = param.as_double_array(); - param = parameters_interface_->get_parameter("admittance.damping_ratio"); - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.damping_ratio': " + validation_result.error_msg())); - } - params_.admittance.damping_ratio = param.as_double_array(); - param = parameters_interface_->get_parameter("admittance.stiffness"); - if(auto validation_result = gen_param_struct_validators::fixed_size(param, 6); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.stiffness': " + validation_result.error_msg())); - } - if(auto validation_result = gen_param_struct_validators::element_bounds(param, 0.0, 100000000.0); - !validation_result.success()) { - throw rclcpp::exceptions::InvalidParameterValueException(fmt::format("Invalid value set during initialization for parameter 'admittance.stiffness': " + validation_result.error_msg())); - } - params_.admittance.stiffness = param.as_double_array(); - param = parameters_interface_->get_parameter("robot_description"); - params_.robot_description = param.as_string(); - param = parameters_interface_->get_parameter("enable_parameter_update_without_reactivation"); - params_.enable_parameter_update_without_reactivation = param.as_bool(); - - - params_.__stamp = clock_.now(); - } - - private: - Params params_; - rclcpp::Clock clock_; - std::shared_ptr handle_; - std::shared_ptr parameters_interface_; - }; - -} // namespace admittance_controller diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 555681dcb4..9155b9c36b 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -29,8 +29,7 @@ tf2_geometry_msgs tf2_ros trajectory_msgs - - + generate_parameter_library ament_cmake_gmock From 451f6d4e0cdf0f4958f647b7c4f7d2a93f4822d5 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 9 Aug 2022 16:33:24 -0600 Subject: [PATCH 056/111] use Eigen tranformation types --- .../admittance_controller/admittance_rule.hpp | 144 +++----- .../admittance_rule_impl.hpp | 319 ++++-------------- admittance_controller/test/test_params.yaml | 2 +- 3 files changed, 110 insertions(+), 355 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 54ee6e081e..44a973cc53 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -35,62 +35,50 @@ #include "pluginlib/class_loader.hpp" -namespace utility { -// Numerical accuracy checks. Used as deadbands. - const double ROT_AXIS_EPSILON = 1e-12; -} // utility namespace - namespace admittance_controller { struct Transforms { - Transforms() { - base_control_.setIdentity(); - base_ft_.setIdentity(); - base_tip_.setIdentity(); - world_base_.setIdentity(); - base_sensor_.setIdentity(); - base_cog_.setIdentity(); - base_desired_ft_.setIdentity(); - } - - Eigen::Matrix base_control_; - Eigen::Matrix base_ft_; - Eigen::Matrix base_desired_ft_; - Eigen::Matrix base_tip_; - Eigen::Matrix world_base_; - Eigen::Matrix base_sensor_; - Eigen::Matrix base_cog_; + Eigen::Isometry3d base_control_; + Eigen::Isometry3d base_ft_; + Eigen::Isometry3d base_tip_; + Eigen::Isometry3d world_base_; + Eigen::Isometry3d base_sensor_; + Eigen::Isometry3d base_cog_; }; struct AdmittanceState { AdmittanceState() = default; AdmittanceState(int num_joints) { - admittance_position_.setIdentity(); - admittance_velocity_.setZero(); - admittance_acceleration_.setZero(); - damping_.setZero(); - mass_.setOnes(); - mass_inv_.setZero(); - stiffness_.setZero(); - selected_axes_.setZero(); - joint_pos_ = Eigen::Matrix::Zero(num_joints); - joint_vel_ = Eigen::Matrix::Zero(num_joints); - joint_acc_ = Eigen::Matrix::Zero(num_joints); + admittance_velocity.setZero(); + admittance_acceleration.setZero(); + damping.setZero(); + mass.setOnes(); + mass_inv.setZero(); + stiffness.setZero(); + selected_axes.setZero(); + current_joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_vel = Eigen::VectorXd::Zero(num_joints); + joint_acc = Eigen::VectorXd::Zero(num_joints); } - Eigen::Matrix joint_pos_; - Eigen::Matrix joint_vel_; - Eigen::Matrix joint_acc_; - Eigen::Matrix damping_; - Eigen::Matrix mass_; - Eigen::Matrix mass_inv_; - Eigen::Matrix selected_axes_; - Eigen::Matrix stiffness_; - Eigen::Matrix wrench_base; - Eigen::Matrix admittance_acceleration_; - Eigen::Matrix admittance_velocity_; - Eigen::Matrix admittance_position_; + Eigen::VectorXd current_joint_pos; + Eigen::VectorXd joint_pos; + Eigen::VectorXd joint_vel; + Eigen::VectorXd joint_acc; + Eigen::Matrix damping; + Eigen::Matrix mass; + Eigen::Matrix mass_inv; + Eigen::Matrix selected_axes; + Eigen::Matrix stiffness; + Eigen::Matrix wrench_base; + Eigen::Matrix admittance_acceleration; + Eigen::Matrix admittance_velocity; + Eigen::Isometry3d admittance_position; + Eigen::Matrix rot_base_control; + Eigen::Isometry3d ref_trans_base_ft; + std::string ft_sensor_frame; }; class AdmittanceRule { @@ -171,14 +159,7 @@ namespace admittance_controller { * \param[in] admittance_state * \param[out] success */ - bool calculate_admittance_rule(const std::vector ¤t_joint_positions, - const Eigen::Matrix &base_control, - Eigen::Matrix base_ft, - Eigen::Matrix ref_base_ft, - Eigen::Matrix base_desired_ft, - const std::string &ft_sensor_frame, - double dt, - AdmittanceState &admittance_state); + bool calculate_admittance_rule(AdmittanceState &admittance_state, double dt); /** * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement. The `wrench_world_` @@ -192,66 +173,24 @@ namespace admittance_controller { const Eigen::Matrix &cog_rot ); - /** - * Returns the axis of rotation of a given rotation matrix - * \param[in] R - * \param[out] rotation_axis - */ - Eigen::Vector3d get_rotation_axis(const Eigen::Matrix3d &R) const; - - /** - * Normalizes given rotation matrix `R` - * \param[in] R - */ - static void normalize_rotation(Eigen::Matrix &R); - - bool convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, - const Eigen::Matrix &cartesian_delta, - const std::string &link_name, - Eigen::Matrix &joint_delta); - - bool convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, - const std::vector &joint_delta, - const std::string &link_name, - Eigen::Matrix &cartesian_delta); - - - /** - * Calculates the transform from the specified link to the robot's base link at the given joint positions. If - * `external` is set to true, then the link transform with will queried using TF lookup. - * \param[in] positions - * \param[in] link_name - * \param[in] external - * \param[in] transform - */ - bool get_transform(const std::vector &positions, const std::string &link_name, bool external, - Eigen::Matrix &transform); - - void eigen_to_msg(const Eigen::Matrix &wrench, const std::string &frame_id, - geometry_msgs::msg::WrenchStamped &wrench_msg); - template - void vec_to_eigen(const std::vector& data, T2 &matrix); + void vec_to_eigen(const std::vector &data, T2 &matrix); template - void eigen_to_vec(const T2 &matrix, std::vector& data); + void eigen_to_vec(const T2 &matrix, std::vector &data); // number of robot joint int num_joints_; // Kinematics interface plugin loader - std::shared_ptr> kinematics_loader_; - std::unique_ptr kinematics_; - - // buffers to pass data to kinematics interface - std::vector transform_buffer_vec_; - std::vector joint_buffer_vec_; - std::vector cart_buffer_vec_; + std::shared_ptr> kinematics_loader_; + std::unique_ptr kinematics_; // measured wrench in ft frame - Eigen::Matrix measured_wrench_; +// Eigen::Matrix measured_wrench_; + // filtered wrench in world frame - Eigen::Matrix wrench_world_; + Eigen::Matrix wrench_world_; // admittance controllers internal state AdmittanceState admittance_state_; @@ -267,7 +206,6 @@ namespace admittance_controller { Eigen::Vector3d ee_weight; // ROS - trajectory_msgs::msg::JointTrajectoryPoint admittance_rule_calculated_values_; control_msgs::msg::AdmittanceControllerState state_message_; std::shared_ptr clock_; std::shared_ptr tf_buffer_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 61478b7a35..132bc845ee 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -42,11 +42,11 @@ namespace admittance_controller { // Load the differential IK plugin if (!parameters_.kinematics.plugin_name.empty()) { try { - kinematics_loader_ = std::make_shared>( - parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsBaseClass"); - kinematics_ = std::unique_ptr( + kinematics_loader_ = std::make_shared>( + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterfaceBase"); + kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); - if (!kinematics_->initialize(node, parameters_.kinematics.tip)) { + if (!kinematics_->initialize(node->get_node_parameters_interface(), parameters_.kinematics.tip)) { return controller_interface::return_type::ERROR; } } @@ -67,11 +67,6 @@ namespace admittance_controller { controller_interface::return_type AdmittanceRule::reset(int num_joints) { // reset all values back to default - //allocate dynamic buffers - joint_buffer_vec_.assign(num_joints, 0.0); - transform_buffer_vec_.assign(16, 0.0); - cart_buffer_vec_.assign(6, 0.0); - // reset state message fields state_message_.error_joint_state.positions.resize(num_joints, 0.0); state_message_.error_joint_state.velocities.resize(num_joints, 0.0); @@ -88,7 +83,6 @@ namespace admittance_controller { // reset forces wrench_world_.setZero(); - measured_wrench_.setZero(); ee_weight.setZero(); // load/initialize Eigen types from parameters @@ -104,14 +98,14 @@ namespace admittance_controller { // update param values ee_weight[2] = -parameters_.gravity_compensation.CoG.force; vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_); - vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass_); - vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness_); - vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes_); + vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); + vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); + vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); for (auto i = 0ul; i < 6; ++i) { - admittance_state_.mass_inv_[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping_[i] = parameters_.admittance.damping_ratio[i] * 2 * - sqrt(admittance_state_.mass_[i] * admittance_state_.stiffness_[i]); + admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); } } @@ -120,39 +114,20 @@ namespace admittance_controller { const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, const AdmittanceState &admittance_state) { // get all reference transforms - bool success = get_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_ref_.base_ft_); - success &= get_transform(reference_joint_state.positions, parameters_.kinematics.tip, - false, trans_ref_.base_tip_); - success &= get_transform(reference_joint_state.positions, parameters_.fixed_world_frame.frame.id, - parameters_.fixed_world_frame.frame.external, trans_ref_.world_base_); - success &= get_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_ref_.base_sensor_); - success &= get_transform(reference_joint_state.positions, parameters_.gravity_compensation.frame.id, - parameters_.gravity_compensation.frame.external, trans_ref_.base_cog_); - success &= get_transform(reference_joint_state.positions, parameters_.control.frame.id, - parameters_.control.frame.external, trans_ref_.base_control_); + bool success = kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, trans_ref_.base_ft_); + success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.kinematics.tip,trans_ref_.base_tip_); + success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.fixed_world_frame.frame.id,trans_ref_.world_base_); + success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id,trans_ref_.base_sensor_); + success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.gravity_compensation.frame.id,trans_ref_.base_cog_); + success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.control.frame.id,trans_ref_.base_control_); // get all current transforms - success &= get_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_.base_ft_); - success &= get_transform(current_joint_state.positions, parameters_.kinematics.tip, - false, trans_.base_tip_); - success &= get_transform(current_joint_state.positions, parameters_.fixed_world_frame.frame.id, - parameters_.fixed_world_frame.frame.external, trans_.world_base_); - success &= get_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_.base_sensor_); - success &= get_transform(current_joint_state.positions, parameters_.gravity_compensation.frame.id, - parameters_.gravity_compensation.frame.external, trans_.base_cog_); - success &= get_transform(current_joint_state.positions, parameters_.control.frame.id, - parameters_.control.frame.external, trans_.base_control_); - - // get desired transforms (reference + admittance) - for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { - joint_buffer_vec_[i] = reference_joint_state.positions[i] + admittance_state.joint_pos_[i]; - } - success &= get_transform(joint_buffer_vec_, parameters_.ft_sensor.frame.id, - parameters_.ft_sensor.frame.external, trans_ref_.base_desired_ft_); + success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id,trans_.base_ft_); + success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.kinematics.tip,trans_.base_tip_); + success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.fixed_world_frame.frame.id,trans_.world_base_); + success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id,trans_.base_sensor_); + success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.gravity_compensation.frame.id,trans_.base_cog_); + success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.control.frame.id,trans_.base_control_); return success; } @@ -174,10 +149,10 @@ namespace admittance_controller { bool success = get_all_transforms(current_joint_state, reference_joint_state, admittance_state_); // calculate needed rotations - Eigen::Matrix rot_base_sensor = trans_.base_sensor_.block<3,3>(0,0); - Eigen::Matrix rot_world_base = trans_.world_base_.block<3,3>(0,0); - Eigen::Matrix rot_base_cog = trans_.base_cog_.block<3,3>(0,0); - Eigen::Matrix rot_base_control = trans_.base_control_.block<3,3>(0,0); + Eigen::Matrix rot_base_sensor = trans_.base_sensor_.rotation(); + Eigen::Matrix rot_world_base = trans_.world_base_.rotation(); + Eigen::Matrix rot_base_cog = trans_.base_cog_.rotation(); + Eigen::Matrix rot_base_control = trans_.base_control_.rotation(); // apply filter and update wrench_world_ vector Eigen::Matrix rot_world_sensor = rot_world_base*rot_base_sensor; @@ -185,12 +160,15 @@ namespace admittance_controller { process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); // transform wrench_world_ into base frame - // transform wrench_world_ into base frame - admittance_state_.wrench_base = rot_world_base.transpose() * wrench_world_; + admittance_state_.wrench_base.block<3,1>(0,0) = rot_world_base.transpose() * wrench_world_.block<3,1>(0,0); + admittance_state_.wrench_base.block<3,1>(3,0) = rot_world_base.transpose() * wrench_world_.block<3,1>(3,0); // Compute admittance control law - success &= calculate_admittance_rule(current_joint_state.positions, rot_base_control, trans_.base_ft_, - trans_ref_.base_ft_, trans_ref_.base_desired_ft_, parameters_.ft_sensor.frame.id, dt, admittance_state_); + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = rot_base_control; + admittance_state_.ref_trans_base_ft = trans_ref_.base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + success &= calculate_admittance_rule(admittance_state_, dt); // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference if (!success) { @@ -200,17 +178,17 @@ namespace admittance_controller { // update joint desired joint state for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { - desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos_[i]; + desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; state_message_.error_joint_state.positions[i] = reference_joint_state.positions[i] - current_joint_state.positions[i]; } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { - desired_joint_state.velocities[i] = admittance_state_.joint_vel_[i]; + desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; state_message_.error_joint_state.velocities[i] = reference_joint_state.velocities[i] - current_joint_state.velocities[i]; } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { - desired_joint_state.accelerations[i] = admittance_state_.joint_acc_[i]; + desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; } // update admittance state message @@ -220,23 +198,17 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - bool AdmittanceRule::calculate_admittance_rule(const std::vector ¤t_joint_positions, - const Eigen::Matrix &base_control, - Eigen::Matrix base_ft, - Eigen::Matrix ref_base_ft, - Eigen::Matrix base_desired_ft, - const std::string &ft_sensor_frame, - double dt, - AdmittanceState &admittance_state) { + bool AdmittanceRule::calculate_admittance_rule(AdmittanceState &admittance_state, double dt) { // create stiffness matrix + auto rot_base_control = admittance_state.rot_base_control; Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); Eigen::Matrix K_rot = Eigen::Matrix::Zero(); - K_pos.diagonal() = admittance_state.stiffness_.block<3, 1>(0, 0); - K_rot.diagonal() = admittance_state.stiffness_.block<3, 1>(3, 0); - K_pos = base_control * K_pos * base_control.transpose(); - K_rot = base_control * K_rot * base_control.transpose(); + K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); + K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); + K_pos = rot_base_control * K_pos * rot_base_control.transpose(); + K_rot = rot_base_control * K_rot * rot_base_control.transpose(); K.block<3, 3>(0, 0) = K_pos; K.block<3, 3>(3, 3) = K_rot; @@ -244,47 +216,44 @@ namespace admittance_controller { Eigen::Matrix D = Eigen::Matrix::Zero(); Eigen::Matrix D_pos = Eigen::Matrix::Zero(); Eigen::Matrix D_rot = Eigen::Matrix::Zero(); - D_pos.diagonal() = admittance_state.damping_.block<3, 1>(0, 0); - D_rot.diagonal() = admittance_state.damping_.block<3, 1>(3, 0); - D_pos = base_control * D_pos * base_control.transpose(); - D_rot = base_control * D_rot * base_control.transpose(); + D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); + D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); + D_pos = rot_base_control * D_pos * rot_base_control.transpose(); + D_rot = rot_base_control * D_rot * rot_base_control.transpose(); D.block<3, 3>(0, 0) = D_pos; D.block<3, 3>(3, 3) = D_rot; // calculate admittance relative offset + Eigen::Isometry3d desired_trans_base_ft; + kinematics_->calculate_link_transform(admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); Eigen::Matrix X; - X.block<3, 1>(0, 0) = base_desired_ft.block<3, 1>(0, 3) - ref_base_ft.block<3, 1>(0, 3); - Eigen::Matrix R_ref = ref_base_ft.block<3, 3>(0, 0); - Eigen::Matrix R_desired = base_desired_ft.block<3, 3>(0, 0); - Eigen::Matrix R = R_desired * R_ref.transpose(); - Eigen::Vector3d V = get_rotation_axis(R); - double theta = acos(std::clamp((1.0 / 2.0) * (R.trace() - 1), -1.0, 1.0)); - X.block<3, 1>(3, 0) = -theta * V; + X.block<3, 1>(0, 0) = desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation(); + auto R_ref = admittance_state.ref_trans_base_ft.rotation(); + auto R_desired = desired_trans_base_ft.rotation(); + auto R = R_desired * R_ref.transpose(); + auto angle_axis = Eigen::AngleAxisd(R); + X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); // get admittance relative velocity - auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity_.data()); + auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); // get external force auto F = Eigen::Matrix(admittance_state.wrench_base.data()); // Compute admittance control law: F = M*a + D*v + S*x - Eigen::Matrix X_ddot = admittance_state.mass_inv_.cwiseProduct(F - D * X_dot - K * X); - auto tmp = Eigen::Matrix(X_ddot.data()); - bool success = convert_cartesian_deltas_to_joint_deltas(current_joint_positions, tmp, - ft_sensor_frame, - admittance_state.joint_acc_); + Eigen::Matrix X_ddot = admittance_state.mass_inv.cwiseProduct(F - D * X_dot - K * X); + bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(admittance_state.current_joint_pos, X_ddot, + admittance_state.ft_sensor_frame, admittance_state.joint_acc); // integrate motion in joint space - admittance_state.joint_vel_ += admittance_state.joint_acc_ * dt; - admittance_state.joint_pos_ += admittance_state.joint_vel_ * dt; + admittance_state.joint_vel += admittance_state.joint_acc * dt; + admittance_state.joint_pos += admittance_state.joint_vel * dt; // calculate admittance velocity corresponding to joint velocity - eigen_to_vec(admittance_state.joint_vel_, joint_buffer_vec_); - success &= convert_joint_deltas_to_cartesian_deltas(current_joint_positions, joint_buffer_vec_, ft_sensor_frame, - admittance_state.admittance_velocity_); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(admittance_state.current_joint_pos, admittance_state.joint_vel, admittance_state.ft_sensor_frame, + admittance_state.admittance_velocity); return success; - } void AdmittanceRule::process_wrench_measurements( @@ -302,9 +271,6 @@ namespace admittance_controller { // transform to world frame Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; - // store value for state message - measured_wrench_ = new_wrench_base; - // apply gravity compensation new_wrench_base(2, 0) -= ee_weight[2]; new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight); @@ -317,152 +283,14 @@ namespace admittance_controller { } - Eigen::Vector3d AdmittanceRule::get_rotation_axis(const Eigen::Matrix3d &R) const { - - Eigen::Vector3d V; - bool solved = false; - double v1 = 1; - double v2, v3; - double R_buffer[9]; - - // rotate matrix rows and columns to hit every edge case - for (int i = 0; i < 3; i++) { - - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - R_buffer[((c * 3 - 3 * i) % 9) + ((r - i) % 3)] = R(r, c); - } - } - auto R11 = R_buffer[0]; - auto R12 = R_buffer[1]; - auto R32 = R_buffer[7]; - auto R13 = R_buffer[2]; - auto R33 = R_buffer[8]; - // degenerate: one axis rotation - if (abs(R12 + R13) < utility::ROT_AXIS_EPSILON && R11 > 0) { - v2 = 0; - v3 = 0; - V[i % 3] = v1; - V[(i + 1) % 3] = v2; - V[(i + 2) % 3] = v3; - solved = true; - break; - } - // degenerate: two axis rotation - if (abs(R12 + R13) < utility::ROT_AXIS_EPSILON && R11 < 0) { - v1 = 0; - v2 = 1; - v3 = R32 / (1 - R33); - V[i % 3] = v1; - V[(i + 1) % 3] = v2; - V[(i + 2) % 3] = v3; - solved = true; - break; - } - } - - // general case: three axis rotation - if (!solved) { - v3 = (-R(0, 1) - ((R(1, 1) - 1) * (1 - R(0, 0))) / R(1, 0)); - // if v3 is zero, special case - if (abs(v3) > utility::ROT_AXIS_EPSILON) { - v3 = v3 / (R(2, 1) - ((R(1, 1) - 1) * (R(2, 0))) / (R(1, 0))); - } - v2 = (1 - R(0, 0) - R(2, 0) * v3) / R(1, 0); - - V[0] = v1; - V[1] = v2; - V[2] = v3; - } - - V.normalize(); - - // if trace of the rotation matrix derivative is negative, then rotation axis needs to be flipped - auto tmp = V[0] * (R(1, 2) - R(2, 1)) + V[1] * (R(2, 0) - R(0, 2)) - + V[2] * (R(0, 1) - R(1, 0)); - double sign = (tmp >= 0) ? 1.0 : -1.0; - - return sign * V; - } - - void AdmittanceRule::normalize_rotation(Eigen::Matrix3d &R) { - // enforce orthonormal constraint - - Eigen::Vector3d R_0 = R.block<3, 1>(0, 0); - R_0.normalize(); - Eigen::Vector3d R_1 = R.block<3, 1>(0, 1); - R_1.normalize(); - Eigen::Vector3d R_2 = R.block<3, 1>(0, 2); - R_2.normalize(); - - double drift = R_0.dot(R_1); - R_1 += R_0 * (-drift); - R_1.normalize(); - - R_2 = R_0.cross(R_1); - R_2.normalize(); - - Eigen::Matrix3d R_out; - R.col(0) << R_0; - R.col(1) << R_1; - R.col(2) << R_2; - - } - - bool - AdmittanceRule::get_transform(const std::vector &positions, const std::string &link_name, bool external, - Eigen::Matrix &transform) { - bool success; - if (external) { - transform.setIdentity(); - try { - auto transform_msg = tf_buffer_->lookupTransform(parameters_.kinematics.base, link_name, tf2::TimePointZero); - transform.block<3, 3>(0, 0) = Eigen::Matrix(tf2::transformToKDL(transform_msg).M.data); - transform(0, 3) = transform_msg.transform.translation.x; - transform(1, 3) = transform_msg.transform.translation.y; - transform(2, 3) = transform_msg.transform.translation.z; - } catch (const tf2::TransformException &e) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE( - rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); - success = false; - } - } else { - success = kinematics_->calculate_link_transform(positions, link_name, transform_buffer_vec_); - vec_to_eigen(transform_buffer_vec_, transform); - } - return success; - } - - bool AdmittanceRule::convert_cartesian_deltas_to_joint_deltas(const std::vector &positions, - const Eigen::Matrix &cartesian_delta, - const std::string &link_name, - Eigen::Matrix &joint_delta) { - memcpy(cart_buffer_vec_.data(), cartesian_delta.data(), 6 * sizeof(double)); - bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(positions, cart_buffer_vec_, link_name, - joint_buffer_vec_); - joint_delta = Eigen::Map>(joint_buffer_vec_.data(), num_joints_, 1); - return success; - } - - bool AdmittanceRule::convert_joint_deltas_to_cartesian_deltas(const std::vector &positions, - const std::vector &joint_delta, - const std::string &link_name, - Eigen::Matrix &cartesian_delta) { - - bool success = kinematics_->convert_joint_deltas_to_cartesian_deltas(positions, joint_delta, link_name, - cart_buffer_vec_); - vec_to_eigen(cart_buffer_vec_, cartesian_delta); - return success; - } - void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { // TODO these fields are not used - eigen_to_msg(0 * wrench_world_, parameters_.control.frame.id, state_message.input_wrench_command); - state_message.input_pose_command.header.frame_id = parameters_.control.frame.id; - - eigen_to_msg(wrench_world_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench_filtered); - eigen_to_msg(measured_wrench_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench); +// eigen_to_msg(0 * wrench_world_, parameters_.control.frame.id, state_message.input_wrench_command); +// state_message.input_pose_command.header.frame_id = parameters_.control.frame.id; +// +// eigen_to_msg(wrench_world_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench_filtered); +// eigen_to_msg(measured_wrench_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench); // eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_.control_.frame_.id_, // state_message.measured_wrench_control_frame); // eigen_to_msg(ee_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_.kinematics_.tip_, @@ -478,18 +306,6 @@ namespace admittance_controller { } - void AdmittanceRule::eigen_to_msg(const Eigen::Matrix &wrench, const std::string &frame_id, - geometry_msgs::msg::WrenchStamped &wrench_msg) { - wrench_msg.wrench.force.x = wrench(0, 0); - wrench_msg.wrench.force.y = wrench(1, 0); - wrench_msg.wrench.force.z = wrench(2, 0); - wrench_msg.wrench.torque.x = wrench(0, 1); - wrench_msg.wrench.torque.y = wrench(1, 1); - wrench_msg.wrench.torque.z = wrench(2, 1); - wrench_msg.header.frame_id = frame_id; - wrench_msg.header.stamp = clock_->now(); - } - template void AdmittanceRule::vec_to_eigen(const std::vector &data, T2 &matrix) { for (auto col = 0; col < matrix.cols(); col++) { @@ -508,6 +324,7 @@ namespace admittance_controller { } } + } // namespace admittance_controller #endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 20ad6c7682..e7676b6c91 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -39,7 +39,7 @@ test_admittance_controller: - velocity kinematics: - plugin_name: kinematics_interface_kdl/KDLKinematics + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL plugin_package: kinematics_interface_kdl base: base_link # Assumed to be stationary tip: tool0 From 8c1bcf482d222b304c89e3f34d4c67f0639827a9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 9 Aug 2022 17:14:20 -0600 Subject: [PATCH 057/111] update state message --- .../admittance_controller/admittance_rule.hpp | 1 + .../admittance_rule_impl.hpp | 41 ++++--------------- .../src/admittance_controller.cpp | 9 +--- .../test/test_admittance_controller.cpp | 5 +-- 4 files changed, 12 insertions(+), 44 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 44a973cc53..78aeaf30e0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -23,6 +23,7 @@ #include "tf2_ros/transform_listener.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include +#include #include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 132bc845ee..4a87dfe187 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -68,11 +68,13 @@ namespace admittance_controller { // reset all values back to default // reset state message fields - state_message_.error_joint_state.positions.resize(num_joints, 0.0); - state_message_.error_joint_state.velocities.resize(num_joints, 0.0); - state_message_.admittance_rule_calculated_values.positions.resize(6, 0.0); - state_message_.admittance_rule_calculated_values.velocities.resize(6, 0.0); - state_message_.admittance_rule_calculated_values.accelerations.resize(6, 0.0); + for (const auto &name : parameters_.joints){ + state_message_.joint_state.name.push_back(name); + } + state_message_.mass.data.resize(6, 0.0); + state_message_.selected_axes.data.resize(6, 0); + state_message_.damping.data.resize(6, 0); + state_message_.stiffness.data.resize(6, 0); // reset admittance state admittance_state_ = AdmittanceState(num_joints); @@ -179,22 +181,14 @@ namespace admittance_controller { // update joint desired joint state for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; - state_message_.error_joint_state.positions[i] = - reference_joint_state.positions[i] - current_joint_state.positions[i]; } for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; - state_message_.error_joint_state.velocities[i] = - reference_joint_state.velocities[i] - current_joint_state.velocities[i]; } for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; } - // update admittance state message - state_message_.actual_joint_state = current_joint_state; - state_message_.desired_joint_state = desired_joint_state; - return controller_interface::return_type::OK; } @@ -284,26 +278,7 @@ namespace admittance_controller { } void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { - - // TODO these fields are not used -// eigen_to_msg(0 * wrench_world_, parameters_.control.frame.id, state_message.input_wrench_command); -// state_message.input_pose_command.header.frame_id = parameters_.control.frame.id; -// -// eigen_to_msg(wrench_world_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench_filtered); -// eigen_to_msg(measured_wrench_, parameters_.fixed_world_frame.frame.id, state_message.measured_wrench); -// eigen_to_msg(control_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_.control_.frame_.id_, -// state_message.measured_wrench_control_frame); -// eigen_to_msg(ee_rot_.transpose() * world_rot_.transpose() * measured_wrench_, parameters_.kinematics_.tip_, -// state_message.measured_wrench_endeffector_frame); - - state_message.joint_names = parameters_.joints; - state_message.desired_joint_state = state_message_.desired_joint_state; - state_message.actual_joint_state = state_message_.actual_joint_state; - state_message.error_joint_state = state_message_.error_joint_state; - - - state_message.admittance_rule_calculated_values = state_message_.admittance_rule_calculated_values; - + // TODO implement } template diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 0dbacbde80..770be90119 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -189,10 +189,7 @@ namespace admittance_controller { // Initialize state message state_publisher_->lock(); - state_publisher_->msg_.joint_names = admittance_->parameters_.joints; - state_publisher_->msg_.actual_joint_state.positions.resize(num_joints_, 0.0); - state_publisher_->msg_.desired_joint_state.positions.resize(num_joints_, 0.0); - state_publisher_->msg_.error_joint_state.positions.resize(num_joints_, 0.0); + admittance_->get_controller_state(state_publisher_->msg_); state_publisher_->unlock(); // Initialize FTS semantic semantic_component @@ -306,10 +303,6 @@ namespace admittance_controller { // Publish controller state state_publisher_->lock(); - state_publisher_->msg_.input_joint_command = pre_admittance_point; - state_publisher_->msg_.desired_joint_state = state_desired_; - state_publisher_->msg_.actual_joint_state = state_current_; - state_publisher_->msg_.error_joint_state = state_error_; admittance_->get_controller_state(state_publisher_->msg_); state_publisher_->unlockAndPublish(); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index b74a4d46ee..47084611ec 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -337,8 +337,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); - ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.wrench_base.header.frame_id, control_frame_); + ASSERT_EQ(msg.wrench_base.header.frame_id, control_frame_); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -349,7 +349,6 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); subscribe_and_get_messages(msg); - ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); } int main(int argc, char** argv) { From bff17df6f4eaaea4c19539a880fc9b4366cc25d2 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 9 Aug 2022 17:28:48 -0600 Subject: [PATCH 058/111] remove warnings --- .../admittance_controller/admittance_controller.hpp | 13 +------------ .../admittance_controller/admittance_rule.hpp | 6 +++--- .../admittance_controller/admittance_rule_impl.hpp | 4 ++-- admittance_controller/src/admittance_controller.cpp | 13 ++++++++----- 4 files changed, 14 insertions(+), 22 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 3ae10cbdf0..5a658cc76b 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -89,17 +89,7 @@ namespace admittance_controller { bool on_set_chained_mode(bool chained_mode) override; - int num_joints_ = 0; - - // vectors to hold joint hardware interfaces -// std::vector joint_position_command_interface_; -// std::vector joint_velocity_command_interface_; -// std::vector joint_acceleration_command_interface_; -// std::vector joint_effort_command_interface_; -// std::vector joint_position_state_interface_; -// std::vector joint_velocity_state_interface_; -// std::vector joint_acceleration_state_interface_; -// std::vector joint_effort_state_interface_; + size_t num_joints_ = 0; size_t state_pos_ind = -1; size_t state_vel_ind = -1; size_t state_acc_ind = -1; @@ -107,7 +97,6 @@ namespace admittance_controller { size_t command_vel_ind = -1; size_t command_acc_ind = -1; - // internal reference values const std::vector reference_interfaces_types_ = {"position", "velocity"}; std::vector> position_reference_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 78aeaf30e0..92a755258d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -94,9 +94,9 @@ namespace admittance_controller { } controller_interface::return_type - configure(const std::shared_ptr &node, int num_joint); + configure(const std::shared_ptr &node, size_t num_joint); - controller_interface::return_type reset(int num_joints); + controller_interface::return_type reset(size_t num_joints); /** * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does @@ -181,7 +181,7 @@ namespace admittance_controller { void eigen_to_vec(const T2 &matrix, std::vector &data); // number of robot joint - int num_joints_; + size_t num_joints_; // Kinematics interface plugin loader std::shared_ptr> kinematics_loader_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 4a87dfe187..e240eafb62 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -26,7 +26,7 @@ namespace admittance_controller { controller_interface::return_type - AdmittanceRule::configure(const std::shared_ptr &node, int num_joints) { + AdmittanceRule::configure(const std::shared_ptr &node, size_t num_joints) { // configure admittance rule using num_joints and load kinematics interface num_joints_ = num_joints; @@ -64,7 +64,7 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - controller_interface::return_type AdmittanceRule::reset(int num_joints) { + controller_interface::return_type AdmittanceRule::reset(size_t num_joints) { // reset all values back to default // reset state message fields diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 770be90119..83b620a699 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -67,7 +67,8 @@ namespace admittance_controller { for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { const auto &interface = admittance_->parameters_.state_interfaces[i]; for (const auto &joint: admittance_->parameters_.joints) { - state_interfaces_config_names.push_back(joint + "/" + interface); + auto full_name = joint + "/" + interface; + state_interfaces_config_names.push_back(full_name); } } @@ -91,7 +92,8 @@ namespace admittance_controller { std::vector command_interfaces_config_names; for (const auto &interface: admittance_->parameters_.command_interfaces) { for (const auto &joint: admittance_->parameters_.joints) { - command_interfaces_config_names.push_back(joint + "/" + interface); + auto full_name = joint + "/" + interface; + command_interfaces_config_names.push_back(full_name); } } @@ -124,9 +126,10 @@ namespace admittance_controller { else if (hardware_interface::HW_IF_VELOCITY == interface) { velocity_reference_.emplace_back(reference_interfaces_[index]); } + auto full_name = joint + "/" + interface; chainable_command_interfaces.emplace_back( hardware_interface::CommandInterface(std::string(get_node()->get_name()), - joint + "/" + interface, + full_name, reference_interfaces_.data() + index++)); } @@ -335,7 +338,7 @@ namespace admittance_controller { // if any interface has nan values, assume state_reference is the last command state for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) { - for (auto inter_ind = 0; inter_ind < 3; inter_ind++) { + for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) { auto ind = joint_ind + num_joints_ * inter_ind; if (inter_ind == state_pos_ind) { state_current.positions[joint_ind] = state_interfaces_[ind].get_value(); @@ -373,7 +376,7 @@ namespace admittance_controller { // if any interface has nan values, assume state_reference is the last command state for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) { - for (auto inter_ind = 0; inter_ind < 3; inter_ind++) { + for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) { auto ind = joint_ind + num_joints_ * inter_ind; if (inter_ind == command_pos_ind) { command_interfaces_[ind].set_value(state_commanded.positions[joint_ind]); From 2c1282eea97a095aa6023a0aaa6333fbc8bad17f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 10 Aug 2022 10:23:27 -0600 Subject: [PATCH 059/111] implement state message information and enable selected axes --- .../admittance_controller/admittance_rule.hpp | 7 +- .../admittance_rule_impl.hpp | 100 +++++++++++++++--- .../src/admittance_controller.cpp | 4 +- 3 files changed, 90 insertions(+), 21 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 92a755258d..d939fb7bcb 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -22,6 +22,7 @@ #include #include "tf2_ros/transform_listener.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_eigen/tf2_eigen.h" #include #include #include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" @@ -50,7 +51,7 @@ namespace admittance_controller { struct AdmittanceState { AdmittanceState() = default; - AdmittanceState(int num_joints) { + AdmittanceState(size_t num_joints) { admittance_velocity.setZero(); admittance_acceleration.setZero(); damping.setZero(); @@ -90,7 +91,7 @@ namespace admittance_controller { parameters_ = parameter_handler_->get_params(); num_joints_ = parameters_.joints.size(); admittance_state_ = AdmittanceState(num_joints_); - + reset(num_joints_); } controller_interface::return_type @@ -138,7 +139,7 @@ namespace admittance_controller { * * \param[in] state_message */ - void get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message); + const control_msgs::msg::AdmittanceControllerState & get_controller_state(); public: // admittance config parameters diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index e240eafb62..d356184c3f 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -68,8 +68,12 @@ namespace admittance_controller { // reset all values back to default // reset state message fields - for (const auto &name : parameters_.joints){ - state_message_.joint_state.name.push_back(name); + state_message_.joint_state.name.assign(num_joints, ""); + state_message_.joint_state.position.assign(num_joints, 0); + state_message_.joint_state.velocity.assign(num_joints, 0); + state_message_.joint_state.effort.assign(num_joints, 0); + for (auto i = 0ul; i < parameters_.joints.size(); i++){ + state_message_.joint_state.name = parameters_.joints; } state_message_.mass.data.resize(6, 0.0); state_message_.selected_axes.data.resize(6, 0); @@ -232,10 +236,19 @@ namespace admittance_controller { auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); // get external force - auto F = Eigen::Matrix(admittance_state.wrench_base.data()); + auto F_base = Eigen::Matrix(admittance_state.wrench_base.data()); + + // zero out any forces in the control frame + Eigen::Matrix F_control; + F_control.block<3,1>(0,0) = rot_base_control.transpose()*F_base.block<3,1>(0,0); + F_control.block<3,1>(3,0) = rot_base_control.transpose()*F_base.block<3,1>(3,0); + F_control = F_control.cwiseProduct(admittance_state.selected_axes); + F_base.block<3,1>(0,0) = rot_base_control*F_control.block<3,1>(0,0); + F_base.block<3,1>(3,0) = rot_base_control*F_control.block<3,1>(3,0); + // Compute admittance control law: F = M*a + D*v + S*x - Eigen::Matrix X_ddot = admittance_state.mass_inv.cwiseProduct(F - D * X_dot - K * X); + Eigen::Matrix X_ddot = admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, admittance_state.joint_acc); @@ -246,6 +259,8 @@ namespace admittance_controller { // calculate admittance velocity corresponding to joint velocity success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(admittance_state.current_joint_pos, admittance_state.joint_vel, admittance_state.ft_sensor_frame, admittance_state.admittance_velocity); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(admittance_state.current_joint_pos, admittance_state.joint_acc, admittance_state.ft_sensor_frame, + admittance_state.admittance_acceleration); return success; } @@ -277,8 +292,71 @@ namespace admittance_controller { } - void AdmittanceRule::get_controller_state(control_msgs::msg::AdmittanceControllerState &state_message) { - // TODO implement + const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() { + for (auto i = 0ul; i < parameters_.joints.size(); i++) { + state_message_.joint_state.name[i] = parameters_.joints[i]; + } + for (auto i = 0l; i < admittance_state_.joint_pos.size(); i++){ + state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; + } + for (auto i = 0l; i < admittance_state_.joint_vel.size(); i++){ + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; + } + for (auto i = 0l; i < admittance_state_.joint_acc.size(); i++){ + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + } + for (auto i = 0l; i < admittance_state_.stiffness.size(); i++){ + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + } + for (auto i = 0l; i < admittance_state_.damping.size(); i++){ + state_message_.damping.data[i] = admittance_state_.damping[i]; + } + for (auto i = 0l; i < admittance_state_.selected_axes.size(); i++){ + state_message_.selected_axes.data[i] = (bool) admittance_state_.selected_axes[i]; + } + for (auto i = 0l; i < admittance_state_.mass.size(); i++){ + state_message_.mass.data[i] = admittance_state_.mass[i]; + } + + state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; + state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; + state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; + state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3]; + state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; + state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; + + state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; + state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; + state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; + state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3]; + state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; + state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; + + state_message_.admittance_acceleration.twist.linear.x = admittance_state_.admittance_acceleration[0]; + state_message_.admittance_acceleration.twist.linear.y = admittance_state_.admittance_acceleration[1]; + state_message_.admittance_acceleration.twist.linear.z = admittance_state_.admittance_acceleration[2]; + state_message_.admittance_acceleration.twist.angular.x = admittance_state_.admittance_acceleration[3]; + state_message_.admittance_acceleration.twist.angular.y = admittance_state_.admittance_acceleration[4]; + state_message_.admittance_acceleration.twist.angular.z = admittance_state_.admittance_acceleration[5]; + + state_message_.admittance_position.transform.translation.x = admittance_state_.admittance_position.translation().x(); + state_message_.admittance_position.transform.translation.y = admittance_state_.admittance_position.translation().y(); + state_message_.admittance_position.transform.translation.z = admittance_state_.admittance_position.translation().z(); + + state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + + state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + Eigen::Quaterniond quat(admittance_state_.rot_base_control); + state_message_.rot_base_control.w = quat.w(); + state_message_.rot_base_control.x = quat.x(); + state_message_.rot_base_control.y = quat.y(); + state_message_.rot_base_control.z = quat.z(); + + // TODO(anyone) remove dynamic allocation here + state_message_.ft_sensor_frame.data = admittance_state_.ft_sensor_frame; + + return state_message_; + } template @@ -290,16 +368,6 @@ namespace admittance_controller { } } - template - void AdmittanceRule::eigen_to_vec(const T2 &matrix, std::vector &data) { - for (auto col = 0; col < matrix.cols(); col++) { - for (auto row = 0; row < matrix.rows(); row++) { - data[row + col * matrix.rows()] = matrix(row, col); - } - } - } - - } // namespace admittance_controller #endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 83b620a699..00a393adfd 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -192,7 +192,7 @@ namespace admittance_controller { // Initialize state message state_publisher_->lock(); - admittance_->get_controller_state(state_publisher_->msg_); + state_publisher_->msg_ = admittance_->get_controller_state(); state_publisher_->unlock(); // Initialize FTS semantic semantic_component @@ -306,7 +306,7 @@ namespace admittance_controller { // Publish controller state state_publisher_->lock(); - admittance_->get_controller_state(state_publisher_->msg_); + state_publisher_->msg_ = admittance_->get_controller_state(); state_publisher_->unlockAndPublish(); return controller_interface::return_type::OK; From cc90178abb3b39e1a53b390cdb933c44f9db92dd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 10 Aug 2022 15:22:05 -0600 Subject: [PATCH 060/111] add test --- .../admittance_controller/admittance_rule.hpp | 16 +-- .../admittance_rule_impl.hpp | 15 +- .../test/test_admittance_controller.cpp | 133 +++++------------- 3 files changed, 47 insertions(+), 117 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index d939fb7bcb..6e2fdca3a5 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -41,11 +41,11 @@ namespace admittance_controller { struct Transforms { Eigen::Isometry3d base_control_; - Eigen::Isometry3d base_ft_; - Eigen::Isometry3d base_tip_; - Eigen::Isometry3d world_base_; - Eigen::Isometry3d base_sensor_; - Eigen::Isometry3d base_cog_; + Eigen::Isometry3d base_ft_; + Eigen::Isometry3d base_tip_; + Eigen::Isometry3d world_base_; + Eigen::Isometry3d base_sensor_; + Eigen::Isometry3d base_cog_; }; struct AdmittanceState { @@ -178,9 +178,6 @@ namespace admittance_controller { template void vec_to_eigen(const std::vector &data, T2 &matrix); - template - void eigen_to_vec(const T2 &matrix, std::vector &data); - // number of robot joint size_t num_joints_; @@ -188,9 +185,6 @@ namespace admittance_controller { std::shared_ptr> kinematics_loader_; std::unique_ptr kinematics_; - // measured wrench in ft frame -// Eigen::Matrix measured_wrench_; - // filtered wrench in world frame Eigen::Matrix wrench_world_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index d356184c3f..2c9b92462c 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -318,6 +318,7 @@ namespace admittance_controller { state_message_.mass.data[i] = admittance_state_.mass[i]; } + state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; @@ -325,6 +326,7 @@ namespace admittance_controller { state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; + state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; @@ -332,6 +334,7 @@ namespace admittance_controller { state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; + state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here state_message_.admittance_acceleration.twist.linear.x = admittance_state_.admittance_acceleration[0]; state_message_.admittance_acceleration.twist.linear.y = admittance_state_.admittance_acceleration[1]; state_message_.admittance_acceleration.twist.linear.z = admittance_state_.admittance_acceleration[2]; @@ -339,21 +342,21 @@ namespace admittance_controller { state_message_.admittance_acceleration.twist.angular.y = admittance_state_.admittance_acceleration[4]; state_message_.admittance_acceleration.twist.angular.z = admittance_state_.admittance_acceleration[5]; - state_message_.admittance_position.transform.translation.x = admittance_state_.admittance_position.translation().x(); - state_message_.admittance_position.transform.translation.y = admittance_state_.admittance_position.translation().y(); - state_message_.admittance_position.transform.translation.z = admittance_state_.admittance_position.translation().z(); - + state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here + state_message_.admittance_position.child_frame_id = "admittance_offset"; // TODO(anyone) remove dynamic allocation here state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; + state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + Eigen::Quaterniond quat(admittance_state_.rot_base_control); state_message_.rot_base_control.w = quat.w(); state_message_.rot_base_control.x = quat.x(); state_message_.rot_base_control.y = quat.y(); state_message_.rot_base_control.z = quat.z(); - // TODO(anyone) remove dynamic allocation here - state_message_.ft_sensor_frame.data = admittance_state_.ft_sensor_frame; + state_message_.ft_sensor_frame.data = admittance_state_.ft_sensor_frame; // TODO(anyone) remove dynamic allocation here return state_message_; diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 47084611ec..deec50b5e7 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -220,104 +220,37 @@ TEST_F(AdmittanceControllerTest, reactivate_success) controller_interface::return_type::OK); } -// TODO (pac48) this test needs to be enabled and edited once a admittance state message definition is finalized -//TEST_F(AdmittanceControllerTest, publish_status_success) -//{ -// -// SetUpController(); -// -// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); -// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); -// -// broadcast_tfs(); -// ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -// -// ControllerStateMsg msg; -// subscribe_and_get_messages(msg); -// -// // Check that wrench command are all zero since not used -// ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); -// ASSERT_EQ(msg.input_wrench_command.wrench.force.x, 0.0); -// ASSERT_EQ(msg.input_wrench_command.wrench.force.y, 0.0); -// ASSERT_EQ(msg.input_wrench_command.wrench.force.z, 0.0); -// ASSERT_EQ(msg.input_wrench_command.wrench.torque.x, 0.0); -// ASSERT_EQ(msg.input_wrench_command.wrench.torque.y, 0.0); -// ASSERT_EQ(msg.input_wrench_command.wrench.torque.z, 0.0); -// -// // Check Cartesian command message -// ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.x)); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.y)); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.position.z)); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.x)); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.y)); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.z)); -// ASSERT_FALSE(std::isnan(msg.input_pose_command.pose.orientation.w)); -// -// // Check joint command message -// ASSERT_TRUE(std::equal( -// msg.input_joint_command.joint_names.begin(), msg.input_joint_command.joint_names.end(), -// joint_names_.begin(), joint_names_.end())); -// ASSERT_EQ(msg.input_joint_command.points.size(), 1u); -// ASSERT_TRUE(std::equal(msg.input_joint_command.points[0].positions.begin(), msg.input_joint_command.points[0].positions.end(), joint_state_values_.begin(), joint_state_values_.end())); -// -// ASSERT_TRUE(std::find_if_not(msg.input_joint_command.points[0].velocities.begin(), msg.input_joint_command.points[0].velocities.end(), -// [](const auto & value){ return value == 0.0;}) == msg.input_joint_command.points[0].velocities.end()); -// -// // Check messages filled from AdmittanceRule.cpp -// ASSERT_EQ(msg.measured_wrench.header.frame_id, sensor_frame_); -// ASSERT_EQ(msg.measured_wrench.wrench.force.x, fts_state_values_[0]); -// ASSERT_EQ(msg.measured_wrench.wrench.force.y, fts_state_values_[1]); -// ASSERT_EQ(msg.measured_wrench.wrench.force.z, fts_state_values_[2]); -// ASSERT_EQ(msg.measured_wrench.wrench.torque.x, fts_state_values_[3]); -// ASSERT_EQ(msg.measured_wrench.wrench.torque.y, fts_state_values_[4]); -// ASSERT_EQ(msg.measured_wrench.wrench.torque.z, fts_state_values_[5]); -// -// ASSERT_EQ(msg.measured_wrench_control_frame.header.frame_id, control_frame_); -// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.x, 0.0); -// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.y, 0.0); -// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.force.z, 0.0); -// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.x, 0.0); -// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.y, 0.0); -// ASSERT_EQ(msg.measured_wrench_control_frame.wrench.torque.z, 0.0); -// -// ASSERT_EQ(msg.desired_pose.header.frame_id, control_frame_); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.x)); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.y)); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.position.z)); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.x)); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.y)); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.z)); -// ASSERT_FALSE(std::isnan(msg.desired_pose.pose.orientation.w)); -// -// ASSERT_EQ(msg.relative_desired_pose.header.frame_id, control_frame_); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.x)); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.y)); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.translation.z)); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.x)); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.y)); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.z)); -// ASSERT_FALSE(std::isnan(msg.relative_desired_pose.transform.rotation.w)); -// -// // Check joint related messages -// ASSERT_TRUE(std::equal( -// msg.joint_names.begin(), msg.joint_names.end(), joint_names_.begin(), joint_names_.end())); -// -// ASSERT_TRUE(std::equal( -// msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), -// joint_state_values_.begin(), joint_state_values_.end())); -// -// ASSERT_TRUE(std::equal( -// msg.actual_joint_state.positions.begin(), msg.actual_joint_state.positions.end(), -// joint_state_values_.begin(), joint_state_values_.end())); -// -// ASSERT_TRUE(std::equal( -// msg.desired_joint_state.positions.begin(), msg.desired_joint_state.positions.end(), -// joint_state_values_.begin(), joint_state_values_.end())); -// -// ASSERT_TRUE(std::find_if_not(msg.error_joint_state.positions.begin(), msg.error_joint_state.positions.end(), -// [](const auto & value){ return value == 0.0;}) == msg.error_joint_state.positions.end()); -//} +TEST_F(AdmittanceControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); + ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // Check that wrench command are all zero since not used + ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); + ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); + ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); + ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); + ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); + ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + + // Check joint command message + for (auto i = 0ul; i < joint_names_.size(); i++){ + ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); + ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); + ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); + ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); + } + +} TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) { @@ -337,8 +270,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.wrench_base.header.frame_id, control_frame_); - ASSERT_EQ(msg.wrench_base.header.frame_id, control_frame_); + ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); From d850b47b24c673777919b7ae19a88edcfe5dab37 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 10 Aug 2022 16:31:22 -0600 Subject: [PATCH 061/111] update README --- admittance_controller/CMakeLists.txt | 87 ++++------- admittance_controller/README.md | 146 +++++++++++++++++- .../admittance_controller.hpp | 2 +- .../src/admittance_controller.cpp | 2 +- .../src/admittance_controller_parameters.yaml | 27 +--- 5 files changed, 175 insertions(+), 89 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 760c5f3da3..20e47860d6 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -10,29 +10,36 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif () # find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + backward_ros + control_msgs + control_toolbox + controller_interface + kinematics_interface + Eigen3 + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + trajectory_msgs + angles + rcutils + tf2_kdl + generate_parameter_library + ) + find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(control_msgs REQUIRED) -find_package(control_toolbox REQUIRED) -find_package(controller_interface REQUIRED) -find_package(kinematics_interface REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(joint_trajectory_controller REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(trajectory_msgs REQUIRED) -find_package(angles REQUIRED) -find_package(rcutils REQUIRED) -find_package(tf2_kdl REQUIRED) -find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() # The admittance controller @@ -55,25 +62,7 @@ target_link_libraries(admittance_controller admittance_controller_parameters) ament_target_dependencies( admittance_controller - backward_ros - control_msgs - control_toolbox - controller_interface - kinematics_interface - Eigen3 - geometry_msgs - hardware_interface - joint_trajectory_controller - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_eigen - tf2_kdl - tf2_geometry_msgs - tf2_ros - angles + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -151,21 +140,7 @@ ament_export_libraries( ) ament_export_dependencies( - backward_ros - control_msgs - control_toolbox - controller_interface - kinematics_interface - geometry_msgs - hardware_interface - joint_trajectory_controller - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_eigen - tf2_geometry_msgs tf2_ros + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() diff --git a/admittance_controller/README.md b/admittance_controller/README.md index 33838bbc9e..1aa49c0e7e 100644 --- a/admittance_controller/README.md +++ b/admittance_controller/README.md @@ -1,12 +1,146 @@ admittance_controller ========================================== -Implementation of admittance controllers for different input and output interface. +The admittance controller operates in one of two modes: chained and non-chained. +In the non-chained mode, the controller listens to joint commands on the `~/joint_commands` topic to get a reference. In chained mode, the admittance controller receives its reference from another controller. In both modes, the controller applies the calculated admittance values to the received reference and write the modified reference to its command interfaces. -Pluginlib-Library: admittance_controller -Plugin: admittance_controller/AdmittanceController (controller_interface::ControllerInterface) +The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. -Requires an external differential IK plugin. One possibility is at: +## Parameters: +admittance.damping_ratio +``` +Type: double array +Description: specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S )) +``` -https://github.com/PickNikRobotics/moveit_differential_ik_plugin -# admittance_controller +admittance.mass +``` +Type: double array +Description: specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation +``` + +admittance.selected_axes +``` +Type: double array +Description: specifies if the axes x, y, z, rx, ry, and rz are enabled +``` + +admittance.stiffness +``` +Type: double array +Description: specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation +``` + +chainable_command_interfaces +``` +Type: string array +Description: specifies which chainable interfaces to claim +``` + +command_interfaces +``` +Type: string array +Description: specifies which command interfaces to claim +``` + +control.frame.id +``` +Type: string +Description: control frame used for admittance control +``` + +enable_parameter_update_without_reactivation +``` +Type: boolean +Description: if enabled, parameters will be dynamically updated in the control loop +``` + +fixed_world_frame.frame.id +``` +Type: string +Description: world frame, gravity points down (neg. Z) in this frame +``` + +ft_sensor.filter_coefficient +``` +Type: double +Description: specifies the coefficient for the sensor's exponential filter +``` + +ft_sensor.frame.id +``` +Type: string +Description: frame of the force torque senso +``` + +ft_sensor.name +``` +Type: string +Description: name of the force torque sensor in the robot description +``` + +gravity_compensation.CoG.force +``` +Type: double +Description: weight of the end effector, e.g mass * 9.81 +``` + +gravity_compensation.CoG.pos +``` +Type: double array +Description: position of the center of gravity (CoG) in its frame +``` + +gravity_compensation.frame.id +``` +Type: string +Description: rame which center of gravity (CoG) is defined in +``` + +joints +``` +Type: string array +Description: specifies which joints will be used by the controller +``` + +kinematics.alpha +``` +Type: double +Description: specifies the damping coefficient for the Jacobian pseudo inverse +``` + +kinematics.base +``` +Type: string +Description: specifies the base link of the robot description used by the kinematics plugin +``` + +kinematics.plugin_name +``` +Type: string +Description: specifies which kinematics plugin to load +``` + +kinematics.plugin_package +``` +Type: string +Description: specifies the package to load the kinematics plugin from +``` + +kinematics.tip +``` +Type: string +Description: specifies the end effector link of the robot description used by the kinematics plugin +``` + +robot_description +``` +Type: string +Description: Contains robot description in URDF format. The description is used to perform forward and inverse kinematics. +``` + +state_interfaces +``` +Type: string array +Description: specifies which state interfaces to claim +``` diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 5a658cc76b..9bc8dc1a6c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -47,7 +47,7 @@ using namespace std::chrono_literals; namespace admittance_controller { using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturn = controller_interface::CallbackReturn; class AdmittanceController : public controller_interface::ChainableControllerInterface { public: diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 00a393adfd..6bb0eaa294 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -30,7 +30,7 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller { - using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturn = controller_interface::CallbackReturn; CallbackReturn AdmittanceController::on_init() { // initialize controller config diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 31a74b8565..45106f24f0 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -4,6 +4,7 @@ admittance_controller: description: "specifies which joints will be used by the controller", read_only: true } + command_interfaces: { type: string_array, @@ -47,6 +48,7 @@ admittance_controller: default_value: 0.0005, description: "specifies the damping coefficient for the Jacobian pseudo inverse" } + ft_sensor: name: { type: string, @@ -57,11 +59,6 @@ admittance_controller: type: string, description: "frame of the force torque sensor" } - external: { - type: bool, - default_value: false, - description: "specifies if the force torque sensor is contained in the kinematics chain from the base to the tip" - } filter_coefficient: { type: double, default_value: 0.005, @@ -74,11 +71,6 @@ admittance_controller: type: string, description: "control frame used for admittance control" } - external: { - type: bool, - default_value: false, - description: "specifies if the control frame is contained in the kinematics chain from the base to the tip" - } fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) frame: @@ -86,11 +78,6 @@ admittance_controller: type: string, description: "world frame, gravity points down (neg. Z) in this frame" } - external: { - type: bool, - default_value: false, - description: "specifies if the world frame is contained in the kinematics chain from the base to the tip" - } gravity_compensation: frame: @@ -98,11 +85,6 @@ admittance_controller: type: string, description: "frame which center of gravity (CoG) is defined in" } - external: { - type: bool, - default_value: false, - description: "specifies if the center of gravity frame is contained in the kinematics chain from the base to the tip" - } CoG: # specifies the center of gravity of the end effector pos: { type: double_array, @@ -126,9 +108,6 @@ admittance_controller: fixed_size<>: 6 } } - - # Having ".0" at the end is MUST, otherwise there is a loading error - # F = M*a + D*v + S*(x - x_d) mass: { type: double_array, description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation", @@ -137,7 +116,6 @@ admittance_controller: element_bounds<>: [ 0.0001, 1000000.0 ] } } - damping_ratio: { type: double_array, description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. @@ -146,7 +124,6 @@ admittance_controller: fixed_size<>: 6 } } - stiffness: { type: double_array, description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", From 51e85c6d19f3791d8bd7d20c4c9ddfce8aeb8be6 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 10 Aug 2022 17:01:13 -0600 Subject: [PATCH 062/111] update variable names --- .../admittance_controller.hpp | 34 +++--- .../admittance_controller/admittance_rule.hpp | 2 +- .../admittance_rule_impl.hpp | 8 +- .../src/admittance_controller.cpp | 100 +++++++++--------- 4 files changed, 68 insertions(+), 76 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9bc8dc1a6c..8fd8974c54 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -39,22 +39,17 @@ #include "rclcpp/time.hpp" #include "rclcpp/duration.hpp" -// TODO(destogl): this is only temporary to work with servo. It should be either trajectory_msgs/msg/JointTrajectoryPoint or std_msgs/msg/Float64MultiArray #include "trajectory_msgs/msg/joint_trajectory.hpp" - -using namespace std::chrono_literals; - namespace admittance_controller { using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; - using CallbackReturn = controller_interface::CallbackReturn; class AdmittanceController : public controller_interface::ChainableControllerInterface { public: ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_init() override; + controller_interface::CallbackReturn on_init() override; ADMITTANCE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -63,19 +58,19 @@ namespace admittance_controller { controller_interface::InterfaceConfiguration state_interface_configuration() const override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override; + controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC controller_interface::return_type update_and_write_commands( @@ -113,29 +108,28 @@ namespace admittance_controller { rclcpp::Publisher::SharedPtr s_publisher_; // ROS messages - std::shared_ptr joint_command_msg; + std::shared_ptr joint_command_msg_; // real-time buffer realtime_tools::RealtimeBuffer> input_joint_command_; std::unique_ptr> state_publisher_; - trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; - trajectory_msgs::msg::JointTrajectoryPoint last_state_reference_; - trajectory_msgs::msg::JointTrajectoryPoint state_offset_; - trajectory_msgs::msg::JointTrajectoryPoint prev_trajectory_point_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; + trajectory_msgs::msg::JointTrajectoryPoint last_reference_; // control loop data - trajectory_msgs::msg::JointTrajectoryPoint state_reference_, state_current_, state_desired_, state_error_; + // reference_: reference value read by the controller + // joint_state_: current joint readings from the hardware + // reference_admittance_: reference value used by the controller after the admittance values are applied + // ft_values_: values read from the force torque sensor + trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; geometry_msgs::msg::Wrench ft_values_; - trajectory_msgs::msg::JointTrajectory pre_admittance_point; - size_t loop_counter = 0; // helper methods void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state_current, geometry_msgs::msg::Wrench &ft_values); void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_commanded); - }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 6e2fdca3a5..b1347907e8 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -199,7 +199,7 @@ namespace admittance_controller { Eigen::Vector3d cog_; // force applied to sensor due to weight of end effector - Eigen::Vector3d ee_weight; + Eigen::Vector3d ee_weight_; // ROS control_msgs::msg::AdmittanceControllerState state_message_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 2c9b92462c..c26f1540c7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -89,7 +89,7 @@ namespace admittance_controller { // reset forces wrench_world_.setZero(); - ee_weight.setZero(); + ee_weight_.setZero(); // load/initialize Eigen types from parameters apply_parameters_update(); @@ -102,7 +102,7 @@ namespace admittance_controller { parameters_ = parameter_handler_->get_params(); } // update param values - ee_weight[2] = -parameters_.gravity_compensation.CoG.force; + ee_weight_[2] = -parameters_.gravity_compensation.CoG.force; vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_); vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); @@ -281,8 +281,8 @@ namespace admittance_controller { Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; // apply gravity compensation - new_wrench_base(2, 0) -= ee_weight[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight); + new_wrench_base(2, 0) -= ee_weight_[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight_); // apply smoothing filter for (auto i = 0; i < 6; i++) { diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6bb0eaa294..e992ef8c0a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -30,28 +30,27 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller { - using CallbackReturn = controller_interface::CallbackReturn; - CallbackReturn AdmittanceController::on_init() { + controller_interface::CallbackReturn AdmittanceController::on_init() { // initialize controller config try { admittance_ = std::make_unique(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 CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } num_joints_ = admittance_->parameters_.joints.size(); // allocate dynamic memory - last_state_reference_.positions.assign(num_joints_, 0.0); - last_state_reference_.velocities.assign(num_joints_, 0.0); - last_state_reference_.accelerations.assign(num_joints_, 0.0); - last_commanded_state_ = last_state_reference_; - state_reference_ = last_state_reference_; - state_desired_ = last_state_reference_; - state_current_ = last_state_reference_; - - return CallbackReturn::SUCCESS; + last_reference_.positions.assign(num_joints_, 0.0); + last_reference_.velocities.assign(num_joints_, 0.0); + last_reference_.accelerations.assign(num_joints_, 0.0); + last_commanded_ = last_reference_; + reference_ = last_reference_; + reference_admittance_ = last_reference_; + joint_state_ = last_reference_; + + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() const { @@ -138,13 +137,13 @@ namespace admittance_controller { return chainable_command_interfaces;; } - CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { + controller_interface::CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { try { admittance_ = std::make_unique(get_node()); num_joints_ = admittance_->parameters_.joints.size(); } catch (const std::exception &e) { RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // print and validate interface types for (const auto &tmp: admittance_->parameters_.state_interfaces) { @@ -160,7 +159,7 @@ namespace admittance_controller { RCLCPP_ERROR(get_node()->get_logger(), "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } } @@ -201,16 +200,16 @@ namespace admittance_controller { // configure admittance rule if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } return LifecycleNodeInterface::on_configure(previous_state); } - CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { + controller_interface::CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { // on_activate is called when the lifecycle node activates. if (!admittance_) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } // update parameters if any have changed @@ -247,20 +246,20 @@ namespace admittance_controller { force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); // handle state after restart or initial startups - read_state_from_hardware(state_current_, ft_values_); + read_state_from_hardware(joint_state_, ft_values_); // initialize states from last read state reference - last_commanded_state_ = state_current_; - last_state_reference_ = state_current_; + last_commanded_ = joint_state_; + last_reference_ = joint_state_; - read_state_reference_interfaces(state_reference_); + read_state_reference_interfaces(reference_); // reset dynamic fields in case non-zero - state_reference_.velocities.assign(num_joints_, 0.0); - state_reference_.accelerations.assign(num_joints_, 0.0); - state_desired_ = state_reference_; + reference_.velocities.assign(num_joints_, 0.0); + reference_.accelerations.assign(num_joints_, 0.0); + reference_admittance_ = reference_; - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { @@ -269,15 +268,15 @@ namespace admittance_controller { return controller_interface::return_type::ERROR; } - joint_command_msg = *input_joint_command_.readFromRT(); + joint_command_msg_ = *input_joint_command_.readFromRT(); // if message exists, load values into references - if (joint_command_msg.get()) { - for (auto i = 0ul; i < joint_command_msg->positions.size(); i++) { - position_reference_[i].get() = joint_command_msg->positions[i]; + if (joint_command_msg_.get()) { + for (auto i = 0ul; i < joint_command_msg_->positions.size(); i++) { + position_reference_[i].get() = joint_command_msg_->positions[i]; } - for (auto i = 0ul; i < joint_command_msg->velocities.size(); i++) { - velocity_reference_[i].get() = joint_command_msg->velocities[i]; + for (auto i = 0ul; i < joint_command_msg_->velocities.size(); i++) { + velocity_reference_[i].get() = joint_command_msg_->velocities[i]; } } @@ -290,19 +289,18 @@ namespace admittance_controller { if (!admittance_) { return controller_interface::return_type::ERROR; } - loop_counter++; // update input reference from chainable interfaces - read_state_reference_interfaces(state_reference_); + read_state_reference_interfaces(reference_); // get all controller inputs - read_state_from_hardware(state_current_, ft_values_); + read_state_from_hardware(joint_state_, ft_values_); // apply admittance control to reference to determine desired state - admittance_->update(state_current_, ft_values_, state_reference_, period, state_desired_); + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); // write calculated values to joint interfaces - write_state_to_hardware(state_desired_); + write_state_to_hardware(reference_admittance_); // Publish controller state state_publisher_->lock(); @@ -312,22 +310,22 @@ namespace admittance_controller { return controller_interface::return_type::OK; } - CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { + controller_interface::CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { force_torque_sensor_->release_interfaces(); return LifecycleNodeInterface::on_deactivate(previous_state); } - CallbackReturn AdmittanceController::on_cleanup(const rclcpp_lifecycle::State &previous_state) { - return CallbackReturn::SUCCESS; + controller_interface::CallbackReturn AdmittanceController::on_cleanup(const rclcpp_lifecycle::State &previous_state) { + return controller_interface::CallbackReturn::SUCCESS; } - CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { + controller_interface::CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { if (!admittance_) { - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } admittance_->reset(num_joints_); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } void AdmittanceController::read_state_from_hardware( @@ -343,19 +341,19 @@ namespace admittance_controller { if (inter_ind == state_pos_ind) { state_current.positions[joint_ind] = state_interfaces_[ind].get_value(); if (std::isnan(state_current.positions[joint_ind])) { - state_current.positions = last_commanded_state_.positions; + state_current.positions = last_commanded_.positions; break; } } else if (inter_ind == state_vel_ind) { state_current.velocities[joint_ind] = state_interfaces_[ind].get_value(); if (std::isnan(state_current.positions[joint_ind])) { - state_current.velocities = last_commanded_state_.velocities; + state_current.velocities = last_commanded_.velocities; break; } } else if (inter_ind == state_acc_ind) { state_current.accelerations[joint_ind] = state_interfaces_[ind].get_value(); if (std::isnan(state_current.positions[joint_ind])) { - state_current.accelerations = last_commanded_state_.accelerations; + state_current.accelerations = last_commanded_.accelerations; break; } } @@ -387,7 +385,7 @@ namespace admittance_controller { } } } - last_commanded_state_ = state_desired_; + last_commanded_ = reference_admittance_; } @@ -398,19 +396,19 @@ namespace admittance_controller { for (auto i = 0ul; i < position_reference_.size(); i++) { if (std::isnan(position_reference_[i])) { - position_reference_[i].get() = last_state_reference_.positions[i]; + position_reference_[i].get() = last_reference_.positions[i]; } state_reference.positions[i] = position_reference_[i]; } - last_state_reference_.positions = state_reference.positions; + last_reference_.positions = state_reference.positions; for (auto i = 0ul; i < velocity_reference_.size(); i++) { if (std::isnan(velocity_reference_[i])) { - velocity_reference_[i].get() = last_state_reference_.velocities[i]; + velocity_reference_[i].get() = last_reference_.velocities[i]; } state_reference.velocities[i] = velocity_reference_[i]; } - last_state_reference_.velocities = state_reference.velocities; + last_reference_.velocities = state_reference.velocities; } From 15413184731a986e5557a2ba1e3513718cfba1df Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 11 Aug 2022 14:49:00 -0600 Subject: [PATCH 063/111] update kinematics interface use --- .../include/admittance_controller/admittance_rule.hpp | 6 +++--- .../include/admittance_controller/admittance_rule_impl.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index b1347907e8..100a736450 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -33,7 +33,7 @@ #include "admittance_controller_parameters.hpp" // kinematics plugins -#include "kinematics_interface/kinematics_interface_base.hpp" +#include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" @@ -182,8 +182,8 @@ namespace admittance_controller { size_t num_joints_; // Kinematics interface plugin loader - std::shared_ptr> kinematics_loader_; - std::unique_ptr kinematics_; + std::shared_ptr> kinematics_loader_; + std::unique_ptr kinematics_; // filtered wrench in world frame Eigen::Matrix wrench_world_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index c26f1540c7..66d0501867 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -42,9 +42,9 @@ namespace admittance_controller { // Load the differential IK plugin if (!parameters_.kinematics.plugin_name.empty()) { try { - kinematics_loader_ = std::make_shared>( - parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterfaceBase"); - kinematics_ = std::unique_ptr( + kinematics_loader_ = std::make_shared>( + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); + kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize(node->get_node_parameters_interface(), parameters_.kinematics.tip)) { return controller_interface::return_type::ERROR; From 932a554c3f7a2c6b2715b07e7f395e802def1897 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 20 Aug 2022 16:15:38 +0200 Subject: [PATCH 064/111] Make tricycle controller the same as master. --- .pre-commit-config.yaml | 2 +- .../tricycle_controller/steering_limiter.hpp | 8 ++------ .../tricycle_controller/traction_limiter.hpp | 6 ++---- .../tricycle_controller/tricycle_controller.hpp | 5 +++-- tricycle_controller/src/steering_limiter.cpp | 4 ++-- tricycle_controller/src/traction_limiter.cpp | 6 +++--- tricycle_controller/src/tricycle_controller.cpp | 15 +++++++++------ .../test/test_load_tricycle_controller.cpp | 4 ++-- 8 files changed, 24 insertions(+), 26 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa8d75100e..58fe46dd90 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -139,5 +139,5 @@ repos: rev: v2.1.0 hooks: - id: codespell - args: ['--write-changes'] + args: ['--write-changes', '--uri-ignore-words-list=ist'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp index 51ecac4cbd..a16f0de0f0 100644 --- a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -23,7 +23,6 @@ namespace tricycle_controller { - class SteeringLimiter { public: @@ -37,9 +36,8 @@ class SteeringLimiter * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] */ SteeringLimiter( - double min_position = NAN, double max_position = NAN, - double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN); + double min_position = NAN, double max_position = NAN, double min_velocity = NAN, + double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN); /** * \brief Limit the position, velocity and acceleration @@ -78,7 +76,6 @@ class SteeringLimiter double limit_acceleration(double & p, double p0, double p1, double dt); private: - // Position limits: double min_position_; double max_position_; @@ -90,7 +87,6 @@ class SteeringLimiter // Acceleration limits: double min_acceleration_; double max_acceleration_; - }; } // namespace tricycle_controller diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index 4c0f297ee8..ea0bb16025 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -23,7 +23,6 @@ namespace tricycle_controller { - class TractionLimiter { public: @@ -39,9 +38,8 @@ class TractionLimiter * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ TractionLimiter( - double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN, - double min_deceleration = NAN, double max_deceleration = NAN, + double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, + double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, double min_jerk = NAN, double max_jerk = NAN); /** diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 959dcc9e69..2178008725 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include "ackermann_msgs/msg/ackermann_drive.hpp" @@ -32,16 +33,16 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "std_srvs/srv/empty.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp" -#include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/steering_limiter.hpp" +#include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" namespace tricycle_controller diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index 6912144bd0..c1649f12dc 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -87,7 +87,7 @@ double SteeringLimiter::limit_velocity(double & p, double p0, double dt) const double dv_min = min_velocity_ * dt; const double dv_max = max_velocity_ * dt; - double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); + double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); dv *= (p - p0 >= 0 ? 1 : -1); p = p0 + dv; @@ -106,7 +106,7 @@ double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, dou const double da_min = min_acceleration_ * dt2; const double da_max = max_acceleration_ * dt2; - double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); + double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); da *= (dv - dp0 >= 0 ? 1 : -1); p = p0 + dp0 + da; diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index c2a0d88ab4..7865d4be71 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -91,7 +91,7 @@ double TractionLimiter::limit_velocity(double & v) { const double tmp = v; - v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); + v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; @@ -113,7 +113,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) dv_min = min_deceleration_ * dt; dv_max = max_deceleration_ * dt; } - double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); + double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); dv *= (v - v0 >= 0 ? 1 : -1); v = v0 + dv; @@ -132,7 +132,7 @@ double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); + double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); da *= (dv - dv0 >= 0 ? 1 : -1); v = v0 + dv0 + da; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index bfae724aef..114731242c 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -221,7 +221,7 @@ controller_interface::return_type TricycleController::update( } else { - // TODO: find the best function, e.g convex power functions + // TODO(anyone): find the best function, e.g convex power functions scale = cos(alpha_delta); } Ws_write *= scale; @@ -238,8 +238,9 @@ controller_interface::return_type TricycleController::update( previous_commands_.pop(); AckermannDrive ackermann_command; - ackermann_command.speed = - Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel + // speed (rad/s) + ackermann_command.speed = Ws_write; ackermann_command.steering_angle = alpha_write; previous_commands_.emplace(ackermann_command); @@ -247,8 +248,9 @@ controller_interface::return_type TricycleController::update( if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; - realtime_ackermann_command.speed = - Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel + // speed (rad/s) + realtime_ackermann_command.speed = Ws_write; realtime_ackermann_command.steering_angle = alpha_write; realtime_ackermann_command_publisher_->unlockAndPublish(); } @@ -298,7 +300,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - cmd_vel_timeout_ = std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + cmd_vel_timeout_ = + std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 088cddbabe..d04b83ae27 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -36,8 +36,8 @@ TEST(TestLoadTricycleController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_tricycle_controller", "tricycle_controller/TricycleController")); + ASSERT_NO_THROW( + cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController")); rclcpp::shutdown(); } From a15728e29e1e53ed06c974e842fed14403f7828a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 20 Aug 2022 16:23:03 +0200 Subject: [PATCH 065/111] Revert "Make tricycle controller the same as master." This reverts commit 932a554c3f7a2c6b2715b07e7f395e802def1897. --- .pre-commit-config.yaml | 2 +- .../tricycle_controller/steering_limiter.hpp | 8 ++++++-- .../tricycle_controller/traction_limiter.hpp | 6 ++++-- .../tricycle_controller/tricycle_controller.hpp | 5 ++--- tricycle_controller/src/steering_limiter.cpp | 4 ++-- tricycle_controller/src/traction_limiter.cpp | 6 +++--- tricycle_controller/src/tricycle_controller.cpp | 15 ++++++--------- .../test/test_load_tricycle_controller.cpp | 4 ++-- 8 files changed, 26 insertions(+), 24 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..fa8d75100e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -139,5 +139,5 @@ repos: rev: v2.1.0 hooks: - id: codespell - args: ['--write-changes', '--uri-ignore-words-list=ist'] + args: ['--write-changes'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp index a16f0de0f0..51ecac4cbd 100644 --- a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -23,6 +23,7 @@ namespace tricycle_controller { + class SteeringLimiter { public: @@ -36,8 +37,9 @@ class SteeringLimiter * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] */ SteeringLimiter( - double min_position = NAN, double max_position = NAN, double min_velocity = NAN, - double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN); + double min_position = NAN, double max_position = NAN, + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN); /** * \brief Limit the position, velocity and acceleration @@ -76,6 +78,7 @@ class SteeringLimiter double limit_acceleration(double & p, double p0, double p1, double dt); private: + // Position limits: double min_position_; double max_position_; @@ -87,6 +90,7 @@ class SteeringLimiter // Acceleration limits: double min_acceleration_; double max_acceleration_; + }; } // namespace tricycle_controller diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index ea0bb16025..4c0f297ee8 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -23,6 +23,7 @@ namespace tricycle_controller { + class TractionLimiter { public: @@ -38,8 +39,9 @@ class TractionLimiter * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ TractionLimiter( - double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, - double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, + double min_deceleration = NAN, double max_deceleration = NAN, double min_jerk = NAN, double max_jerk = NAN); /** diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 2178008725..959dcc9e69 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include "ackermann_msgs/msg/ackermann_drive.hpp" @@ -33,16 +32,16 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "std_srvs/srv/empty.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp" -#include "tricycle_controller/steering_limiter.hpp" #include "tricycle_controller/traction_limiter.hpp" +#include "tricycle_controller/steering_limiter.hpp" #include "tricycle_controller/visibility_control.h" namespace tricycle_controller diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index c1649f12dc..6912144bd0 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -87,7 +87,7 @@ double SteeringLimiter::limit_velocity(double & p, double p0, double dt) const double dv_min = min_velocity_ * dt; const double dv_max = max_velocity_ * dt; - double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); + double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); dv *= (p - p0 >= 0 ? 1 : -1); p = p0 + dv; @@ -106,7 +106,7 @@ double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, dou const double da_min = min_acceleration_ * dt2; const double da_max = max_acceleration_ * dt2; - double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); + double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); da *= (dv - dp0 >= 0 ? 1 : -1); p = p0 + dp0 + da; diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index 7865d4be71..c2a0d88ab4 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -91,7 +91,7 @@ double TractionLimiter::limit_velocity(double & v) { const double tmp = v; - v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); + v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; @@ -113,7 +113,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) dv_min = min_deceleration_ * dt; dv_max = max_deceleration_ * dt; } - double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); + double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); dv *= (v - v0 >= 0 ? 1 : -1); v = v0 + dv; @@ -132,7 +132,7 @@ double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); + double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); da *= (dv - dv0 >= 0 ? 1 : -1); v = v0 + dv0 + da; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 114731242c..bfae724aef 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -221,7 +221,7 @@ controller_interface::return_type TricycleController::update( } else { - // TODO(anyone): find the best function, e.g convex power functions + // TODO: find the best function, e.g convex power functions scale = cos(alpha_delta); } Ws_write *= scale; @@ -238,9 +238,8 @@ controller_interface::return_type TricycleController::update( previous_commands_.pop(); AckermannDrive ackermann_command; - // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel - // speed (rad/s) - ackermann_command.speed = Ws_write; + ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) ackermann_command.steering_angle = alpha_write; previous_commands_.emplace(ackermann_command); @@ -248,9 +247,8 @@ controller_interface::return_type TricycleController::update( if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; - // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel - // speed (rad/s) - realtime_ackermann_command.speed = Ws_write; + realtime_ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) realtime_ackermann_command.steering_angle = alpha_write; realtime_ackermann_command_publisher_->unlockAndPublish(); } @@ -300,8 +298,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - cmd_vel_timeout_ = - std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + cmd_vel_timeout_ = std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index d04b83ae27..088cddbabe 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -36,8 +36,8 @@ TEST(TestLoadTricycleController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController")); + ASSERT_NO_THROW(cm.load_controller( + "test_tricycle_controller", "tricycle_controller/TricycleController")); rclcpp::shutdown(); } From ccec03de00b590b7d3dae6235e0ceae1e6212ce7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 21 Aug 2022 17:56:26 +0200 Subject: [PATCH 066/111] Correct formatting and linting issues. --- admittance_controller/CMakeLists.txt | 208 +- admittance_controller/README.md | 6 +- .../admittance_controller.hpp | 201 +- .../admittance_controller/admittance_rule.hpp | 268 ++- .../admittance_rule_impl.hpp | 714 +++--- .../src/admittance_controller.cpp | 743 +++--- .../test/test_admittance_controller.cpp | 177 +- .../test/test_admittance_controller.hpp | 186 +- .../test/test_load_admittance_controller.cpp | 15 +- .../joint_trajectory_controller.hpp | 404 ++-- .../src/joint_trajectory_controller.cpp | 2111 +++++++++-------- 11 files changed, 2594 insertions(+), 2439 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 20e47860d6..09279f7509 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -1,146 +1,120 @@ cmake_minimum_required(VERSION 3.5) project(admittance_controller) -if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif () +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() -if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif () +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS - ament_cmake - backward_ros - control_msgs - control_toolbox - controller_interface - kinematics_interface - Eigen3 - geometry_msgs - hardware_interface - joint_trajectory_controller - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - trajectory_msgs - angles - rcutils - tf2_kdl - generate_parameter_library - ) + angles + control_msgs + control_toolbox + controller_interface + kinematics_interface + Eigen3 + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs +) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) + find_package(${Dependency} REQUIRED) endforeach() # The admittance controller -add_library(admittance_controller SHARED - src/admittance_controller.cpp - ) - -target_include_directories( - admittance_controller - PRIVATE - include -) - -generate_parameter_library(admittance_controller_parameters - src/admittance_controller_parameters.yaml - ) - -target_link_libraries(admittance_controller admittance_controller_parameters) - - -ament_target_dependencies( - admittance_controller - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - +add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) +target_include_directories(${PROJECT_NAME} PRIVATE include) +generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) +target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) - -install( - TARGETS admittance_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib +install(DIRECTORY include/ + DESTINATION include ) -install( - DIRECTORY include/ - DESTINATION include +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) -if (BUILD_TESTING) - # create custom test function to pass yaml file into test main - function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) - add_executable(${TARGET} ${SOURCES}) - _ament_cmake_gmock_find_gmock() - target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") - target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) - set(executable "$") - set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") - ament_add_test( - ${TARGET} - COMMAND ${executable} --ros-args --params-file ${YAML_FILE} - --gtest_output=xml:${result_file} - OUTPUT_FILE ${CMAKE_BINARY_DIR}/ament_cmake_gmock/test_load_admittance_controller.txt - RESULT_FILE ${result_file} - ) - endfunction() - # find testing packages - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - # test loading admittance controller - add_test_with_yaml_input(test_load_admittance_controller "test/test_load_admittance_controller.cpp" - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_load_admittance_controller PUBLIC "${GMOCK_INCLUDE_DIRS}") - target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) - ament_target_dependencies( - test_load_admittance_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) - # test admittance controller function - add_test_with_yaml_input(test_admittance_controller "test/test_admittance_controller.cpp" - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_admittance_controller PRIVATE include) - target_link_libraries(test_admittance_controller admittance_controller) - ament_target_dependencies( - test_admittance_controller - control_msgs - controller_interface - hardware_interface - ros2_control_test_assets +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + # create custom test function to pass yaml file into test main + function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) + add_executable(${TARGET} ${SOURCES}) + _ament_cmake_gmock_find_gmock() + target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") + target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) + set(executable "$") + set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") + ament_add_test( + ${TARGET} + COMMAND ${executable} --ros-args --params-file ${YAML_FILE} + --gtest_output=xml:${result_file} + OUTPUT_FILE ${CMAKE_BINARY_DIR}/ament_cmake_gmock/test_load_admittance_controller.txt + RESULT_FILE ${result_file} ) + endfunction() + + # test loading admittance controller + add_test_with_yaml_input(test_load_admittance_controller "test/test_load_admittance_controller.cpp" + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_load_admittance_controller PUBLIC "${GMOCK_INCLUDE_DIRS}") + target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) + ament_target_dependencies( + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + # test admittance controller function + add_test_with_yaml_input(test_admittance_controller "test/test_admittance_controller.cpp" + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_admittance_controller PRIVATE include) + target_link_libraries(test_admittance_controller admittance_controller) + ament_target_dependencies( + test_admittance_controller + control_msgs + controller_interface + hardware_interface + ros2_control_test_assets + ) +endif() -endif () - +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) ament_export_include_directories( - include + include ) - ament_export_libraries( - admittance_controller + ${PROJECT_NAME} ) - -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - ament_package() diff --git a/admittance_controller/README.md b/admittance_controller/README.md index 1aa49c0e7e..f8f35f8b4c 100644 --- a/admittance_controller/README.md +++ b/admittance_controller/README.md @@ -1,8 +1,8 @@ admittance_controller ========================================== -The admittance controller operates in one of two modes: chained and non-chained. -In the non-chained mode, the controller listens to joint commands on the `~/joint_commands` topic to get a reference. In chained mode, the admittance controller receives its reference from another controller. In both modes, the controller applies the calculated admittance values to the received reference and write the modified reference to its command interfaces. +The admittance controller operates in one of two modes: chained and non-chained. +In the non-chained mode, the controller listens to joint commands on the `~/joint_commands` topic to get a reference. In chained mode, the admittance controller receives its reference from another controller. In both modes, the controller applies the calculated admittance values to the received reference and write the modified reference to its command interfaces. The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. @@ -63,7 +63,7 @@ Description: world frame, gravity points down (neg. Z) in this frame ft_sensor.filter_coefficient ``` -Type: double +Type: double Description: specifies the coefficient for the sensor's exponential filter ``` diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 8fd8974c54..3b5dfff95b 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -31,106 +31,123 @@ #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" -namespace admittance_controller { - using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; - - class AdmittanceController : public controller_interface::ChainableControllerInterface { - public: - ADMITTANCE_CONTROLLER_PUBLIC - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state) override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state) override; - - ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time &time, const rclcpp::Duration &period) override; - - - protected: - std::vector on_export_reference_interfaces() override; - - controller_interface::return_type update_reference_from_subscribers() override; - - bool on_set_chained_mode(bool chained_mode) override; - - size_t num_joints_ = 0; - size_t state_pos_ind = -1; - size_t state_vel_ind = -1; - size_t state_acc_ind = -1; - size_t command_pos_ind = -1; - size_t command_vel_ind = -1; - size_t command_acc_ind = -1; - - // internal reference values - const std::vector reference_interfaces_types_ = {"position", "velocity"}; - std::vector> position_reference_; - std::vector> velocity_reference_; - - // Admittance rule and dependent variables; - std::unique_ptr admittance_; - - // force torque sensor - std::unique_ptr force_torque_sensor_; - - // ROS subscribers - rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; - rclcpp::Publisher::SharedPtr s_publisher_; - - // ROS messages - std::shared_ptr joint_command_msg_; - - // real-time buffer - realtime_tools::RealtimeBuffer> input_joint_command_; - std::unique_ptr> state_publisher_; - - trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; - trajectory_msgs::msg::JointTrajectoryPoint last_reference_; - - // control loop data - // reference_: reference value read by the controller - // joint_state_: current joint readings from the hardware - // reference_admittance_: reference value used by the controller after the admittance values are applied - // ft_values_: values read from the force torque sensor - trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; - geometry_msgs::msg::Wrench ft_values_; - - // helper methods - void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint &state_current, - geometry_msgs::msg::Wrench &ft_values); - void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint &state); - void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_commanded); - }; +namespace admittance_controller +{ +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +class AdmittanceController : public controller_interface::ChainableControllerInterface +{ +public: + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers() override; + + bool on_set_chained_mode(bool chained_mode) override; + + size_t num_joints_ = 0; + size_t state_pos_ind = -1; + size_t state_vel_ind = -1; + size_t state_acc_ind = -1; + size_t command_pos_ind = -1; + size_t command_vel_ind = -1; + size_t command_acc_ind = -1; + + // internal reference values + const std::vector reference_interfaces_types_ = {"position", "velocity"}; + std::vector> position_reference_; + std::vector> velocity_reference_; + + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + + // force torque sensor + std::unique_ptr force_torque_sensor_; + + // ROS subscribers + rclcpp::Subscription::SharedPtr + input_joint_command_subscriber_; + rclcpp::Publisher::SharedPtr s_publisher_; + + // ROS messages + std::shared_ptr joint_command_msg_; + + // real-time buffer + realtime_tools::RealtimeBuffer> + input_joint_command_; + std::unique_ptr> state_publisher_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; + trajectory_msgs::msg::JointTrajectoryPoint last_reference_; + + // control loop data + // reference_: reference value read by the controller + // joint_state_: current joint readings from the hardware + // reference_admittance_: reference value used by the controller after the admittance values are + // applied ft_values_: values read from the force torque sensor + trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; + geometry_msgs::msg::Wrench ft_values_; + + // helper methods + void read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values); + void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded); +}; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 100a736450..c9f0ab4a74 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -17,89 +17,98 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ -#include #include #include -#include "tf2_ros/transform_listener.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_eigen/tf2_eigen.h" -#include -#include -#include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" +#include +#include +#include +#include + +#include "tf2_ros/buffer.h" + +#include "admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" -#include "controller_interface/controller_interface.hpp" #include "control_toolbox/filters.hpp" +#include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "admittance_controller_parameters.hpp" - -// kinematics plugins #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" - - -namespace admittance_controller { - - struct Transforms { - Eigen::Isometry3d base_control_; - Eigen::Isometry3d base_ft_; - Eigen::Isometry3d base_tip_; - Eigen::Isometry3d world_base_; - Eigen::Isometry3d base_sensor_; - Eigen::Isometry3d base_cog_; - }; - - struct AdmittanceState { - AdmittanceState() = default; - - AdmittanceState(size_t num_joints) { - admittance_velocity.setZero(); - admittance_acceleration.setZero(); - damping.setZero(); - mass.setOnes(); - mass_inv.setZero(); - stiffness.setZero(); - selected_axes.setZero(); - current_joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_vel = Eigen::VectorXd::Zero(num_joints); - joint_acc = Eigen::VectorXd::Zero(num_joints); - } - - Eigen::VectorXd current_joint_pos; - Eigen::VectorXd joint_pos; - Eigen::VectorXd joint_vel; - Eigen::VectorXd joint_acc; - Eigen::Matrix damping; - Eigen::Matrix mass; - Eigen::Matrix mass_inv; - Eigen::Matrix selected_axes; - Eigen::Matrix stiffness; - Eigen::Matrix wrench_base; - Eigen::Matrix admittance_acceleration; - Eigen::Matrix admittance_velocity; - Eigen::Isometry3d admittance_position; - Eigen::Matrix rot_base_control; - Eigen::Isometry3d ref_trans_base_ft; - std::string ft_sensor_frame; - }; - - class AdmittanceRule { - public: - AdmittanceRule(const std::shared_ptr &lifecycl_node) { - parameter_handler_ = std::make_shared( - lifecycl_node->get_node_parameters_interface()); - parameters_ = parameter_handler_->get_params(); - num_joints_ = parameters_.joints.size(); - admittance_state_ = AdmittanceState(num_joints_); - reset(num_joints_); - } - - controller_interface::return_type - configure(const std::shared_ptr &node, size_t num_joint); - - controller_interface::return_type reset(size_t num_joints); - - /** +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" +#include "tf2_ros/transform_listener.h" +// TODO(destogl): remove this include - this is wrong! +#include "trajectory_msgs/msg/detail/joint_trajectory_point__struct.hpp" + +namespace admittance_controller +{ +struct Transforms +{ + Eigen::Isometry3d base_control_; + Eigen::Isometry3d base_ft_; + Eigen::Isometry3d base_tip_; + Eigen::Isometry3d world_base_; + Eigen::Isometry3d base_sensor_; + Eigen::Isometry3d base_cog_; +}; + +struct AdmittanceState +{ + // TODO(destogl): does this constructor makes sense at all? + AdmittanceState() = default; + + explicit AdmittanceState(size_t num_joints) + { + admittance_velocity.setZero(); + admittance_acceleration.setZero(); + damping.setZero(); + mass.setOnes(); + mass_inv.setZero(); + stiffness.setZero(); + selected_axes.setZero(); + current_joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_vel = Eigen::VectorXd::Zero(num_joints); + joint_acc = Eigen::VectorXd::Zero(num_joints); + } + + Eigen::VectorXd current_joint_pos; + Eigen::VectorXd joint_pos; + Eigen::VectorXd joint_vel; + Eigen::VectorXd joint_acc; + Eigen::Matrix damping; + Eigen::Matrix mass; + Eigen::Matrix mass_inv; + Eigen::Matrix selected_axes; + Eigen::Matrix stiffness; + Eigen::Matrix wrench_base; + Eigen::Matrix admittance_acceleration; + Eigen::Matrix admittance_velocity; + Eigen::Isometry3d admittance_position; + Eigen::Matrix rot_base_control; + Eigen::Isometry3d ref_trans_base_ft; + std::string ft_sensor_frame; +}; + +class AdmittanceRule +{ +public: + explicit AdmittanceRule(const std::shared_ptr & lifecycl_node) + { + parameter_handler_ = std::make_shared( + lifecycl_node->get_node_parameters_interface()); + parameters_ = parameter_handler_->get_params(); + num_joints_ = parameters_.joints.size(); + admittance_state_ = AdmittanceState(num_joints_); + reset(num_joints_); + } + + controller_interface::return_type configure( + const std::shared_ptr & node, size_t num_joint); + + controller_interface::return_type reset(size_t num_joints); + + /** * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation * are calculated without an error @@ -107,17 +116,18 @@ namespace admittance_controller { * \param[in] reference_joint_state * \param[in] success */ - bool get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, - const AdmittanceState &admittance_state); + bool get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const AdmittanceState & admittance_state); - /** + /** * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field * members (ee_weight_, cog_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated */ - void apply_parameters_update(); + void apply_parameters_update(); - /** + /** * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and * 'current_joint_state'. * @@ -127,26 +137,27 @@ namespace admittance_controller { * \param[in] period * \param[out] desired_joint_state */ - controller_interface::return_type update( - const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const geometry_msgs::msg::Wrench &measured_wrench, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, - const rclcpp::Duration &period, - trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_states); - - /** + controller_interface::return_type update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + + /** * Fill fields of `state_message` from current admittance controller state. * * \param[in] state_message */ - const control_msgs::msg::AdmittanceControllerState & get_controller_state(); - - public: - // admittance config parameters - std::shared_ptr parameter_handler_; - admittance_controller::Params parameters_; - protected: - /** + const control_msgs::msg::AdmittanceControllerState & get_controller_state(); + +public: + // admittance config parameters + std::shared_ptr parameter_handler_; + admittance_controller::Params parameters_; + +protected: + /** * Calculate the admittance rule from given the robot's current joint angles. the control frame rotation, the * current force torque transform, the reference force torque transform, the force torque sensor frame id, * the time delta, and the current admittance controller state. The admittance controller state input is updated @@ -161,53 +172,52 @@ namespace admittance_controller { * \param[in] admittance_state * \param[out] success */ - bool calculate_admittance_rule(AdmittanceState &admittance_state, double dt); + bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); - /** + /** * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement. The `wrench_world_` * estimate includes gravity compensation * \param[in] measured_wrench * \param[in] sensor_rot * \param[in] cog_rot */ - void process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &sensor_rot, - const Eigen::Matrix &cog_rot - ); - - template - void vec_to_eigen(const std::vector &data, T2 &matrix); + void process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_rot, const Eigen::Matrix & cog_rot); - // number of robot joint - size_t num_joints_; + template + void vec_to_eigen(const std::vector & data, T2 & matrix); - // Kinematics interface plugin loader - std::shared_ptr> kinematics_loader_; - std::unique_ptr kinematics_; + // number of robot joint + size_t num_joints_; - // filtered wrench in world frame - Eigen::Matrix wrench_world_; + // Kinematics interface plugin loader + std::shared_ptr> + kinematics_loader_; + std::unique_ptr kinematics_; - // admittance controllers internal state - AdmittanceState admittance_state_; + // filtered wrench in world frame + Eigen::Matrix wrench_world_; - // transforms needed for admittance update - Transforms trans_; - Transforms trans_ref_; + // admittance controllers internal state + AdmittanceState admittance_state_; - // position of center of gravity in cog_frame - Eigen::Vector3d cog_; + // transforms needed for admittance update + Transforms trans_; + Transforms trans_ref_; - // force applied to sensor due to weight of end effector - Eigen::Vector3d ee_weight_; + // position of center of gravity in cog_frame + Eigen::Vector3d cog_; - // ROS - control_msgs::msg::AdmittanceControllerState state_message_; - std::shared_ptr clock_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; + // force applied to sensor due to weight of end effector + Eigen::Vector3d ee_weight_; - }; + // ROS + control_msgs::msg::AdmittanceControllerState state_message_; + std::shared_ptr clock_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 66d0501867..c1e22d0c10 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -17,359 +17,431 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ -#include #include "admittance_controller/admittance_rule.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/utilities.hpp" - -namespace admittance_controller { - - controller_interface::return_type - AdmittanceRule::configure(const std::shared_ptr &node, size_t num_joints) { - // configure admittance rule using num_joints and load kinematics interface +#include +#include - num_joints_ = num_joints; +#include "tf2_ros/transform_listener.h" - // initialize ROS functions - clock_ = node->get_clock(); - tf_buffer_ = std::make_shared(clock_); - tf_listener_ = std::make_shared(*tf_buffer_); - - // initialize memory and values to zero (non-realtime function) - reset(num_joints); +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" - // Load the differential IK plugin - if (!parameters_.kinematics.plugin_name.empty()) { - try { - kinematics_loader_ = std::make_shared>( - parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); - kinematics_ = std::unique_ptr( - kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); - if (!kinematics_->initialize(node->get_node_parameters_interface(), parameters_.kinematics.tip)) { - return controller_interface::return_type::ERROR; - } - } - catch (pluginlib::PluginlibException &ex) { - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", - parameters_.kinematics.plugin_name.c_str(), ex.what()); +namespace admittance_controller +{ +controller_interface::return_type AdmittanceRule::configure( + const std::shared_ptr & node, size_t num_joints) +{ + // configure admittance rule using num_joints and load kinematics interface + + num_joints_ = num_joints; + + // initialize ROS functions + clock_ = node->get_clock(); + tf_buffer_ = std::make_shared(clock_); + tf_listener_ = std::make_shared(*tf_buffer_); + + // initialize memory and values to zero (non-realtime function) + reset(num_joints); + + // Load the differential IK plugin + if (!parameters_.kinematics.plugin_name.empty()) + { + try + { + kinematics_loader_ = + std::make_shared>( + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); + kinematics_ = std::unique_ptr( + kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( + node->get_node_parameters_interface(), parameters_.kinematics.tip)) + { return controller_interface::return_type::ERROR; } - } else { - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), - "A differential IK plugin name was not specified in the config file."); + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", + parameters_.kinematics.plugin_name.c_str(), ex.what()); return controller_interface::return_type::ERROR; } + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), + "A differential IK plugin name was not specified in the config file."); + return controller_interface::return_type::ERROR; + } - return controller_interface::return_type::OK; + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::reset(size_t num_joints) +{ + // reset all values back to default + + // reset state message fields + state_message_.joint_state.name.assign(num_joints, ""); + state_message_.joint_state.position.assign(num_joints, 0); + state_message_.joint_state.velocity.assign(num_joints, 0); + state_message_.joint_state.effort.assign(num_joints, 0); + for (auto i = 0ul; i < parameters_.joints.size(); i++) + { + state_message_.joint_state.name = parameters_.joints; } + state_message_.mass.data.resize(6, 0.0); + state_message_.selected_axes.data.resize(6, 0); + state_message_.damping.data.resize(6, 0); + state_message_.stiffness.data.resize(6, 0); - controller_interface::return_type AdmittanceRule::reset(size_t num_joints) { - // reset all values back to default + // reset admittance state + admittance_state_ = AdmittanceState(num_joints); - // reset state message fields - state_message_.joint_state.name.assign(num_joints, ""); - state_message_.joint_state.position.assign(num_joints, 0); - state_message_.joint_state.velocity.assign(num_joints, 0); - state_message_.joint_state.effort.assign(num_joints, 0); - for (auto i = 0ul; i < parameters_.joints.size(); i++){ - state_message_.joint_state.name = parameters_.joints; - } - state_message_.mass.data.resize(6, 0.0); - state_message_.selected_axes.data.resize(6, 0); - state_message_.damping.data.resize(6, 0); - state_message_.stiffness.data.resize(6, 0); + // reset transforms and rotations + trans_ = Transforms(); + trans_ref_ = Transforms(); - // reset admittance state - admittance_state_ = AdmittanceState(num_joints); + // reset forces + wrench_world_.setZero(); + ee_weight_.setZero(); - // reset transforms and rotations - trans_ = Transforms(); - trans_ref_ = Transforms(); + // load/initialize Eigen types from parameters + apply_parameters_update(); - // reset forces - wrench_world_.setZero(); - ee_weight_.setZero(); + return controller_interface::return_type::OK; +} - // load/initialize Eigen types from parameters +void AdmittanceRule::apply_parameters_update() +{ + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + } + // update param values + ee_weight_[2] = -parameters_.gravity_compensation.CoG.force; + vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_); + vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); + vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); + vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); + + for (auto i = 0ul; i < 6; ++i) + { + admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + } +} + +bool AdmittanceRule::get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const AdmittanceState & admittance_state) +{ + // get all reference transforms + bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, trans_ref_.base_ft_); + success &= kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.kinematics.tip, trans_ref_.base_tip_); + success &= kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.fixed_world_frame.frame.id, + trans_ref_.world_base_); + success &= kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, trans_ref_.base_sensor_); + success &= kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.gravity_compensation.frame.id, + trans_ref_.base_cog_); + success &= kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.control.frame.id, trans_ref_.base_control_); + + // get all current transforms + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, trans_.base_ft_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.kinematics.tip, trans_.base_tip_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.fixed_world_frame.frame.id, trans_.world_base_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, trans_.base_sensor_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.gravity_compensation.frame.id, trans_.base_cog_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.control.frame.id, trans_.base_control_); + + return success; +} + +// Update from reference joint states +controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) +{ + double dt = period.seconds(); + + if (parameters_.enable_parameter_update_without_reactivation) + { apply_parameters_update(); - - return controller_interface::return_type::OK; } - void AdmittanceRule::apply_parameters_update() { - if (parameter_handler_->is_old(parameters_)) { - parameters_ = parameter_handler_->get_params(); - } - // update param values - ee_weight_[2] = -parameters_.gravity_compensation.CoG.force; - vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_); - vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); - vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); - vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); - - for (auto i = 0ul; i < 6; ++i) { - admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * - sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); - } - + bool success = get_all_transforms(current_joint_state, reference_joint_state, admittance_state_); + + // calculate needed rotations + Eigen::Matrix rot_base_sensor = trans_.base_sensor_.rotation(); + Eigen::Matrix rot_world_base = trans_.world_base_.rotation(); + Eigen::Matrix rot_base_cog = trans_.base_cog_.rotation(); + Eigen::Matrix rot_base_control = trans_.base_control_.rotation(); + + // apply filter and update wrench_world_ vector + Eigen::Matrix rot_world_sensor = rot_world_base * rot_base_sensor; + Eigen::Matrix rot_world_cog = rot_world_base * rot_base_cog; + process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); + + // transform wrench_world_ into base frame + admittance_state_.wrench_base.block<3, 1>(0, 0) = + rot_world_base.transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_state_.wrench_base.block<3, 1>(3, 0) = + rot_world_base.transpose() * wrench_world_.block<3, 1>(3, 0); + + // Compute admittance control law + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = rot_base_control; + admittance_state_.ref_trans_base_ft = trans_ref_.base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + success &= calculate_admittance_rule(admittance_state_, dt); + + // if a failure occurred during any kinematics interface calls, return an error and don't + // modify the desired reference + if (!success) + { + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; } - bool AdmittanceRule::get_all_transforms(const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, - const AdmittanceState &admittance_state) { - // get all reference transforms - bool success = kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id, trans_ref_.base_ft_); - success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.kinematics.tip,trans_ref_.base_tip_); - success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.fixed_world_frame.frame.id,trans_ref_.world_base_); - success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.ft_sensor.frame.id,trans_ref_.base_sensor_); - success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.gravity_compensation.frame.id,trans_ref_.base_cog_); - success &= kinematics_->calculate_link_transform(reference_joint_state.positions, parameters_.control.frame.id,trans_ref_.base_control_); - - // get all current transforms - success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id,trans_.base_ft_); - success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.kinematics.tip,trans_.base_tip_); - success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.fixed_world_frame.frame.id,trans_.world_base_); - success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.ft_sensor.frame.id,trans_.base_sensor_); - success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.gravity_compensation.frame.id,trans_.base_cog_); - success &= kinematics_->calculate_link_transform(current_joint_state.positions, parameters_.control.frame.id,trans_.base_control_); - - return success; + // update joint desired joint state + for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) + { + desired_joint_state.positions[i] = + reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; } - - // Update from reference joint states - controller_interface::return_type AdmittanceRule::update( - const trajectory_msgs::msg::JointTrajectoryPoint ¤t_joint_state, - const geometry_msgs::msg::Wrench &measured_wrench, - const trajectory_msgs::msg::JointTrajectoryPoint &reference_joint_state, - const rclcpp::Duration &period, - trajectory_msgs::msg::JointTrajectoryPoint &desired_joint_state) { - - double dt = period.seconds(); - - if (parameters_.enable_parameter_update_without_reactivation) { - apply_parameters_update(); - } - - bool success = get_all_transforms(current_joint_state, reference_joint_state, admittance_state_); - - // calculate needed rotations - Eigen::Matrix rot_base_sensor = trans_.base_sensor_.rotation(); - Eigen::Matrix rot_world_base = trans_.world_base_.rotation(); - Eigen::Matrix rot_base_cog = trans_.base_cog_.rotation(); - Eigen::Matrix rot_base_control = trans_.base_control_.rotation(); - - // apply filter and update wrench_world_ vector - Eigen::Matrix rot_world_sensor = rot_world_base*rot_base_sensor; - Eigen::Matrix rot_world_cog = rot_world_base*rot_base_cog; - process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); - - // transform wrench_world_ into base frame - admittance_state_.wrench_base.block<3,1>(0,0) = rot_world_base.transpose() * wrench_world_.block<3,1>(0,0); - admittance_state_.wrench_base.block<3,1>(3,0) = rot_world_base.transpose() * wrench_world_.block<3,1>(3,0); - - // Compute admittance control law - vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); - admittance_state_.rot_base_control = rot_base_control; - admittance_state_.ref_trans_base_ft = trans_ref_.base_ft_; - admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; - success &= calculate_admittance_rule(admittance_state_, dt); - - // if a failure occurred during any kinematics interface calls, return an error and don't modify the desired reference - if (!success) { - desired_joint_state = reference_joint_state; - return controller_interface::return_type::ERROR; - } - - // update joint desired joint state - for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) { - desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; - } - for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) { - desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; - } - for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) { - desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; - } - - return controller_interface::return_type::OK; + for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) + { + desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; } - - bool AdmittanceRule::calculate_admittance_rule(AdmittanceState &admittance_state, double dt) { - - // create stiffness matrix - auto rot_base_control = admittance_state.rot_base_control; - Eigen::Matrix K = Eigen::Matrix::Zero(); - Eigen::Matrix K_pos = Eigen::Matrix::Zero(); - Eigen::Matrix K_rot = Eigen::Matrix::Zero(); - K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); - K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); - K_pos = rot_base_control * K_pos * rot_base_control.transpose(); - K_rot = rot_base_control * K_rot * rot_base_control.transpose(); - K.block<3, 3>(0, 0) = K_pos; - K.block<3, 3>(3, 3) = K_rot; - - // create damping matrix - Eigen::Matrix D = Eigen::Matrix::Zero(); - Eigen::Matrix D_pos = Eigen::Matrix::Zero(); - Eigen::Matrix D_rot = Eigen::Matrix::Zero(); - D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); - D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); - D_pos = rot_base_control * D_pos * rot_base_control.transpose(); - D_rot = rot_base_control * D_rot * rot_base_control.transpose(); - D.block<3, 3>(0, 0) = D_pos; - D.block<3, 3>(3, 3) = D_rot; - - // calculate admittance relative offset - Eigen::Isometry3d desired_trans_base_ft; - kinematics_->calculate_link_transform(admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); - Eigen::Matrix X; - X.block<3, 1>(0, 0) = desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation(); - auto R_ref = admittance_state.ref_trans_base_ft.rotation(); - auto R_desired = desired_trans_base_ft.rotation(); - auto R = R_desired * R_ref.transpose(); - auto angle_axis = Eigen::AngleAxisd(R); - X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); - - // get admittance relative velocity - auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); - - // get external force - auto F_base = Eigen::Matrix(admittance_state.wrench_base.data()); - - // zero out any forces in the control frame - Eigen::Matrix F_control; - F_control.block<3,1>(0,0) = rot_base_control.transpose()*F_base.block<3,1>(0,0); - F_control.block<3,1>(3,0) = rot_base_control.transpose()*F_base.block<3,1>(3,0); - F_control = F_control.cwiseProduct(admittance_state.selected_axes); - F_base.block<3,1>(0,0) = rot_base_control*F_control.block<3,1>(0,0); - F_base.block<3,1>(3,0) = rot_base_control*F_control.block<3,1>(3,0); - - - // Compute admittance control law: F = M*a + D*v + S*x - Eigen::Matrix X_ddot = admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); - bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas(admittance_state.current_joint_pos, X_ddot, - admittance_state.ft_sensor_frame, admittance_state.joint_acc); - - // integrate motion in joint space - admittance_state.joint_vel += admittance_state.joint_acc * dt; - admittance_state.joint_pos += admittance_state.joint_vel * dt; - - // calculate admittance velocity corresponding to joint velocity - success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(admittance_state.current_joint_pos, admittance_state.joint_vel, admittance_state.ft_sensor_frame, - admittance_state.admittance_velocity); - success &= kinematics_->convert_joint_deltas_to_cartesian_deltas(admittance_state.current_joint_pos, admittance_state.joint_acc, admittance_state.ft_sensor_frame, - admittance_state.admittance_acceleration); - - return success; + for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) + { + desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; } - void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench &measured_wrench, const Eigen::Matrix &sensor_world_rot, - const Eigen::Matrix &cog_world_rot - ) { - Eigen::Matrix new_wrench; - new_wrench(0, 0) = measured_wrench.force.x; - new_wrench(1, 0) = measured_wrench.force.y; - new_wrench(2, 0) = measured_wrench.force.z; - new_wrench(0, 1) = measured_wrench.torque.x; - new_wrench(1, 1) = measured_wrench.torque.y; - new_wrench(2, 1) = measured_wrench.torque.z; - - // transform to world frame - Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; - - // apply gravity compensation - new_wrench_base(2, 0) -= ee_weight_[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight_); - - // apply smoothing filter - for (auto i = 0; i < 6; i++) { - wrench_world_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); - } - + return controller_interface::return_type::OK; +} + +bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) +{ + // create stiffness matrix + auto rot_base_control = admittance_state.rot_base_control; + Eigen::Matrix K = Eigen::Matrix::Zero(); + Eigen::Matrix K_pos = Eigen::Matrix::Zero(); + Eigen::Matrix K_rot = Eigen::Matrix::Zero(); + K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); + K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); + K_pos = rot_base_control * K_pos * rot_base_control.transpose(); + K_rot = rot_base_control * K_rot * rot_base_control.transpose(); + K.block<3, 3>(0, 0) = K_pos; + K.block<3, 3>(3, 3) = K_rot; + + // create damping matrix + Eigen::Matrix D = Eigen::Matrix::Zero(); + Eigen::Matrix D_pos = Eigen::Matrix::Zero(); + Eigen::Matrix D_rot = Eigen::Matrix::Zero(); + D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); + D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); + D_pos = rot_base_control * D_pos * rot_base_control.transpose(); + D_rot = rot_base_control * D_rot * rot_base_control.transpose(); + D.block<3, 3>(0, 0) = D_pos; + D.block<3, 3>(3, 3) = D_rot; + + // calculate admittance relative offset + Eigen::Isometry3d desired_trans_base_ft; + kinematics_->calculate_link_transform( + admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); + Eigen::Matrix X; + X.block<3, 1>(0, 0) = + desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation(); + auto R_ref = admittance_state.ref_trans_base_ft.rotation(); + auto R_desired = desired_trans_base_ft.rotation(); + auto R = R_desired * R_ref.transpose(); + auto angle_axis = Eigen::AngleAxisd(R); + X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); + + // get admittance relative velocity + auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); + + // get external force + auto F_base = Eigen::Matrix(admittance_state.wrench_base.data()); + + // zero out any forces in the control frame + Eigen::Matrix F_control; + F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); + F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + F_control = F_control.cwiseProduct(admittance_state.selected_axes); + F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); + F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); + + // Compute admittance control law: F = M*a + D*v + S*x + Eigen::Matrix X_ddot = + admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); + bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( + admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, + admittance_state.joint_acc); + + // integrate motion in joint space + admittance_state.joint_vel += admittance_state.joint_acc * dt; + admittance_state.joint_pos += admittance_state.joint_vel * dt; + + // calculate admittance velocity corresponding to joint velocity + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_vel, + admittance_state.ft_sensor_frame, admittance_state.admittance_velocity); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_acc, + admittance_state.ft_sensor_frame, admittance_state.admittance_acceleration); + + return success; +} + +void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot) +{ + Eigen::Matrix new_wrench; + new_wrench(0, 0) = measured_wrench.force.x; + new_wrench(1, 0) = measured_wrench.force.y; + new_wrench(2, 0) = measured_wrench.force.z; + new_wrench(0, 1) = measured_wrench.torque.x; + new_wrench(1, 1) = measured_wrench.torque.y; + new_wrench(2, 1) = measured_wrench.torque.z; + + // transform to world frame + Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; + + // apply gravity compensation + new_wrench_base(2, 0) -= ee_weight_[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight_); + + // apply smoothing filter + for (auto i = 0; i < 6; i++) + { + wrench_world_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); } +} - const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() { - for (auto i = 0ul; i < parameters_.joints.size(); i++) { - state_message_.joint_state.name[i] = parameters_.joints[i]; - } - for (auto i = 0l; i < admittance_state_.joint_pos.size(); i++){ - state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; - } - for (auto i = 0l; i < admittance_state_.joint_vel.size(); i++){ - state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; - } - for (auto i = 0l; i < admittance_state_.joint_acc.size(); i++){ - state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; - } - for (auto i = 0l; i < admittance_state_.stiffness.size(); i++){ - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - } - for (auto i = 0l; i < admittance_state_.damping.size(); i++){ - state_message_.damping.data[i] = admittance_state_.damping[i]; - } - for (auto i = 0l; i < admittance_state_.selected_axes.size(); i++){ - state_message_.selected_axes.data[i] = (bool) admittance_state_.selected_axes[i]; - } - for (auto i = 0l; i < admittance_state_.mass.size(); i++){ - state_message_.mass.data[i] = admittance_state_.mass[i]; - } - - state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here - state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; - state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; - state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; - state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3]; - state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; - state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; - - state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here - state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; - state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; - state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; - state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3]; - state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; - state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; - - state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here - state_message_.admittance_acceleration.twist.linear.x = admittance_state_.admittance_acceleration[0]; - state_message_.admittance_acceleration.twist.linear.y = admittance_state_.admittance_acceleration[1]; - state_message_.admittance_acceleration.twist.linear.z = admittance_state_.admittance_acceleration[2]; - state_message_.admittance_acceleration.twist.angular.x = admittance_state_.admittance_acceleration[3]; - state_message_.admittance_acceleration.twist.angular.y = admittance_state_.admittance_acceleration[4]; - state_message_.admittance_acceleration.twist.angular.z = admittance_state_.admittance_acceleration[5]; - - state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here - state_message_.admittance_position.child_frame_id = "admittance_offset"; // TODO(anyone) remove dynamic allocation here - state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); - - state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; - state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; - state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); - - Eigen::Quaterniond quat(admittance_state_.rot_base_control); - state_message_.rot_base_control.w = quat.w(); - state_message_.rot_base_control.x = quat.x(); - state_message_.rot_base_control.y = quat.y(); - state_message_.rot_base_control.z = quat.z(); - - state_message_.ft_sensor_frame.data = admittance_state_.ft_sensor_frame; // TODO(anyone) remove dynamic allocation here - - return state_message_; - +const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() +{ + for (auto i = 0ul; i < parameters_.joints.size(); i++) + { + state_message_.joint_state.name[i] = parameters_.joints[i]; + } + for (auto i = 0l; i < admittance_state_.joint_pos.size(); i++) + { + state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; + } + for (auto i = 0l; i < admittance_state_.joint_vel.size(); i++) + { + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; + } + for (auto i = 0l; i < admittance_state_.joint_acc.size(); i++) + { + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + } + for (auto i = 0l; i < admittance_state_.stiffness.size(); i++) + { + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + } + for (auto i = 0l; i < admittance_state_.damping.size(); i++) + { + state_message_.damping.data[i] = admittance_state_.damping[i]; + } + for (auto i = 0l; i < admittance_state_.selected_axes.size(); i++) + { + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); + } + for (auto i = 0l; i < admittance_state_.mass.size(); i++) + { + state_message_.mass.data[i] = admittance_state_.mass[i]; } - template - void AdmittanceRule::vec_to_eigen(const std::vector &data, T2 &matrix) { - for (auto col = 0; col < matrix.cols(); col++) { - for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col * matrix.rows()]; - } + state_message_.wrench_base.header.frame_id = + parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here + state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; + state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; + state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; + state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3]; + state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; + state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; + + state_message_.admittance_velocity.header.frame_id = + parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here + state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; + state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; + state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; + state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3]; + state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; + state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; + + state_message_.admittance_acceleration.header.frame_id = + parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here + state_message_.admittance_acceleration.twist.linear.x = + admittance_state_.admittance_acceleration[0]; + state_message_.admittance_acceleration.twist.linear.y = + admittance_state_.admittance_acceleration[1]; + state_message_.admittance_acceleration.twist.linear.z = + admittance_state_.admittance_acceleration[2]; + state_message_.admittance_acceleration.twist.angular.x = + admittance_state_.admittance_acceleration[3]; + state_message_.admittance_acceleration.twist.angular.y = + admittance_state_.admittance_acceleration[4]; + state_message_.admittance_acceleration.twist.angular.z = + admittance_state_.admittance_acceleration[5]; + + state_message_.admittance_position.header.frame_id = + parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here + state_message_.admittance_position.child_frame_id = + "admittance_offset"; // TODO(anyone) remove dynamic allocation here + state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + + state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; + state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; + state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + + Eigen::Quaterniond quat(admittance_state_.rot_base_control); + state_message_.rot_base_control.w = quat.w(); + state_message_.rot_base_control.x = quat.x(); + state_message_.rot_base_control.y = quat.y(); + state_message_.rot_base_control.z = quat.z(); + + state_message_.ft_sensor_frame.data = + admittance_state_.ft_sensor_frame; // TODO(anyone) remove dynamic allocation here + + return state_message_; +} + +template +void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) +{ + for (auto col = 0; col < matrix.cols(); col++) + { + for (auto row = 0; row < matrix.rows(); row++) + { + matrix(row, col) = data[row + col * matrix.rows()]; } } +} } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index e992ef8c0a..51b726f35e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,411 +16,490 @@ #include "admittance_controller/admittance_controller.hpp" -#include #include +#include #include #include #include #include -#include +#include "tf2_ros/buffer.h" + #include "admittance_controller/admittance_rule_impl.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "rcutils/logging_macros.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -namespace admittance_controller { +namespace admittance_controller +{ +controller_interface::CallbackReturn AdmittanceController::on_init() +{ + // initialize controller config + try + { + admittance_ = std::make_unique(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; + } + num_joints_ = admittance_->parameters_.joints.size(); + + // allocate dynamic memory + last_reference_.positions.assign(num_joints_, 0.0); + last_reference_.velocities.assign(num_joints_, 0.0); + last_reference_.accelerations.assign(num_joints_, 0.0); + last_commanded_ = last_reference_; + reference_ = last_reference_; + reference_admittance_ = last_reference_; + joint_state_ = last_reference_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() + const +{ + if (!admittance_) + { + return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; + } - controller_interface::CallbackReturn AdmittanceController::on_init() { - // initialize controller config - try { - admittance_ = std::make_unique(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; + std::vector command_interfaces_config_names; + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + for (const auto & joint : admittance_->parameters_.joints) + { + auto full_name = joint + "/" + interface; + command_interfaces_config_names.push_back(full_name); } - num_joints_ = admittance_->parameters_.joints.size(); - - // allocate dynamic memory - last_reference_.positions.assign(num_joints_, 0.0); - last_reference_.velocities.assign(num_joints_, 0.0); - last_reference_.accelerations.assign(num_joints_, 0.0); - last_commanded_ = last_reference_; - reference_ = last_reference_; - reference_admittance_ = last_reference_; - joint_state_ = last_reference_; - - return controller_interface::CallbackReturn::SUCCESS; } - controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() const { - // Create an InterfaceConfiguration for the controller manager. The controller manager will then try to - // claim the joint + interface combinations for state interfaces from the resource manager. Finally, - // controller manager will populate the state_interfaces_ vector field via the ControllerInterfaceBase. - // Note: state_interface_types_ contains position, velocity, acceleration; effort is not supported - if (!admittance_) { - return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; - } + return { + controller_interface::interface_configuration_type::INDIVIDUAL, + command_interfaces_config_names}; +} + +controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() + const +{ + if (!admittance_) + { + return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; + } - std::vector state_interfaces_config_names; - for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { - const auto &interface = admittance_->parameters_.state_interfaces[i]; - for (const auto &joint: admittance_->parameters_.joints) { - auto full_name = joint + "/" + interface; - state_interfaces_config_names.push_back(full_name); - } + std::vector state_interfaces_config_names; + for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) + { + const auto & interface = admittance_->parameters_.state_interfaces[i]; + for (const auto & joint : admittance_->parameters_.joints) + { + auto full_name = joint + "/" + interface; + state_interfaces_config_names.push_back(full_name); } - - auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); - state_interfaces_config_names.insert(state_interfaces_config_names.end(), ft_interfaces.begin(), - ft_interfaces.end()); - - return {controller_interface::interface_configuration_type::INDIVIDUAL, - state_interfaces_config_names}; } - controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() const { - // Create an InterfaceConfiguration for the controller manager. The controller manager will then try to - // claim the joint + interface combinations for command interfaces from the resource manager. Finally, - // controller manager will populate the command_interfaces_ vector field via the ControllerInterfaceBase - // Note: command_interface_types_ contains position, velocity; acceleration, effort are not supported - if (!admittance_) { - return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; - } + auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); + state_interfaces_config_names.insert( + state_interfaces_config_names.end(), ft_interfaces.begin(), ft_interfaces.end()); + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names}; +} + +std::vector +AdmittanceController::on_export_reference_interfaces() +{ + // create CommandInterface interfaces that other controllers will be able to chain with + if (!admittance_) + { + return {}; + } - std::vector command_interfaces_config_names; - for (const auto &interface: admittance_->parameters_.command_interfaces) { - for (const auto &joint: admittance_->parameters_.joints) { - auto full_name = joint + "/" + interface; - command_interfaces_config_names.push_back(full_name); + std::vector chainable_command_interfaces; + auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * + admittance_->parameters_.joints.size(); + + // allocate dynamic memory + chainable_command_interfaces.reserve(num_chainable_interfaces); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + position_reference_ = {}; + velocity_reference_ = {}; + + // assign reference interfaces + auto index = 0ul; + for (const auto & interface : reference_interfaces_types_) + { + for (const auto & joint : admittance_->parameters_.joints) + { + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_[index]); + else if (hardware_interface::HW_IF_VELOCITY == interface) + { + velocity_reference_.emplace_back(reference_interfaces_[index]); } + auto full_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index++)); } - - return {controller_interface::interface_configuration_type::INDIVIDUAL, - command_interfaces_config_names}; } - std::vector AdmittanceController::on_export_reference_interfaces() { - // create CommandInterface interfaces that other controllers will be able to chain with - if (!admittance_) { - return {}; - } + return chainable_command_interfaces; + ; +} - std::vector chainable_command_interfaces; - auto num_chainable_interfaces = - admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); - - //allocate dynamic memory - chainable_command_interfaces.reserve(num_chainable_interfaces); - reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); - position_reference_ = {}; - velocity_reference_ = {}; - - // assign reference interfaces - auto index = 0ul; - for (const auto &interface: reference_interfaces_types_) { - for (const auto &joint: admittance_->parameters_.joints) { - if (hardware_interface::HW_IF_POSITION == interface) - position_reference_.emplace_back(reference_interfaces_[index]); - else if (hardware_interface::HW_IF_VELOCITY == interface) { - velocity_reference_.emplace_back(reference_interfaces_[index]); - } - auto full_name = joint + "/" + interface; - chainable_command_interfaces.emplace_back( - hardware_interface::CommandInterface(std::string(get_node()->get_name()), - full_name, - reference_interfaces_.data() + - index++)); - } - } - - return chainable_command_interfaces;; +controller_interface::CallbackReturn AdmittanceController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + try + { + admittance_ = std::make_unique(get_node()); + num_joints_ = admittance_->parameters_.joints.size(); } - - controller_interface::CallbackReturn AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { - try { - admittance_ = std::make_unique(get_node()); - num_joints_ = admittance_->parameters_.joints.size(); - } 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; - } - // print and validate interface types - for (const auto &tmp: admittance_->parameters_.state_interfaces) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); - } - for (const auto &tmp: admittance_->parameters_.command_interfaces) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + 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; + } + // print and validate interface types + for (const auto & tmp : admittance_->parameters_.state_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto & tmp : admittance_->parameters_.command_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + for (const auto & tmp : admittance_->parameters_.chainable_command_interfaces) + { + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); } - for (const auto &tmp: admittance_->parameters_.chainable_command_interfaces) { - if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) { - RCLCPP_INFO(get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); - } else { - RCLCPP_ERROR(get_node()->get_logger(), - "chainable interface type %s is not supported. Supported types are %s and %s", - tmp.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); - return controller_interface::CallbackReturn::ERROR; - } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return controller_interface::CallbackReturn::ERROR; } + } - auto get_interface_list = [](const std::vector &interface_types) { - std::stringstream ss_command_interfaces; - for (size_t index = 0; index < interface_types.size(); ++index) { - if (index != 0) { - ss_command_interfaces << " "; - } - ss_command_interfaces << interface_types[index]; + auto get_interface_list = [](const std::vector & interface_types) + { + std::stringstream ss_command_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) + { + if (index != 0) + { + ss_command_interfaces << " "; } - return ss_command_interfaces.str(); - }; - RCLCPP_INFO( - get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", - get_interface_list(admittance_->parameters_.command_interfaces).c_str(), - get_interface_list(admittance_->parameters_.state_interfaces).c_str()); - - // setup subscribers and publishers - auto joint_command_callback = [this](const std::shared_ptr msg) { - input_joint_command_.writeFromNonRT(msg); - }; - input_joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_commands", rclcpp::SystemDefaultsQoS(), joint_command_callback); - s_publisher_ = get_node()->create_publisher( - "~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = - std::make_unique>(s_publisher_); - - // Initialize state message - state_publisher_->lock(); - state_publisher_->msg_ = admittance_->get_controller_state(); - state_publisher_->unlock(); - - // Initialize FTS semantic semantic_component - force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); - - // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) { - return controller_interface::CallbackReturn::ERROR; + ss_command_interfaces << interface_types[index]; } - - return LifecycleNodeInterface::on_configure(previous_state); + return ss_command_interfaces.str(); + }; + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(admittance_->parameters_.command_interfaces).c_str(), + get_interface_list(admittance_->parameters_.state_interfaces).c_str()); + + // setup subscribers and publishers + auto joint_command_callback = + [this](const std::shared_ptr msg) + { input_joint_command_.writeFromNonRT(msg); }; + input_joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_commands", rclcpp::SystemDefaultsQoS(), joint_command_callback); + s_publisher_ = get_node()->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = + std::make_unique>(s_publisher_); + + // Initialize state message + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlock(); + + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); + + // configure admittance rule + if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + { + return controller_interface::CallbackReturn::ERROR; } - controller_interface::CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { - // on_activate is called when the lifecycle node activates. - if (!admittance_) { - return controller_interface::CallbackReturn::ERROR; - } + return LifecycleNodeInterface::on_configure(previous_state); +} - // update parameters if any have changed - if (admittance_->parameter_handler_->is_old(admittance_->parameters_)) { - admittance_->parameters_ = admittance_->parameter_handler_->get_params(); - } +controller_interface::CallbackReturn AdmittanceController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + // on_activate is called when the lifecycle node activates. + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } - // get state interface inds - std::unordered_map inter_to_ind = {{hardware_interface::HW_IF_POSITION, -1}, - {hardware_interface::HW_IF_VELOCITY, -1}, - {hardware_interface::HW_IF_ACCELERATION, -1}}; - for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) { - const auto &interface = admittance_->parameters_.state_interfaces[i]; - inter_to_ind[interface] = i; - } - state_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; - state_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; - state_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; - - // get state interface inds - inter_to_ind = {{hardware_interface::HW_IF_POSITION, -1}, - {hardware_interface::HW_IF_VELOCITY, -1}, - {hardware_interface::HW_IF_ACCELERATION, -1}}; - for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) { - const auto &interface = admittance_->parameters_.command_interfaces[i]; - inter_to_ind[interface] = i; - } - command_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; - command_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; - command_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; + // update parameters if any have changed + if (admittance_->parameter_handler_->is_old(admittance_->parameters_)) + { + admittance_->parameters_ = admittance_->parameter_handler_->get_params(); + } + + // get state interface inds + std::unordered_map inter_to_ind = { + {hardware_interface::HW_IF_POSITION, -1}, + {hardware_interface::HW_IF_VELOCITY, -1}, + {hardware_interface::HW_IF_ACCELERATION, -1}}; + for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) + { + const auto & interface = admittance_->parameters_.state_interfaces[i]; + inter_to_ind[interface] = i; + } + state_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; + state_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; + state_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; + + // get state interface inds + inter_to_ind = { + {hardware_interface::HW_IF_POSITION, -1}, + {hardware_interface::HW_IF_VELOCITY, -1}, + {hardware_interface::HW_IF_ACCELERATION, -1}}; + for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) + { + const auto & interface = admittance_->parameters_.command_interfaces[i]; + inter_to_ind[interface] = i; + } + command_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; + command_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; + command_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; + // initialize interface of the FTS semantic semantic component + force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); - // initialize interface of the FTS semantic semantic component - force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + // handle state after restart or initial startups + read_state_from_hardware(joint_state_, ft_values_); - // handle state after restart or initial startups - read_state_from_hardware(joint_state_, ft_values_); + // initialize states from last read state reference + last_commanded_ = joint_state_; + last_reference_ = joint_state_; - // initialize states from last read state reference - last_commanded_ = joint_state_; - last_reference_ = joint_state_; + read_state_reference_interfaces(reference_); - read_state_reference_interfaces(reference_); + // reset dynamic fields in case non-zero + reference_.velocities.assign(num_joints_, 0.0); + reference_.accelerations.assign(num_joints_, 0.0); + reference_admittance_ = reference_; - // reset dynamic fields in case non-zero - reference_.velocities.assign(num_joints_, 0.0); - reference_.accelerations.assign(num_joints_, 0.0); - reference_admittance_ = reference_; + return controller_interface::CallbackReturn::SUCCESS; +} - return controller_interface::CallbackReturn::SUCCESS; +controller_interface::return_type AdmittanceController::update_reference_from_subscribers() +{ + // update input reference from ros subscriber message + if (!admittance_) + { + return controller_interface::return_type::ERROR; } - controller_interface::return_type AdmittanceController::update_reference_from_subscribers() { - // update input reference from ros subscriber message - if (!admittance_) { - return controller_interface::return_type::ERROR; - } - - joint_command_msg_ = *input_joint_command_.readFromRT(); + joint_command_msg_ = *input_joint_command_.readFromRT(); - // if message exists, load values into references - if (joint_command_msg_.get()) { - for (auto i = 0ul; i < joint_command_msg_->positions.size(); i++) { - position_reference_[i].get() = joint_command_msg_->positions[i]; - } - for (auto i = 0ul; i < joint_command_msg_->velocities.size(); i++) { - velocity_reference_[i].get() = joint_command_msg_->velocities[i]; - } + // if message exists, load values into references + if (joint_command_msg_.get()) + { + for (auto i = 0ul; i < joint_command_msg_->positions.size(); i++) + { + position_reference_[i].get() = joint_command_msg_->positions[i]; + } + for (auto i = 0ul; i < joint_command_msg_->velocities.size(); i++) + { + velocity_reference_[i].get() = joint_command_msg_->velocities[i]; } - - return controller_interface::return_type::OK; } - controller_interface::return_type - AdmittanceController::update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) { - // Realtime constraints are required in this function - if (!admittance_) { - return controller_interface::return_type::ERROR; - } + return controller_interface::return_type::OK; +} - // update input reference from chainable interfaces - read_state_reference_interfaces(reference_); +controller_interface::return_type AdmittanceController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // Realtime constraints are required in this function + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } - // get all controller inputs - read_state_from_hardware(joint_state_, ft_values_); + // update input reference from chainable interfaces + read_state_reference_interfaces(reference_); - // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + // get all controller inputs + read_state_from_hardware(joint_state_, ft_values_); - // write calculated values to joint interfaces - write_state_to_hardware(reference_admittance_); + // apply admittance control to reference to determine desired state + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); - // Publish controller state - state_publisher_->lock(); - state_publisher_->msg_ = admittance_->get_controller_state(); - state_publisher_->unlockAndPublish(); + // write calculated values to joint interfaces + write_state_to_hardware(reference_admittance_); - return controller_interface::return_type::OK; - } + // Publish controller state + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlockAndPublish(); - controller_interface::CallbackReturn AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { - force_torque_sensor_->release_interfaces(); + return controller_interface::return_type::OK; +} - return LifecycleNodeInterface::on_deactivate(previous_state); - } +controller_interface::CallbackReturn AdmittanceController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + force_torque_sensor_->release_interfaces(); - controller_interface::CallbackReturn AdmittanceController::on_cleanup(const rclcpp_lifecycle::State &previous_state) { - return controller_interface::CallbackReturn::SUCCESS; - } + return LifecycleNodeInterface::on_deactivate(previous_state); +} - controller_interface::CallbackReturn AdmittanceController::on_error(const rclcpp_lifecycle::State &previous_state) { - if (!admittance_) { - return controller_interface::CallbackReturn::ERROR; - } - admittance_->reset(num_joints_); - return controller_interface::CallbackReturn::SUCCESS; - } +controller_interface::CallbackReturn AdmittanceController::on_cleanup( + const rclcpp_lifecycle::State & previous_state) +{ + return controller_interface::CallbackReturn::SUCCESS; +} - void AdmittanceController::read_state_from_hardware( - trajectory_msgs::msg::JointTrajectoryPoint &state_current, - geometry_msgs::msg::Wrench &ft_values) { - // Fill fields of state_reference argument from hardware state interfaces. If the hardware does not exist or - // the values are nan, the corresponding state field will be set to empty. - - // if any interface has nan values, assume state_reference is the last command state - for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) { - for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) { - auto ind = joint_ind + num_joints_ * inter_ind; - if (inter_ind == state_pos_ind) { - state_current.positions[joint_ind] = state_interfaces_[ind].get_value(); - if (std::isnan(state_current.positions[joint_ind])) { - state_current.positions = last_commanded_.positions; - break; - } - } else if (inter_ind == state_vel_ind) { - state_current.velocities[joint_ind] = state_interfaces_[ind].get_value(); - if (std::isnan(state_current.positions[joint_ind])) { - state_current.velocities = last_commanded_.velocities; - break; - } - } else if (inter_ind == state_acc_ind) { - state_current.accelerations[joint_ind] = state_interfaces_[ind].get_value(); - if (std::isnan(state_current.positions[joint_ind])) { - state_current.accelerations = last_commanded_.accelerations; - break; - } +controller_interface::CallbackReturn AdmittanceController::on_error( + const rclcpp_lifecycle::State & previous_state) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + admittance_->reset(num_joints_); + return controller_interface::CallbackReturn::SUCCESS; +} + +void AdmittanceController::read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values) +{ + // Fill fields of state_reference argument from hardware state interfaces. If the hardware does + // not exist or the values are nan, the corresponding state field will be set to empty. + + // if any interface has nan values, assume state_reference is the last command state + for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) + { + for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) + { + auto ind = joint_ind + num_joints_ * inter_ind; + if (inter_ind == state_pos_ind) + { + state_current.positions[joint_ind] = state_interfaces_[ind].get_value(); + if (std::isnan(state_current.positions[joint_ind])) + { + state_current.positions = last_commanded_.positions; + break; } } - } - force_torque_sensor_->get_values_as_message(ft_values); - if (std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || std::isnan(ft_values.force.z) - || std::isnan(ft_values.torque.x) || std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)){ - ft_values = geometry_msgs::msg::Wrench(); - } - - } - - void - AdmittanceController::write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint &state_commanded) { - // Fill fields of state_reference argument from hardware state interfaces. If the hardware does not exist or - // the values are nan, the corresponding state field will be set to empty. - - // if any interface has nan values, assume state_reference is the last command state - for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) { - for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) { - auto ind = joint_ind + num_joints_ * inter_ind; - if (inter_ind == command_pos_ind) { - command_interfaces_[ind].set_value(state_commanded.positions[joint_ind]); - } else if (inter_ind == command_vel_ind) { - command_interfaces_[ind].set_value(state_commanded.velocities[joint_ind]); - } else if (inter_ind == command_acc_ind) { - command_interfaces_[ind].set_value(state_commanded.accelerations[joint_ind]); + else if (inter_ind == state_vel_ind) + { + state_current.velocities[joint_ind] = state_interfaces_[ind].get_value(); + if (std::isnan(state_current.positions[joint_ind])) + { + state_current.velocities = last_commanded_.velocities; + break; + } + } + else if (inter_ind == state_acc_ind) + { + state_current.accelerations[joint_ind] = state_interfaces_[ind].get_value(); + if (std::isnan(state_current.positions[joint_ind])) + { + state_current.accelerations = last_commanded_.accelerations; + break; } } } - last_commanded_ = reference_admittance_; - } - - void AdmittanceController::read_state_reference_interfaces( - trajectory_msgs::msg::JointTrajectoryPoint &state_reference) { - // Fill fields of state_reference argument from controller references. If the interface does not exist or - // the values are nan, the corresponding field will be set to empty - - for (auto i = 0ul; i < position_reference_.size(); i++) { - if (std::isnan(position_reference_[i])) { - position_reference_[i].get() = last_reference_.positions[i]; + force_torque_sensor_->get_values_as_message(ft_values); + if ( + std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || + std::isnan(ft_values.force.z) || std::isnan(ft_values.torque.x) || + std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)) + { + ft_values = geometry_msgs::msg::Wrench(); + } +} + +void AdmittanceController::write_state_to_hardware( + const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded) +{ + // Fill fields of state_reference argument from hardware state interfaces. If the hardware does + // not exist or the values are nan, the corresponding state field will be set to empty. + + // if any interface has nan values, assume state_reference is the last command state + for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) + { + for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) + { + auto ind = joint_ind + num_joints_ * inter_ind; + if (inter_ind == command_pos_ind) + { + command_interfaces_[ind].set_value(state_commanded.positions[joint_ind]); } - state_reference.positions[i] = position_reference_[i]; - } - last_reference_.positions = state_reference.positions; - - for (auto i = 0ul; i < velocity_reference_.size(); i++) { - if (std::isnan(velocity_reference_[i])) { - velocity_reference_[i].get() = last_reference_.velocities[i]; + else if (inter_ind == command_vel_ind) + { + command_interfaces_[ind].set_value(state_commanded.velocities[joint_ind]); + } + else if (inter_ind == command_acc_ind) + { + command_interfaces_[ind].set_value(state_commanded.accelerations[joint_ind]); } - state_reference.velocities[i] = velocity_reference_[i]; } - last_reference_.velocities = state_reference.velocities; - } + last_commanded_ = reference_admittance_; +} + +void AdmittanceController::read_state_reference_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint & state_reference) +{ + // TODO(destogl): check why is this here? + // Fill fields of state_reference argument from controller references. + // If the interface does not exist or + // the values are nan, the corresponding field will be set to empty + + for (auto i = 0ul; i < position_reference_.size(); i++) + { + if (std::isnan(position_reference_[i])) + { + position_reference_[i].get() = last_reference_.positions[i]; + } + state_reference.positions[i] = position_reference_[i]; + } + last_reference_.positions = state_reference.positions; - bool AdmittanceController::on_set_chained_mode(bool chained_mode) { - // this method sets the chained mode to value of argument chained_mode if true is returned. - return true; + for (auto i = 0ul; i < velocity_reference_.size(); i++) + { + if (std::isnan(velocity_reference_[i])) + { + velocity_reference_[i].get() = last_reference_.velocities[i]; + } + state_reference.velocities[i] = velocity_reference_[i]; } + last_reference_.velocities = state_reference.velocities; +} + +bool AdmittanceController::on_set_chained_mode(bool chained_mode) +{ + // this method sets the chained mode to value of argument chained_mode if true is returned. + return true; +} } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(admittance_controller::AdmittanceController, - controller_interface::ChainableControllerInterface -) +PLUGINLIB_EXPORT_CLASS( + admittance_controller::AdmittanceController, controller_interface::ChainableControllerInterface) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index deec50b5e7..55173bea33 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -21,7 +21,6 @@ #include #include - // Test on_configure returns ERROR when a required parameter is missing TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) { @@ -30,79 +29,51 @@ TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_ } INSTANTIATE_TEST_SUITE_P( - MissingMandatoryParameterDuringConfiguration, - AdmittanceControllerTestParameterizedMissingParameters, - ::testing::Values( - "admittance.mass", - "admittance.selected_axes", - "admittance.stiffness", - "chainable_command_interfaces", - "command_interfaces", - "control.frame.id", - "fixed_world_frame.frame.id", - "ft_sensor.frame.id", - "ft_sensor.name", - "gravity_compensation.CoG.pos", - "gravity_compensation.frame.id", - "joints", - "kinematics.base", - "kinematics.plugin_name", - "kinematics.plugin_package", - "kinematics.tip", - "state_interfaces" - ) -); + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingParameters, + ::testing::Values( + "admittance.mass", "admittance.selected_axes", "admittance.stiffness", + "chainable_command_interfaces", "command_interfaces", "control.frame.id", + "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", + "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", + "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); INSTANTIATE_TEST_SUITE_P( - InvalidParameterDuringConfiguration, - AdmittanceControllerTestParameterizedInvalidParameters, - ::testing::Values( + InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, + ::testing::Values( // wrong length COG std::make_tuple( - std::string("gravity_compensation.CoG.pos"), - rclcpp::ParameterValue(std::vector()={1,2,3,4}) - ), + std::string("gravity_compensation.CoG.pos"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), // wrong length stiffness std::make_tuple( - std::string("admittance.stiffness"), - rclcpp::ParameterValue(std::vector()={1,2,3}) - ), - // negative stiffness + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative stiffness std::make_tuple( - std::string("admittance.stiffness"), - rclcpp::ParameterValue(std::vector()={-1,-2,3,4,5,6}) - ), + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), // wrong length mass std::make_tuple( - std::string("admittance.mass"), - rclcpp::ParameterValue(std::vector()={1,2,3}) - ), + std::string("admittance.mass"), rclcpp::ParameterValue(std::vector() = {1, 2, 3})), // negative mass std::make_tuple( - std::string("admittance.mass"), - rclcpp::ParameterValue(std::vector()={-1,-2,3,4,5,6}) - ), + std::string("admittance.mass"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), // wrong length damping ratio std::make_tuple( - std::string("admittance.damping_ratio"), - rclcpp::ParameterValue(std::vector()={1,2,3}) - ), + std::string("admittance.damping_ratio"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), // wrong length selected axes std::make_tuple( - std::string("admittance.selected_axes"), - rclcpp::ParameterValue(std::vector()={1,2,3}) - ), + std::string("admittance.selected_axes"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), // invalid robot description std::make_tuple( - std::string("robot_description"), - rclcpp::ParameterValue(std::string()="bad_robot") - ) - ) -); + std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) { - auto result = SetUpController(); ASSERT_EQ(result, controller_interface::return_type::OK); @@ -111,51 +82,65 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); - ASSERT_TRUE(std::equal(controller_->admittance_->parameters_.joints.begin(), controller_->admittance_->parameters_.joints.end(), - joint_names_.begin(), joint_names_.end())); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.joints.begin(), + controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.command_interfaces.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.command_interfaces.size() == command_interface_types_.size()); + ASSERT_TRUE( + controller_->admittance_->parameters_.command_interfaces.size() == + command_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.command_interfaces.begin(), controller_->admittance_->parameters_.command_interfaces.end(), + controller_->admittance_->parameters_.command_interfaces.begin(), + controller_->admittance_->parameters_.command_interfaces.end(), command_interface_types_.begin(), command_interface_types_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.state_interfaces.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); + ASSERT_TRUE( + controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.state_interfaces.begin(), controller_->admittance_->parameters_.state_interfaces.end(), - state_interface_types_.begin(), state_interface_types_.end())); + controller_->admittance_->parameters_.state_interfaces.begin(), + controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), + state_interface_types_.end())); ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.admittance.selected_axes.size() == admittance_selected_axes_.size()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.selected_axes.size() == + admittance_selected_axes_.size()); ASSERT_TRUE(std::equal( controller_->admittance_->parameters_.admittance.selected_axes.begin(), controller_->admittance_->parameters_.admittance.selected_axes.end(), - admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.mass.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.admittance.mass.begin(), controller_->admittance_->parameters_.admittance.mass.end(), - admittance_mass_.begin(), admittance_mass_.end())); + controller_->admittance_->parameters_.admittance.mass.begin(), + controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), + admittance_mass_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.damping_ratio.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.admittance.damping_ratio.size() == admittance_damping_ratio_.size()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.damping_ratio.size() == + admittance_damping_ratio_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->parameters_.admittance.damping_ratio.begin(), - controller_->admittance_->parameters_.admittance.damping_ratio.end(), - admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); + controller_->admittance_->parameters_.admittance.damping_ratio.begin(), + controller_->admittance_->parameters_.admittance.damping_ratio.end(), + admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.stiffness.empty()); - ASSERT_TRUE(controller_->admittance_->parameters_.admittance.stiffness.size() == admittance_stiffness_.size()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.stiffness.size() == + admittance_stiffness_.size()); ASSERT_TRUE(std::equal( controller_->admittance_->parameters_.admittance.stiffness.begin(), - controller_->admittance_->parameters_.admittance.stiffness.end(), - admittance_stiffness_.begin(), admittance_stiffness_.end())); + controller_->admittance_->parameters_.admittance.stiffness.end(), admittance_stiffness_.begin(), + admittance_stiffness_.end())); } TEST_F(AdmittanceControllerTest, check_interfaces) @@ -167,23 +152,25 @@ TEST_F(AdmittanceControllerTest, check_interfaces) auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); - ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + ASSERT_EQ( + controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); - ASSERT_EQ(controller_->state_interfaces_.size(), state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); + ASSERT_EQ( + controller_->state_interfaces_.size(), + state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); } - TEST_F(AdmittanceControllerTest, activate_success) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + ASSERT_EQ( + controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - } TEST_F(AdmittanceControllerTest, update_success) @@ -193,8 +180,9 @@ TEST_F(AdmittanceControllerTest, update_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); broadcast_tfs(); - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(AdmittanceControllerTest, deactivate_success) @@ -216,8 +204,9 @@ TEST_F(AdmittanceControllerTest, reactivate_success) assign_interfaces(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); broadcast_tfs(); - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); } TEST_F(AdmittanceControllerTest, publish_status_success) @@ -228,7 +217,9 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); broadcast_tfs(); - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -243,13 +234,13 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); // Check joint command message - for (auto i = 0ul; i < joint_names_.size(); i++){ + for (auto i = 0ul; i < joint_names_.size(); i++) + { ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); } - } TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) @@ -261,10 +252,13 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); broadcast_tfs(); - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // After first update state, commanded position should be near the start state - for (auto i = 0ul; i < joint_state_values_.size(); i++){ + for (auto i = 0ul; i < joint_state_values_.size(); i++) + { ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); } @@ -277,17 +271,20 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ASSERT_TRUE(controller_->wait_for_commands(executor)); broadcast_tfs(); - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); subscribe_and_get_messages(msg); } -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ ::testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); return result; } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7fd50b4ea0..40e96bd8b1 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -23,19 +23,19 @@ #include "gmock/gmock.h" +#include "6d_robot_description.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "6d_robot_description.hpp" -#include "semantic_components/force_torque_sensor.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "semantic_components/force_torque_sensor.hpp" #include "tf2_ros/transform_broadcaster.h" +#include "trajectory_msgs/msg/joint_trajectory.hpp" // TODO(anyone): replace the state and command message types using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; @@ -48,9 +48,9 @@ namespace const double COMMON_THRESHOLD = 0.001; constexpr auto NODE_SUCCESS = -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) { @@ -63,8 +63,7 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription } // namespace // subclassing and friending so we can access member variables -class TestableAdmittanceController -: public admittance_controller::AdmittanceController +class TestableAdmittanceController : public admittance_controller::AdmittanceController { FRIEND_TEST(AdmittanceControllerTest, joint_names_parameter_not_set); FRIEND_TEST(AdmittanceControllerTest, interface_parameter_not_set); @@ -74,25 +73,23 @@ class TestableAdmittanceController FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); public: - - CallbackReturn on_init() override { get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); - get_node()->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->declare_parameter( + "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); get_node()->set_parameter({"robot_description", robot_description_}); get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); return admittance_controller::AdmittanceController::on_init(); } - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - auto ret = - admittance_controller::AdmittanceController::on_configure(previous_state); + auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) { + if (ret == CallbackReturn::SUCCESS) + { input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); } return ret; @@ -107,11 +104,13 @@ class TestableAdmittanceController */ bool wait_for_commands( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds {500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + bool success = + input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) { + if (success) + { executor.spin_some(); } return success; @@ -129,7 +128,7 @@ class AdmittanceControllerTest : public ::testing::Test public: static void SetUpTestCase() { -// rclcpp::init(0, nullptr); + // rclcpp::init(0, nullptr); } void SetUp() @@ -138,10 +137,11 @@ class AdmittanceControllerTest : public ::testing::Test controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); - force_command_publisher_ = command_publisher_node_->create_publisher( - "/test_admittance_controller/force_commands", rclcpp::SystemDefaultsQoS()); -// pose_command_publisher_ = command_publisher_node_->create_publisher( -// "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + force_command_publisher_ = + command_publisher_node_->create_publisher( + "/test_admittance_controller/force_commands", rclcpp::SystemDefaultsQoS()); + // pose_command_publisher_ = command_publisher_node_->create_publisher( + // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); joint_command_publisher_ = command_publisher_node_->create_publisher( "/test_admittance_controller/joint_commands", rclcpp::SystemDefaultsQoS()); @@ -151,32 +151,33 @@ class AdmittanceControllerTest : public ::testing::Test static void TearDownTestCase() { -// rclcpp::shutdown(); + // rclcpp::shutdown(); } - void TearDown() - { - controller_.reset(nullptr); - } + void TearDown() { controller_.reset(nullptr); } protected: - - controller_interface::return_type SetUpController(const std::string& controller_name, - const std::vector & parameter_overrides) { + controller_interface::return_type SetUpController( + const std::string & controller_name, const std::vector & parameter_overrides) + { auto options = rclcpp::NodeOptions() - .allow_undeclared_parameters(false).parameter_overrides(parameter_overrides) - .automatically_declare_parameters_from_overrides(true); + .allow_undeclared_parameters(false) + .parameter_overrides(parameter_overrides) + .automatically_declare_parameters_from_overrides(true); return SetUpControllerCommon(controller_name, options); } - controller_interface::return_type SetUpController(const std::string& controller_name="test_admittance_controller") { + controller_interface::return_type SetUpController( + const std::string & controller_name = "test_admittance_controller") + { auto options = rclcpp::NodeOptions() - .allow_undeclared_parameters(false) - .automatically_declare_parameters_from_overrides(true); + .allow_undeclared_parameters(false) + .automatically_declare_parameters_from_overrides(true); return SetUpControllerCommon(controller_name, options); } - controller_interface::return_type SetUpControllerCommon(const std::string& controller_name, const rclcpp::NodeOptions& options) + controller_interface::return_type SetUpControllerCommon( + const std::string & controller_name, const rclcpp::NodeOptions & options) { auto result = controller_->init(controller_name, "", options); @@ -184,7 +185,6 @@ class AdmittanceControllerTest : public ::testing::Test assign_interfaces(); return result; - } void assign_interfaces() @@ -193,9 +193,10 @@ class AdmittanceControllerTest : public ::testing::Test command_itfs_.reserve(joint_command_values_.size()); command_ifs.reserve(joint_command_values_.size()); - for (auto i = 0u; i < joint_command_values_.size(); ++i) { - command_itfs_.emplace_back( - hardware_interface::CommandInterface(joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + for (auto i = 0u; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } @@ -207,17 +208,20 @@ class AdmittanceControllerTest : public ::testing::Test state_itfs_.reserve(num_state_ifs); state_ifs.reserve(num_state_ifs); - for (auto i = 0u; i < joint_state_values_.size(); ++i) { - state_itfs_.emplace_back( - hardware_interface::StateInterface(joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + for (auto i = 0u; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } - std::vector fts_itf_names = {"force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"}; + std::vector fts_itf_names = {"force.x", "force.y", "force.z", + "torque.x", "torque.y", "torque.z"}; - for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back( - hardware_interface::StateInterface(ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + for (auto i = 0u; i < fts_state_names_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } @@ -269,13 +273,14 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr){}; - auto subscription = - test_subscription_node_->create_subscription( + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/state", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -290,10 +295,12 @@ class AdmittanceControllerTest : public ::testing::Test auto wait_for_topic = [&](const auto topic_name) { size_t wait_count = 0; - while (command_publisher_node_->count_subscribers(topic_name) == 0) { - if (wait_count >= 5) { + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { auto error_msg = - std::string("publishing to ") + topic_name + " but no node subscribes to it"; + std::string("publishing to ") + topic_name + " but no node subscribes to it"; throw std::runtime_error(error_msg); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -302,11 +309,11 @@ class AdmittanceControllerTest : public ::testing::Test }; // TODO(destogl): comment in when using unified mode -// if (controller_->admittance_->unified_mode_) { -// wait_for_topic(force_command_publisher_->get_topic_name()); -// } + // if (controller_->admittance_->unified_mode_) { + // wait_for_topic(force_command_publisher_->get_topic_name()); + // } -// wait_for_topic(pose_command_publisher_->get_topic_name()); + // wait_for_topic(pose_command_publisher_->get_topic_name()); ControllerCommandWrenchMsg force_msg; force_msg.header.frame_id = sensor_frame_; @@ -328,20 +335,21 @@ class AdmittanceControllerTest : public ::testing::Test pose_msg.pose.orientation.w = 1; // TODO(destogl): comment in when using unified mode -// if (controller_->admittance_->unified_mode_) { -// force_command_publisher_->publish(force_msg); -// } -// pose_command_publisher_->publish(pose_msg); + // if (controller_->admittance_->unified_mode_) { + // force_command_publisher_->publish(force_msg); + // } + // pose_command_publisher_->publish(pose_msg); wait_for_topic(joint_command_publisher_->get_topic_name()); ControllerCommandJointMsg joint_msg; -// joint_msg.joint_names = joint_names_; + // joint_msg.joint_names = joint_names_; trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; auto num_joints = joint_names_.size(); trajectory_point.positions.reserve(num_joints); trajectory_point.velocities.resize(num_joints, 0.0); - for (auto index = 0u; index < num_joints; ++index) { + for (auto index = 0u; index < num_joints; ++index) + { trajectory_point.positions.emplace_back(joint_state_values_[index]); } joint_msg = trajectory_point; @@ -353,7 +361,8 @@ class AdmittanceControllerTest : public ::testing::Test // TODO(anyone): adjust the members as needed // Controller-related parameters - const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + const std::vector joint_names_ = {"joint1", "joint2", "joint3", + "joint4", "joint5", "joint6"}; const std::vector command_interface_types_ = {"position"}; const std::vector state_interface_types_ = {"position"}; const std::string ft_sensor_name_ = "ft_sensor_name"; @@ -363,8 +372,8 @@ class AdmittanceControllerTest : public ::testing::Test const std::string ik_base_frame_ = "base_link"; const std::string ik_tip_frame_ = "tool0"; const std::string ik_group_name_ = "arm"; -// const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; -// const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; const std::string control_frame_ = "tool0"; const std::string endeffector_frame_ = "endeffector_frame"; @@ -373,7 +382,8 @@ class AdmittanceControllerTest : public ::testing::Test std::array admittance_selected_axes_ = {true, true, true, true, true, true}; std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; - std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427 , 2.828427, 2.828427, 2.828427 }; + std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427, + 2.828427, 2.828427, 2.828427}; std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; @@ -388,17 +398,16 @@ class AdmittanceControllerTest : public ::testing::Test std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr force_command_publisher_; -// rclcpp::Publisher::SharedPtr pose_command_publisher_; + // rclcpp::Publisher::SharedPtr pose_command_publisher_; rclcpp::Publisher::SharedPtr joint_command_publisher_; rclcpp::Node::SharedPtr test_subscription_node_; rclcpp::Node::SharedPtr test_broadcaster_node_; }; - // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class AdmittanceControllerTestParameterizedMissingParameters - : public AdmittanceControllerTest, - public ::testing::WithParamInterface +: public AdmittanceControllerTest, + public ::testing::WithParamInterface { public: virtual void SetUp() @@ -408,47 +417,40 @@ class AdmittanceControllerTestParameterizedMissingParameters overrides_ = node->get_node_parameters_interface()->get_parameter_overrides(); } - static void TearDownTestCase() - { - AdmittanceControllerTest::TearDownTestCase(); - } + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } protected: - controller_interface::return_type SetUpController(const std::string& remove_name) + controller_interface::return_type SetUpController(const std::string & remove_name) { std::vector parameter_overrides; - for (const auto& override : overrides_){ - if (override.first != remove_name){ + for (const auto & override : overrides_) + { + if (override.first != remove_name) + { rclcpp::Parameter param(override.first, override.second); parameter_overrides.push_back(param); } } - return AdmittanceControllerTest::SetUpController("test_admittance_controller_no_overrides", parameter_overrides); + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller_no_overrides", parameter_overrides); } std::map overrides_; }; - // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class AdmittanceControllerTestParameterizedInvalidParameters : public AdmittanceControllerTest, -public ::testing::WithParamInterface> + public ::testing::WithParamInterface> { public: - virtual void SetUp() - { - AdmittanceControllerTest::SetUp(); - } + virtual void SetUp() { AdmittanceControllerTest::SetUp(); } - static void TearDownTestCase() - { - AdmittanceControllerTest::TearDownTestCase(); - } + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } protected: - void SetUpController(bool val=false) + void SetUpController(bool val = false) { AdmittanceControllerTest::SetUpController("test_admittance_controller"); } diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index ce535a1df7..1f290aeff6 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -25,26 +25,23 @@ TEST(TestLoadAdmittanceController, load_controller) { - - std::shared_ptr executor = std::make_shared(); - controller_manager::ControllerManager cm(std::make_unique( + controller_manager::ControllerManager cm( + std::make_unique( ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); ASSERT_NO_THROW( - cm.load_controller( - "load_admittance_controller", - "admittance_controller/AdmittanceController")); - + cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController")); } -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ ::testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); return result; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index f7cd5b388b..8c262fd056 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -49,226 +49,226 @@ using namespace std::chrono_literals; // NOLINT namespace rclcpp_action { - template - class ServerGoalHandle; +template +class ServerGoalHandle; } // namespace rclcpp_action namespace rclcpp_lifecycle { - class State; +class State; } // namespace rclcpp_lifecycle namespace joint_trajectory_controller { - class Trajectory; +class Trajectory; - class JointTrajectoryController : public controller_interface::ControllerInterface - { - public: - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - JointTrajectoryController(); +class JointTrajectoryController : public controller_interface::ControllerInterface +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + JointTrajectoryController(); - /** + /** * @brief command_interface_configuration This controller requires the position command * interfaces for the controlled joints */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; - /** + /** * @brief command_interface_configuration This controller requires the position and velocity * state interfaces for the controlled joints */ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_error( - const rclcpp_lifecycle::State & previous_state) override; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State & previous_state) override; - - protected: - std::vector joint_names_; - std::vector command_joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - - // To reduce number of variables and to make the code shorter the interfaces are ordered in types - // as the following constants - const std::vector allowed_interface_types_ = { - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, - hardware_interface::HW_IF_EFFORT, - }; - - // Preallocate variables used in the realtime update() function - trajectory_msgs::msg::JointTrajectoryPoint state_current_; - trajectory_msgs::msg::JointTrajectoryPoint state_desired_; - trajectory_msgs::msg::JointTrajectoryPoint state_error_; - - // Degrees of freedom - size_t dof_; - - // Parameters for some special cases, e.g. hydraulics powered robots - // Run the controller in open-loop, i.e., read hardware states only when starting controller. - // This is useful when robot is not exactly following the commanded trajectory. - bool open_loop_control_ = false; - - trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; - /// Allow integration in goal trajectories to accept goals without position or velocity specified - bool allow_integration_in_goal_trajectories_ = false; - /// Specify interpolation method. Default to splines. - interpolation_methods::InterpolationMethod interpolation_method_{ - interpolation_methods::DEFAULT_INTERPOLATION}; - - double state_publish_rate_; - double action_monitor_rate_; - - // The interfaces are defined as the types in 'allowed_interface_types_' member. - // For convenience, for each type the interfaces are ordered so that i-th position - // matches i-th index in joint_names_ - template - using InterfaceReferences = std::vector>>; - - InterfaceReferences joint_command_interface_; - InterfaceReferences joint_state_interface_; - - bool has_position_state_interface_ = false; - bool has_velocity_state_interface_ = false; - bool has_acceleration_state_interface_ = false; - bool has_position_command_interface_ = false; - bool has_velocity_command_interface_ = false; - bool has_acceleration_command_interface_ = false; - bool has_effort_command_interface_ = false; - - /// If true, a velocity feedforward term plus corrective PID term is used - bool use_closed_loop_pid_adapter_ = false; - using PidPtr = std::shared_ptr; - std::vector pids_; - // Feed-forward velocity weight factor when calculating closed loop pid adapter's command - std::vector ff_velocity_scale_; - // reserved storage for result of the command when closed loop pid adapter is used - std::vector tmp_command_; - - // TODO(karsten1987): eventually activate and deactivate subscriber directly when it's supported - bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr joint_command_subscriber_ = - nullptr; - - std::shared_ptr * traj_point_active_ptr_ = nullptr; - std::shared_ptr traj_external_point_ptr_ = nullptr; - std::shared_ptr traj_home_point_ptr_ = nullptr; - std::shared_ptr traj_msg_home_ptr_ = nullptr; - realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; - - using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; - using StatePublisher = realtime_tools::RealtimePublisher; - using StatePublisherPtr = std::unique_ptr; - rclcpp::Publisher::SharedPtr publisher_; - StatePublisherPtr state_publisher_; - - rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); - rclcpp::Time last_state_publish_time_; - - using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; - using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; - using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; - - rclcpp_action::Server::SharedPtr action_server_; - bool allow_partial_joints_goal_ = false; - RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. - rclcpp::TimerBase::SharedPtr goal_handle_timer_; - rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); - - // joint limits for JTC -// std::vector joint_limits_; - - // callbacks for action_server_ - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::GoalResponse goal_callback( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::CancelResponse cancel_callback( - const std::shared_ptr> goal_handle); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void feedback_setup_callback( - std::shared_ptr> goal_handle); - - // fill trajectory_msg so it matches joints controlled by this controller - // positions set to current position, velocities, accelerations and efforts to 0.0 - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void fill_partial_goal( - std::shared_ptr trajectory_msg) const; - // sorts the joints of the incoming message to our local order - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( - const std::shared_ptr & traj_msg); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const; - - SegmentTolerances default_tolerances_; - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void preempt_active_goal(); - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void set_hold_position(); - - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - bool reset(); - - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; - JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error); - - void read_state_from_hardware(JointTrajectoryPoint & state); - - bool read_state_from_command_interfaces(JointTrajectoryPoint & state); - - private: - bool contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type); - - void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::vector joint_names_; + std::vector command_joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, + hardware_interface::HW_IF_EFFORT, }; + // Preallocate variables used in the realtime update() function + trajectory_msgs::msg::JointTrajectoryPoint state_current_; + trajectory_msgs::msg::JointTrajectoryPoint state_desired_; + trajectory_msgs::msg::JointTrajectoryPoint state_error_; + + // Degrees of freedom + size_t dof_; + + // Parameters for some special cases, e.g. hydraulics powered robots + // Run the controller in open-loop, i.e., read hardware states only when starting controller. + // This is useful when robot is not exactly following the commanded trajectory. + bool open_loop_control_ = false; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + /// Allow integration in goal trajectories to accept goals without position or velocity specified + bool allow_integration_in_goal_trajectories_ = false; + /// Specify interpolation method. Default to splines. + interpolation_methods::InterpolationMethod interpolation_method_{ + interpolation_methods::DEFAULT_INTERPOLATION}; + + double state_publish_rate_; + double action_monitor_rate_; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_state_interface_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + /// If true, a velocity feedforward term plus corrective PID term is used + bool use_closed_loop_pid_adapter_ = false; + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + // reserved storage for result of the command when closed loop pid adapter is used + std::vector tmp_command_; + + // TODO(karsten1987): eventually activate and deactivate subscriber directly when it's supported + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr joint_command_subscriber_ = + nullptr; + + std::shared_ptr * traj_point_active_ptr_ = nullptr; + std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr traj_home_point_ptr_ = nullptr; + std::shared_ptr traj_msg_home_ptr_ = nullptr; + realtime_tools::RealtimeBuffer> + traj_msg_external_point_ptr_; + + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; + using StatePublisher = realtime_tools::RealtimePublisher; + using StatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr publisher_; + StatePublisherPtr state_publisher_; + + rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); + rclcpp::Time last_state_publish_time_; + + using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + rclcpp_action::Server::SharedPtr action_server_; + bool allow_partial_joints_goal_ = false; + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + // joint limits for JTC + // std::vector joint_limits_; + + // callbacks for action_server_ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + rclcpp_action::CancelResponse cancel_callback( + const std::shared_ptr> goal_handle); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void feedback_setup_callback( + std::shared_ptr> goal_handle); + + // fill trajectory_msg so it matches joints controlled by this controller + // positions set to current position, velocities, accelerations and efforts to 0.0 + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void fill_partial_goal( + std::shared_ptr trajectory_msg) const; + // sorts the joints of the incoming message to our local order + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void sort_to_local_joint_order( + std::shared_ptr trajectory_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void add_new_trajectory_msg( + const std::shared_ptr & traj_msg); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + + SegmentTolerances default_tolerances_; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void preempt_active_goal(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void set_hold_position(); + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + bool reset(); + + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void publish_state( + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error); + + void read_state_from_hardware(JointTrajectoryPoint & state); + + bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + +private: + bool contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type); + + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); +}; + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 438ebe46cd..bd8346e964 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -46,1341 +46,1348 @@ namespace joint_trajectory_controller { - JointTrajectoryController::JointTrajectoryController() - : controller_interface::ControllerInterface(), joint_names_({}), dof_(0) +JointTrajectoryController::JointTrajectoryController() +: controller_interface::ControllerInterface(), joint_names_({}), dof_(0) +{ +} + +controller_interface::CallbackReturn JointTrajectoryController::on_init() +{ + try + { + // with the lifecycle node being initialized, we can declare parameters + joint_names_ = auto_declare>("joints", joint_names_); + command_joint_names_ = + auto_declare>("command_joints", command_interface_types_); + command_interface_types_ = + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("state_interfaces", state_interface_types_); + allow_partial_joints_goal_ = + auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); + open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); + allow_integration_in_goal_trajectories_ = auto_declare( + "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); + state_publish_rate_ = auto_declare("state_publish_rate", 50.0); + action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); + + const std::string interpolation_string = auto_declare( + "interpolation_method", interpolation_methods::InterpolationMethodMap.at( + interpolation_methods::DEFAULT_INTERPOLATION)); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + auto_declare("constraints.stopped_velocity_tolerance", 0.01); + auto_declare("constraints.goal_time", 0.0); + } + catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; } - controller_interface::CallbackReturn JointTrajectoryController::on_init() + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +JointTrajectoryController::command_interface_configuration() const +{ + if (dof_ == 0) + { + fprintf( + stderr, + "During ros2_control interface configuration, degrees of freedom is not valid;" + " it should be positive. Actual DOF is %zu\n", + dof_); + std::exit(EXIT_FAILURE); + } + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint_name : command_joint_names_) { - try + for (const auto & interface_type : command_interface_types_) { - // with the lifecycle node being initialized, we can declare parameters - joint_names_ = auto_declare>("joints", joint_names_); - command_joint_names_ = - auto_declare>("command_joints", command_interface_types_); - command_interface_types_ = - auto_declare>("command_interfaces", command_interface_types_); - state_interface_types_ = - auto_declare>("state_interfaces", state_interface_types_); - allow_partial_joints_goal_ = - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); - allow_integration_in_goal_trajectories_ = auto_declare( - "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); - state_publish_rate_ = auto_declare("state_publish_rate", 50.0); - action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); - - const std::string interpolation_string = auto_declare( - "interpolation_method", interpolation_methods::InterpolationMethodMap.at( - interpolation_methods::DEFAULT_INTERPOLATION)); - interpolation_method_ = interpolation_methods::from_string(interpolation_string); - auto_declare("constraints.stopped_velocity_tolerance", 0.01); - auto_declare("constraints.goal_time", 0.0); + conf.names.push_back(joint_name + "/" + interface_type); } - catch (const std::exception & e) + } + return conf; +} + +controller_interface::InterfaceConfiguration +JointTrajectoryController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(dof_ * state_interface_types_.size()); + for (const auto & joint_name : joint_names_) + { + for (const auto & interface_type : state_interface_types_) { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + conf.names.push_back(joint_name + "/" + interface_type); } + } + return conf; +} - return CallbackReturn::SUCCESS; +controller_interface::return_type JointTrajectoryController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + return controller_interface::return_type::OK; } - controller_interface::InterfaceConfiguration - JointTrajectoryController::command_interface_configuration() const + auto compute_error_for_joint = [&]( + JointTrajectoryPoint & error, int index, + const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) { - if (dof_ == 0) + // error defined as the difference between current and desired + if (has_position_state_interface_ && has_position_command_interface_) { - fprintf( - stderr, - "During ros2_control interface configuration, degrees of freedom is not valid;" - " it should be positive. Actual DOF is %zu\n", - dof_); - std::exit(EXIT_FAILURE); + error.positions[index] = + angles::shortest_angular_distance(current.positions[index], desired.positions[index]); } - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : command_joint_names_) + if (has_velocity_state_interface_ && has_velocity_command_interface_) { - for (const auto & interface_type : command_interface_types_) - { - conf.names.push_back(joint_name + "/" + interface_type); - } + error.velocities[index] = desired.velocities[index] - current.velocities[index]; + } + if (has_acceleration_state_interface_ && has_acceleration_command_interface_) + { + error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; } - return conf; + }; + + // Check if a new external message has been received from nonRT threads + auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); + auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + if (current_external_msg != *new_external_msg) + { + fill_partial_goal(*new_external_msg); + sort_to_local_joint_order(*new_external_msg); + // TODO(denis): Add here integration of position and velocity + traj_external_point_ptr_->update(*new_external_msg); } - controller_interface::InterfaceConfiguration - JointTrajectoryController::state_interface_configuration() const + // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not + // changed, but its value only? + auto assign_interface_from_point = + [&](auto & joint_interface, const std::vector & trajectory_point_interface) { - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(dof_ * state_interface_types_.size()); - for (const auto & joint_name : joint_names_) + for (size_t index = 0; index < dof_; ++index) { - for (const auto & interface_type : state_interface_types_) - { - conf.names.push_back(joint_name + "/" + interface_type); - } + joint_interface[index].get().set_value(trajectory_point_interface[index]); } - return conf; - } + }; + + // current state update + state_current_.time_from_start.set__sec(0); + read_state_from_hardware(state_current_); - controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & time, const rclcpp::Duration & period) + // currently carrying out a trajectory + if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) { - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + bool first_sample = false; + // if sampling the first time, set the point before you sample + if (!(*traj_point_active_ptr_)->is_sampled_already()) { - return controller_interface::return_type::OK; - } + first_sample = true; - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) { - // error defined as the difference between current and desired - if (has_position_state_interface_ && has_position_command_interface_) + if (open_loop_control_) { - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); } - if (has_velocity_state_interface_ && has_velocity_command_interface_) - { - error.velocities[index] = desired.velocities[index] - current.velocities[index]; - } - if (has_acceleration_state_interface_ && has_acceleration_command_interface_) + else { - error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; + (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); } - }; - - // Check if a new external message has been received from nonRT threads - auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); - if (current_external_msg != *new_external_msg) - { - fill_partial_goal(*new_external_msg); - sort_to_local_joint_order(*new_external_msg); - // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; + // find segment for current timestamp + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + const bool valid_point = + (*traj_point_active_ptr_) + ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); - // current state update - state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); - - // currently carrying out a trajectory - if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) + if (!valid_point) { - bool first_sample = false; - // if sampling the first time, set the point before you sample - if (!(*traj_point_active_ptr_)->is_sampled_already()) - { - first_sample = true; - + return controller_interface::return_type::ERROR; + } - if (open_loop_control_) - { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); - } - else - { - (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, state_current_); - } - } + bool tolerance_violated_while_moving = false; + bool outside_goal_tolerance = false; + bool within_goal_time = true; + double time_difference = 0.0; + const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = - (*traj_point_active_ptr_) - ->sample(time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + // Check state/goal tolerance + for (size_t index = 0; index < dof_; ++index) + { + compute_error_for_joint(state_error_, index, state_current_, state_desired_); - if (!valid_point) + // Always check the state tolerance on the first sample in case the first sample + // is the last point + if ( + (before_last_point || first_sample) && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.state_tolerance[index], false)) { - return controller_interface::return_type::ERROR; + tolerance_violated_while_moving = true; } - - bool tolerance_violated_while_moving = false; - bool outside_goal_tolerance = false; - bool within_goal_time = true; - double time_difference = 0.0; - const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); - - // Check state/goal tolerance - for (size_t index = 0; index < dof_; ++index) + // past the final point, check that we end up inside goal tolerance + if ( + !before_last_point && + !check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) { - compute_error_for_joint(state_error_, index, state_current_, state_desired_); - - // Always check the state tolerance on the first sample in case the first sample - // is the last point - if ( - (before_last_point || first_sample) && - !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) - { - tolerance_violated_while_moving = true; - } - // past the final point, check that we end up inside goal tolerance - if ( - !before_last_point && - !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) - { - outside_goal_tolerance = true; + outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) - { - // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; + if (default_tolerances_.goal_time_tolerance != 0.0) + { + // if we exceed goal_time_tolerance set it to aborted + const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); + const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - time_difference = get_node()->now().seconds() - traj_end.seconds(); + time_difference = get_node()->now().seconds() - traj_end.seconds(); - if (time_difference > default_tolerances_.goal_time_tolerance) - { - within_goal_time = false; - } + if (time_difference > default_tolerances_.goal_time_tolerance) + { + within_goal_time = false; } } } + } - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) { - if (use_closed_loop_pid_adapter_) + // Update PIDs + for (auto i = 0ul; i < dof_; ++i) { - // Update PIDs - for (auto i = 0ul; i < dof_; ++i) - { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_desired_.positions[i] - state_current_.positions[i], - state_desired_.velocities[i] - state_current_.velocities[i], - (uint64_t)period.nanoseconds()); - } + tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_desired_.positions[i] - state_current_.positions[i], + state_desired_.velocities[i] - state_current_.velocities[i], + (uint64_t)period.nanoseconds()); } + } - // set values for next hardware write() - if (has_position_command_interface_) + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (use_closed_loop_pid_adapter_) { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + assign_interface_from_point(joint_command_interface_[1], tmp_command_); } - if (has_velocity_command_interface_) + else { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[1], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); - } + assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); } - if (has_acceleration_command_interface_) + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter_) { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + assign_interface_from_point(joint_command_interface_[3], tmp_command_); } - if (has_effort_command_interface_) + else { - if (use_closed_loop_pid_adapter_) - { - assign_interface_from_point(joint_command_interface_[3], tmp_command_); - } - else - { - assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); - } + assign_interface_from_point(joint_command_interface_[3], state_desired_.effort); } - - // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; } - const auto active_goal = *rt_active_goal_.readFromRT(); - if (active_goal) + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } + + const auto active_goal = *rt_active_goal_.readFromRT(); + if (active_goal) + { + // send feedback + auto feedback = std::make_shared(); + feedback->header.stamp = time; + feedback->joint_names = joint_names_; + + feedback->actual = state_current_; + feedback->desired = state_desired_; + feedback->error = state_error_; + active_goal->setFeedback(feedback); + + // check abort + if (tolerance_violated_while_moving) + { + set_hold_position(); + auto result = std::make_shared(); + RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + + // check goal tolerance + } + else if (!before_last_point) { - // send feedback - auto feedback = std::make_shared(); - feedback->header.stamp = time; - feedback->joint_names = joint_names_; - - feedback->actual = state_current_; - feedback->desired = state_desired_; - feedback->error = state_error_; - active_goal->setFeedback(feedback); - - // check abort - if (tolerance_violated_while_moving) + if (!outside_goal_tolerance) { - set_hold_position(); - auto result = std::make_shared(); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); - active_goal->setAborted(result); + auto res = std::make_shared(); + res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + active_goal->setSucceeded(res); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - // check goal tolerance + RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); } - else if (!before_last_point) + else if (!within_goal_time) { - if (!outside_goal_tolerance) - { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - } - else if (!within_goal_time) - { - set_hold_position(); - auto result = std::make_shared(); - result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); - active_goal->setAborted(result); - // TODO(matthew-reynolds): Need a lock-free write here - // See https://github.com/ros-controls/ros2_controllers/issues/168 - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); - } - // else, run another cycle while waiting for outside_goal_tolerance - // to be satisfied or violated within the goal_time_tolerance + set_hold_position(); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + active_goal->setAborted(result); + // TODO(matthew-reynolds): Need a lock-free write here + // See https://github.com/ros-controls/ros2_controllers/issues/168 + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + RCLCPP_WARN( + get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", + time_difference); } + // else, run another cycle while waiting for outside_goal_tolerance + // to be satisfied or violated within the goal_time_tolerance } } - - publish_state(state_desired_, state_current_, state_error_); - return controller_interface::return_type::OK; } - void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) + publish_state(state_desired_, state_current_, state_error_); + return controller_interface::return_type::OK; +} + +void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +{ + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; - // Assign values from the hardware - // Position states always exist - assign_point_from_interface(state.positions, joint_state_interface_[0]); - // velocity and acceleration states are optional - if (has_velocity_state_interface_) + // Assign values from the hardware + // Position states always exist + assign_point_from_interface(state.positions, joint_state_interface_[0]); + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + assign_point_from_interface(state.velocities, joint_state_interface_[1]); + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) { - assign_point_from_interface(state.velocities, joint_state_interface_[1]); - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) - { - assign_point_from_interface(state.accelerations, joint_state_interface_[2]); - } - else - { - // Make empty so the property is ignored during interpolation - state.accelerations.clear(); - } + assign_point_from_interface(state.accelerations, joint_state_interface_[2]); } else { // Make empty so the property is ignored during interpolation - state.velocities.clear(); state.accelerations.clear(); } } - - bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) + else { - bool has_values = true; - - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + // Make empty so the property is ignored during interpolation + state.velocities.clear(); + state.accelerations.clear(); + } +} - auto interface_has_values = [](const auto & joint_interface) { - return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { - return std::isnan(interface.get().get_value()); - }) == joint_interface.end(); - }; +bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) +{ + bool has_values = true; - // Assign values from the command interfaces as state. Therefore needs check for both. - // Position state interface has to exist always - if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) - { - assign_point_from_interface(state.positions, joint_command_interface_[0]); - } - else + auto assign_point_from_interface = + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) { - state.positions.clear(); - has_values = false; + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } - // velocity and acceleration states are optional - if (has_velocity_state_interface_) + }; + + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + }; + + // Assign values from the command interfaces as state. Therefore needs check for both. + // Position state interface has to exist always + if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0])) + { + assign_point_from_interface(state.positions, joint_command_interface_[0]); + } + else + { + state.positions.clear(); + has_values = false; + } + // velocity and acceleration states are optional + if (has_velocity_state_interface_) + { + if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) { - if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) - { - assign_point_from_interface(state.velocities, joint_command_interface_[1]); - } - else - { - state.velocities.clear(); - has_values = false; - } + assign_point_from_interface(state.velocities, joint_command_interface_[1]); } else { state.velocities.clear(); + has_values = false; } - // Acceleration is used only in combination with velocity - if (has_acceleration_state_interface_) + } + else + { + state.velocities.clear(); + } + // Acceleration is used only in combination with velocity + if (has_acceleration_state_interface_) + { + if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) { - if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2])) - { - assign_point_from_interface(state.accelerations, joint_command_interface_[2]); - } - else - { - state.accelerations.clear(); - has_values = false; - } + assign_point_from_interface(state.accelerations, joint_command_interface_[2]); } else { state.accelerations.clear(); + has_values = false; } + } + else + { + state.accelerations.clear(); + } + + return has_values; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); - return has_values; + // update parameters + joint_names_ = get_node()->get_parameter("joints").as_string_array(); + if ((dof_ > 0) && (joint_names_.size() != dof_)) + { + RCLCPP_ERROR( + logger, + "The JointTrajectoryController does not support restarting with a different number of DOF"); + // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we + // can continue + return CallbackReturn::FAILURE; } - controller_interface::CallbackReturn JointTrajectoryController::on_configure( - const rclcpp_lifecycle::State &) + dof_ = joint_names_.size(); + + // TODO(destogl): why is this here? Add comment or move + if (!reset()) { - const auto logger = get_node()->get_logger(); + return CallbackReturn::FAILURE; + } - // update parameters - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - if ((dof_ > 0) && (joint_names_.size() != dof_)) - { - RCLCPP_ERROR( - logger, - "The JointTrajectoryController does not support restarting with a different number of DOF"); - // TODO(andyz): update vector lengths if num. joints did change and re-initialize them so we - // can continue - return CallbackReturn::FAILURE; - } + if (joint_names_.empty()) + { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? + RCLCPP_WARN(logger, "'joints' parameter is empty."); + } - dof_ = joint_names_.size(); + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); - // TODO(destogl): why is this here? Add comment or move - if (!reset()) - { - return CallbackReturn::FAILURE; - } + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + + // Specialized, child controllers set interfaces before calling configure function. + if (command_interface_types_.empty()) + { + command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); + } + + if (command_interface_types_.empty()) + { + RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); + return CallbackReturn::FAILURE; + } - if (joint_names_.empty()) + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : command_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) { - // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? - RCLCPP_WARN(logger, "'joints' parameter is empty."); + RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; } + } - command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); - - if (command_joint_names_.empty()) + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 2. position [velocity, [acceleration]] + + has_position_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); + + if (has_velocity_command_interface_) + { + // if there is only velocity then use also PID adapter + if (command_interface_types_.size() == 1) { - command_joint_names_ = joint_names_; - RCLCPP_INFO( - logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + use_closed_loop_pid_adapter_ = true; } - else if (command_joint_names_.size() != joint_names_.size()) + else if (!has_position_command_interface_) { RCLCPP_ERROR( - logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + logger, + "'velocity' command interface can be used either alone or 'position' " + "interface has to be present."); return CallbackReturn::FAILURE; } + // invalid: acceleration is defined and no velocity + } + else if (has_acceleration_command_interface_) + { + RCLCPP_ERROR( + logger, + "'acceleration' command interface can only be used if 'velocity' and " + "'position' interfaces are present"); + return CallbackReturn::FAILURE; + } - // Specialized, child controllers set interfaces before calling configure function. - if (command_interface_types_.empty()) + // effort can't be combined with other interfaces + if (has_effort_command_interface_) + { + if (command_interface_types_.size() == 1) { - command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); + use_closed_loop_pid_adapter_ = true; } - - if (command_interface_types_.empty()) + else { - RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); + RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); return CallbackReturn::FAILURE; } + } - // Check if only allowed interface types are used and initialize storage to avoid memory - // allocation during activation - joint_command_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } + if (use_closed_loop_pid_adapter_) + { + pids_.resize(dof_); + ff_velocity_scale_.resize(dof_); + tmp_command_.resize(dof_, 0.0); + + // Init PID gains from ROS parameter server + for (size_t i = 0; i < pids_.size(); ++i) + { + const std::string prefix = "gains." + command_joint_names_[i]; + const auto k_p = auto_declare(prefix + ".p", 0.0); + const auto k_i = auto_declare(prefix + ".i", 0.0); + const auto k_d = auto_declare(prefix + ".d", 0.0); + const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); + ff_velocity_scale_[i] = + auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); + // Initialize PID + pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); } + } - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 2. position [velocity, [acceleration]] + // Read always state interfaces from the parameter because they can be used + // independently from the controller's type. + // Specialized, child controllers should set its default value. + state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); - has_position_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); - has_velocity_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); - has_acceleration_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); - has_effort_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); + if (state_interface_types_.empty()) + { + RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); + return CallbackReturn::FAILURE; + } - if (has_velocity_command_interface_) - { - // if there is only velocity then use also PID adapter - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter_ = true; - } - else if (!has_position_command_interface_) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can be used either alone or 'position' " - "interface has to be present."); - return CallbackReturn::FAILURE; - } - // invalid: acceleration is defined and no velocity - } - else if (has_acceleration_command_interface_) + if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) + { + RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); + return CallbackReturn::FAILURE; + } + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + // Note: 'effort' storage is also here, but never used. Still, for this is OK. + joint_state_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : state_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) { - RCLCPP_ERROR( - logger, - "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); + RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); return CallbackReturn::FAILURE; } + } - // effort can't be combined with other interfaces - if (has_effort_command_interface_) - { - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter_ = true; - } - else - { - RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); - return CallbackReturn::FAILURE; - } - } + has_position_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); - if (use_closed_loop_pid_adapter_) + if (has_velocity_state_interface_) + { + if (!has_position_state_interface_) { - pids_.resize(dof_); - ff_velocity_scale_.resize(dof_); - tmp_command_.resize(dof_, 0.0); - - // Init PID gains from ROS parameter server - for (size_t i = 0; i < pids_.size(); ++i) - { - const std::string prefix = "gains." + command_joint_names_[i]; - const auto k_p = auto_declare(prefix + ".p", 0.0); - const auto k_i = auto_declare(prefix + ".i", 0.0); - const auto k_d = auto_declare(prefix + ".d", 0.0); - const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = - auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); - // Initialize PID - pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); - } + RCLCPP_ERROR( + logger, + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + return CallbackReturn::FAILURE; } - - - // Read always state interfaces from the parameter because they can be used - // independently from the controller's type. - // Specialized, child controllers should set its default value. - state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); - - if (state_interface_types_.empty()) + } + else + { + if (has_acceleration_state_interface_) { - RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); + RCLCPP_ERROR( + logger, + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); return CallbackReturn::FAILURE; } - - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) + if (has_velocity_command_interface_ && command_interface_types_.size() == 1) { - RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); return CallbackReturn::FAILURE; } - - // Check if only allowed interface types are used and initialize storage to avoid memory - // allocation during activation - // Note: 'effort' storage is also here, but never used. Still, for this is OK. - joint_state_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : state_interface_types_) + // effort is always used alone so no need for size check + if (has_effort_command_interface_) { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; } + } - has_position_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); - has_velocity_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); - has_acceleration_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); - - if (has_velocity_state_interface_) + auto get_interface_list = [](const std::vector & interface_types) + { + std::stringstream ss_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) { - if (!has_position_state_interface_) + if (index != 0) { - RCLCPP_ERROR( - logger, - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - return CallbackReturn::FAILURE; + ss_interfaces << " "; } + ss_interfaces << interface_types[index]; + } + return ss_interfaces.str(); + }; + + // Print output so users can be sure the interface setup is correct + RCLCPP_INFO( + logger, "Command interfaces are [%s] and state interfaces are [%s].", + get_interface_list(command_interface_types_).c_str(), + get_interface_list(state_interface_types_).c_str()); + + default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); + + // Read parameters customizing controller for special cases + open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); + allow_integration_in_goal_trajectories_ = + get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + + const std::string interpolation_string = + get_node()->get_parameter("interpolation_method").as_string(); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + RCLCPP_INFO( + logger, "Using '%s' interpolation method.", + interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + + // subscriber callback + // non realtime + // TODO(karsten): check if traj msg and point time are valid + auto callback = [this](const std::shared_ptr msg) -> void + { + if (!validate_trajectory_msg(*msg)) + { + return; } - else + + // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement + // always replace old msg with new one for now + if (subscriber_is_active_) { - if (has_acceleration_state_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; - } - if (has_velocity_command_interface_ && command_interface_types_.size() == 1) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; - } - // effort is always used alone so no need for size check - if (has_effort_command_interface_) - { - RCLCPP_ERROR( - logger, - "'effort' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; - } + add_new_trajectory_msg(msg); } + }; - auto get_interface_list = [](const std::vector & interface_types) { - std::stringstream ss_interfaces; - for (size_t index = 0; index < interface_types.size(); ++index) - { - if (index != 0) - { - ss_interfaces << " "; - } - ss_interfaces << interface_types[index]; - } - return ss_interfaces.str(); - }; + // TODO(karsten1987): create subscriber with subscription deactivated + joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); - // Print output so users can be sure the interface setup is correct - RCLCPP_INFO( - logger, "Command interfaces are [%s] and state interfaces are [%s].", - get_interface_list(command_interface_types_).c_str(), - get_interface_list(state_interface_types_).c_str()); + // TODO(karsten1987): no lifecycle for subscriber yet + // joint_command_subscriber_->on_activate(); - default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); + // State publisher + state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); + if (state_publish_rate_ > 0.0) + { + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); + } + else + { + state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); + } - // Read parameters customizing controller for special cases - open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); - allow_integration_in_goal_trajectories_ = - get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(publisher_); + + state_publisher_->lock(); + state_publisher_->msg_.joint_names = command_joint_names_; + state_publisher_->msg_.desired.positions.resize(dof_); + state_publisher_->msg_.desired.velocities.resize(dof_); + state_publisher_->msg_.desired.accelerations.resize(dof_); + state_publisher_->msg_.actual.positions.resize(dof_); + state_publisher_->msg_.error.positions.resize(dof_); + if (has_velocity_state_interface_) + { + state_publisher_->msg_.actual.velocities.resize(dof_); + state_publisher_->msg_.error.velocities.resize(dof_); + } + if (has_acceleration_state_interface_) + { + state_publisher_->msg_.actual.accelerations.resize(dof_); + state_publisher_->msg_.error.accelerations.resize(dof_); + } + state_publisher_->unlock(); - const std::string interpolation_string = - get_node()->get_parameter("interpolation_method").as_string(); - interpolation_method_ = interpolation_methods::from_string(interpolation_string); - RCLCPP_INFO( - logger, "Using '%s' interpolation method.", - interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); - - // subscriber callback - // non realtime - // TODO(karsten): check if traj msg and point time are valid - auto callback = [this](const std::shared_ptr msg) -> void { - if (!validate_trajectory_msg(*msg)) - { - return; - } + last_state_publish_time_ = get_node()->now(); - // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement - // always replace old msg with new one for now - if (subscriber_is_active_) - { - add_new_trajectory_msg(msg); - } - }; + // action server configuration + allow_partial_joints_goal_ = + get_node()->get_parameter("allow_partial_joints_goal").get_value(); + if (allow_partial_joints_goal_) + { + RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); + } - // TODO(karsten1987): create subscriber with subscription deactivated - joint_command_subscriber_ = - get_node()->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); + action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); + RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); - // TODO(karsten1987): no lifecycle for subscriber yet - // joint_command_subscriber_->on_activate(); + using namespace std::placeholders; + action_server_ = rclcpp_action::create_server( + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string(get_node()->get_name()) + "/follow_joint_trajectory", + std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), + std::bind(&JointTrajectoryController::cancel_callback, this, _1), + std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); - // State publisher - state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); - if (state_publish_rate_ > 0.0) - { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); - } - else - { - state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); - } + RCLCPP_ERROR(get_node()->get_logger(), "configure called!!!"); - publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = std::make_unique(publisher_); - - state_publisher_->lock(); - state_publisher_->msg_.joint_names = command_joint_names_; - state_publisher_->msg_.desired.positions.resize(dof_); - state_publisher_->msg_.desired.velocities.resize(dof_); - state_publisher_->msg_.desired.accelerations.resize(dof_); - state_publisher_->msg_.actual.positions.resize(dof_); - state_publisher_->msg_.error.positions.resize(dof_); - if (has_velocity_state_interface_) - { - state_publisher_->msg_.actual.velocities.resize(dof_); - state_publisher_->msg_.error.velocities.resize(dof_); - } - if (has_acceleration_state_interface_) + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_activate( + const rclcpp_lifecycle::State &) +{ + // order all joints in the storage + for (const auto & interface : command_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { - state_publisher_->msg_.actual.accelerations.resize(dof_); - state_publisher_->msg_.error.accelerations.resize(dof_); + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; } - state_publisher_->unlock(); - - last_state_publish_time_ = get_node()->now(); - - // action server configuration - allow_partial_joints_goal_ = - get_node()->get_parameter("allow_partial_joints_goal").get_value(); - if (allow_partial_joints_goal_) + } + for (const auto & interface : state_interface_types_) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { - RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + interface.c_str(), joint_state_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; } + } - action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); - RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); - - using namespace std::placeholders; - action_server_ = rclcpp_action::create_server( - get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), - get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), - std::string(get_node()->get_name()) + "/follow_joint_trajectory", - std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), - std::bind(&JointTrajectoryController::cancel_callback, this, _1), - std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); - - RCLCPP_ERROR(get_node()->get_logger(), "configure called!!!"); + // Store 'home' pose + traj_msg_home_ptr_ = std::make_shared(); + traj_msg_home_ptr_->header.stamp.sec = 0; + traj_msg_home_ptr_->header.stamp.nanosec = 0; + traj_msg_home_ptr_->points.resize(1); + traj_msg_home_ptr_->points[0].time_from_start.sec = 0; + traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; + traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); + for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) + { + traj_msg_home_ptr_->points[0].positions[index] = + joint_state_interface_[0][index].get().get_value(); + } - return CallbackReturn::SUCCESS; + traj_external_point_ptr_ = std::make_shared(); + traj_home_point_ptr_ = std::make_shared(); + traj_msg_external_point_ptr_.writeFromNonRT( + std::shared_ptr()); + + subscriber_is_active_ = true; + traj_point_active_ptr_ = &traj_external_point_ptr_; + last_state_publish_time_ = get_node()->now(); + + // Initialize current state storage if hardware state has tracking offset + resize_joint_trajectory_point(state_current_, dof_); + resize_joint_trajectory_point(state_desired_, dof_); + resize_joint_trajectory_point(state_error_, dof_); + resize_joint_trajectory_point(last_commanded_state_, dof_); + + RCLCPP_ERROR(get_node()->get_logger(), "activate called!!!"); + read_state_from_hardware(state_current_); + read_state_from_hardware(state_desired_); + read_state_from_hardware(last_commanded_state_); + // Handle restart of controller by reading from commands if + // those are not nan + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + if (read_state_from_command_interfaces(state)) + { + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; } - controller_interface::CallbackReturn JointTrajectoryController::on_activate( - const rclcpp_lifecycle::State &) + // TODO(karsten1987): activate subscriptions of subscriber + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + // TODO(anyone): How to halt when using effort commands? + for (size_t index = 0; index < dof_; ++index) { - // order all joints in the storage - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); - if (!controller_interface::get_ordered_interfaces( - command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); - return controller_interface::CallbackReturn::ERROR; - } - } - for (const auto & interface : state_interface_types_) + if (has_position_command_interface_) { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); - if (!controller_interface::get_ordered_interfaces( - state_interfaces_, joint_names_, interface, joint_state_interface_[index])) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); - return controller_interface::CallbackReturn::ERROR; - } + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); } - // Store 'home' pose - traj_msg_home_ptr_ = std::make_shared(); - traj_msg_home_ptr_->header.stamp.sec = 0; - traj_msg_home_ptr_->header.stamp.nanosec = 0; - traj_msg_home_ptr_->points.resize(1); - traj_msg_home_ptr_->points[0].time_from_start.sec = 0; - traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000; - traj_msg_home_ptr_->points[0].positions.resize(joint_state_interface_[0].size()); - for (size_t index = 0; index < joint_state_interface_[0].size(); ++index) + if (has_velocity_command_interface_) { - traj_msg_home_ptr_->points[0].positions[index] = - joint_state_interface_[0][index].get().get_value(); + joint_command_interface_[1][index].get().set_value(0.0); } - traj_external_point_ptr_ = std::make_shared(); - traj_home_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); - - subscriber_is_active_ = true; - traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = get_node()->now(); - - // Initialize current state storage if hardware state has tracking offset - resize_joint_trajectory_point(state_current_, dof_); - resize_joint_trajectory_point(state_desired_, dof_); - resize_joint_trajectory_point(state_error_, dof_); - resize_joint_trajectory_point(last_commanded_state_, dof_); - - RCLCPP_ERROR(get_node()->get_logger(), "activate called!!!"); - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading from commands if - // those are not nan - trajectory_msgs::msg::JointTrajectoryPoint state; - resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if (has_effort_command_interface_) { - state_current_ = state; - state_desired_ = state; - last_commanded_state_ = state; + joint_command_interface_[3][index].get().set_value(0.0); } - - // TODO(karsten1987): activate subscriptions of subscriber - return CallbackReturn::SUCCESS; } - controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( - const rclcpp_lifecycle::State &) + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) { - // TODO(anyone): How to halt when using effort commands? - for (size_t index = 0; index < dof_; ++index) - { - if (has_position_command_interface_) - { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); - } + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); - if (has_velocity_command_interface_) - { - joint_command_interface_[1][index].get().set_value(0.0); - } + subscriber_is_active_ = false; - if (has_effort_command_interface_) - { - joint_command_interface_[3][index].get().set_value(0.0); - } - } + return CallbackReturn::SUCCESS; +} - for (size_t index = 0; index < allowed_interface_types_.size(); ++index) - { - joint_command_interface_[index].clear(); - joint_state_interface_[index].clear(); - } - release_interfaces(); +controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + // go home + traj_home_point_ptr_->update(traj_msg_home_ptr_); + traj_point_active_ptr_ = &traj_home_point_ptr_; - subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} - return CallbackReturn::SUCCESS; +controller_interface::CallbackReturn JointTrajectoryController::on_error( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; } + return CallbackReturn::SUCCESS; +} - controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( - const rclcpp_lifecycle::State &) - { - // go home - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; +bool JointTrajectoryController::reset() +{ + subscriber_is_active_ = false; + joint_command_subscriber_.reset(); - return CallbackReturn::SUCCESS; + for (const auto & pid : pids_) + { + pid->reset(); } - controller_interface::CallbackReturn JointTrajectoryController::on_error( - const rclcpp_lifecycle::State &) + // iterator has no default value + // prev_traj_point_ptr_; + traj_point_active_ptr_ = nullptr; + traj_external_point_ptr_.reset(); + traj_home_point_ptr_.reset(); + traj_msg_home_ptr_.reset(); + + // reset pids + for (const auto & pid : pids_) { - if (!reset()) - { - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; + pid->reset(); } - bool JointTrajectoryController::reset() - { - subscriber_is_active_ = false; - joint_command_subscriber_.reset(); + return true; +} - for (const auto & pid : pids_) - { - pid->reset(); - } - - // iterator has no default value - // prev_traj_point_ptr_; - traj_point_active_ptr_ = nullptr; - traj_external_point_ptr_.reset(); - traj_home_point_ptr_.reset(); - traj_msg_home_ptr_.reset(); +controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( + const rclcpp_lifecycle::State &) +{ + // TODO(karsten1987): what to do? - // reset pids - for (const auto & pid : pids_) - { - pid->reset(); - } + return CallbackReturn::SUCCESS; +} - return true; +void JointTrajectoryController::publish_state( + const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, + const JointTrajectoryPoint & state_error) +{ + if (state_publisher_period_.seconds() <= 0.0) + { + return; } - controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( - const rclcpp_lifecycle::State &) + if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) { - // TODO(karsten1987): what to do? - - return CallbackReturn::SUCCESS; + return; } - void JointTrajectoryController::publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error) + if (state_publisher_ && state_publisher_->trylock()) { - if (state_publisher_period_.seconds() <= 0.0) + last_state_publish_time_ = get_node()->now(); + state_publisher_->msg_.header.stamp = last_state_publish_time_; + state_publisher_->msg_.desired.positions = desired_state.positions; + state_publisher_->msg_.desired.velocities = desired_state.velocities; + state_publisher_->msg_.desired.accelerations = desired_state.accelerations; + state_publisher_->msg_.actual.positions = current_state.positions; + state_publisher_->msg_.error.positions = state_error.positions; + if (has_velocity_state_interface_) { - return; + state_publisher_->msg_.actual.velocities = current_state.velocities; + state_publisher_->msg_.error.velocities = state_error.velocities; } - - if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) + if (has_acceleration_state_interface_) { - return; + state_publisher_->msg_.actual.accelerations = current_state.accelerations; + state_publisher_->msg_.error.accelerations = state_error.accelerations; } - if (state_publisher_ && state_publisher_->trylock()) - { - last_state_publish_time_ = get_node()->now(); - state_publisher_->msg_.header.stamp = last_state_publish_time_; - state_publisher_->msg_.desired.positions = desired_state.positions; - state_publisher_->msg_.desired.velocities = desired_state.velocities; - state_publisher_->msg_.desired.accelerations = desired_state.accelerations; - state_publisher_->msg_.actual.positions = current_state.positions; - state_publisher_->msg_.error.positions = state_error.positions; - if (has_velocity_state_interface_) - { - state_publisher_->msg_.actual.velocities = current_state.velocities; - state_publisher_->msg_.error.velocities = state_error.velocities; - } - if (has_acceleration_state_interface_) - { - state_publisher_->msg_.actual.accelerations = current_state.accelerations; - state_publisher_->msg_.error.accelerations = state_error.accelerations; - } - - state_publisher_->unlockAndPublish(); - } + state_publisher_->unlockAndPublish(); } +} + +rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); - rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( - const rclcpp_action::GoalUUID &, std::shared_ptr goal) + // Precondition: Running controller + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); + RCLCPP_ERROR( + get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } - // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); - return rclcpp_action::GoalResponse::REJECT; - } + if (!validate_trajectory_msg(goal->trajectory)) + { + return rclcpp_action::GoalResponse::REJECT; + } - if (!validate_trajectory_msg(goal->trajectory)) - { - return rclcpp_action::GoalResponse::REJECT; - } + RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} - RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } +rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); - rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( - const std::shared_ptr> goal_handle) + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { - RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); - - // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal && active_goal->gh_ == goal_handle) - { - // Controller uptime - // Enter hold current position mode - set_hold_position(); + // Controller uptime + // Enter hold current position mode + set_hold_position(); - RCLCPP_DEBUG( - get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + RCLCPP_DEBUG( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); - // Mark the current goal as canceled - auto action_res = std::make_shared(); - active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } - return rclcpp_action::CancelResponse::ACCEPT; + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } + return rclcpp_action::CancelResponse::ACCEPT; +} - void JointTrajectoryController::feedback_setup_callback( - std::shared_ptr> goal_handle) +void JointTrajectoryController::feedback_setup_callback( + std::shared_ptr> goal_handle) +{ + // Update new trajectory { - // Update new trajectory - { - preempt_active_goal(); - auto traj_msg = - std::make_shared(goal_handle->get_goal()->trajectory); + preempt_active_goal(); + auto traj_msg = + std::make_shared(goal_handle->get_goal()->trajectory); - add_new_trajectory_msg(traj_msg); - } + add_new_trajectory_msg(traj_msg); + } - // Update the active goal - RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); - rt_goal->preallocated_feedback_->joint_names = joint_names_; - rt_goal->execute(); - rt_active_goal_.writeFromNonRT(rt_goal); + // Update the active goal + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->preallocated_feedback_->joint_names = joint_names_; + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); - // Setup goal status checking timer - goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); - } + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} - void JointTrajectoryController::fill_partial_goal( - std::shared_ptr trajectory_msg) const +void JointTrajectoryController::fill_partial_goal( + std::shared_ptr trajectory_msg) const +{ + // joint names in the goal are a subset of existing joints, as checked in goal_callback + // so if the size matches, the goal contains all controller joints + if (dof_ == trajectory_msg->joint_names.size()) { - // joint names in the goal are a subset of existing joints, as checked in goal_callback - // so if the size matches, the goal contains all controller joints - if (dof_ == trajectory_msg->joint_names.size()) - { - return; - } + return; + } - trajectory_msg->joint_names.reserve(dof_); + trajectory_msg->joint_names.reserve(dof_); - for (size_t index = 0; index < dof_; ++index) + for (size_t index = 0; index < dof_; ++index) + { { + if ( + std::find( + trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), + joint_names_[index]) != trajectory_msg->joint_names.end()) { - if ( - std::find( - trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), - joint_names_[index]) != trajectory_msg->joint_names.end()) - { - // joint found on msg - continue; - } - trajectory_msg->joint_names.push_back(joint_names_[index]); + // joint found on msg + continue; + } + trajectory_msg->joint_names.push_back(joint_names_[index]); - for (auto & it : trajectory_msg->points) + for (auto & it : trajectory_msg->points) + { + // Assume hold position with 0 velocity and acceleration for missing joints + if (!it.positions.empty()) { - // Assume hold position with 0 velocity and acceleration for missing joints - if (!it.positions.empty()) - { - if ( - has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) - { - // copy last command if cmd interface exists - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); - } - else if (has_position_state_interface_) - { - // copy current state if state interface exists - it.positions.push_back(joint_state_interface_[0][index].get().get_value()); - } - } - if (!it.velocities.empty()) + if ( + has_position_command_interface_ && + !std::isnan(joint_command_interface_[0][index].get().get_value())) { - it.velocities.push_back(0.0); + // copy last command if cmd interface exists + it.positions.push_back(joint_command_interface_[0][index].get().get_value()); } - if (!it.accelerations.empty()) + else if (has_position_state_interface_) { - it.accelerations.push_back(0.0); - } - if (!it.effort.empty()) - { - it.effort.push_back(0.0); + // copy current state if state interface exists + it.positions.push_back(joint_state_interface_[0][index].get().get_value()); } } + if (!it.velocities.empty()) + { + it.velocities.push_back(0.0); + } + if (!it.accelerations.empty()) + { + it.accelerations.push_back(0.0); + } + if (!it.effort.empty()) + { + it.effort.push_back(0.0); + } } } } +} - void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) +void JointTrajectoryController::sort_to_local_joint_order( + std::shared_ptr trajectory_msg) +{ + // rearrange all points in the trajectory message based on mapping + std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); + auto remap = [this]( + const std::vector & to_remap, + const std::vector & mapping) -> std::vector { - // rearrange all points in the trajectory message based on mapping - std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); - auto remap = [this]( - const std::vector & to_remap, - const std::vector & mapping) -> std::vector { - if (to_remap.empty()) - { - return to_remap; - } - if (to_remap.size() != mapping.size()) - { - RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); - return to_remap; - } - std::vector output; - output.resize(mapping.size(), 0.0); - for (size_t index = 0; index < mapping.size(); ++index) - { - auto map_index = mapping[index]; - output[map_index] = to_remap[index]; - } - return output; - }; - - for (size_t index = 0; index < trajectory_msg->points.size(); ++index) + if (to_remap.empty()) { - trajectory_msg->points[index].positions = - remap(trajectory_msg->points[index].positions, mapping_vector); + return to_remap; + } + if (to_remap.size() != mapping.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + return to_remap; + } + std::vector output; + output.resize(mapping.size(), 0.0); + for (size_t index = 0; index < mapping.size(); ++index) + { + auto map_index = mapping[index]; + output[map_index] = to_remap[index]; + } + return output; + }; - trajectory_msg->points[index].velocities = - remap(trajectory_msg->points[index].velocities, mapping_vector); + for (size_t index = 0; index < trajectory_msg->points.size(); ++index) + { + trajectory_msg->points[index].positions = + remap(trajectory_msg->points[index].positions, mapping_vector); - trajectory_msg->points[index].accelerations = - remap(trajectory_msg->points[index].accelerations, mapping_vector); + trajectory_msg->points[index].velocities = + remap(trajectory_msg->points[index].velocities, mapping_vector); - trajectory_msg->points[index].effort = - remap(trajectory_msg->points[index].effort, mapping_vector); - } + trajectory_msg->points[index].accelerations = + remap(trajectory_msg->points[index].accelerations, mapping_vector); + + trajectory_msg->points[index].effort = + remap(trajectory_msg->points[index].effort, mapping_vector); } +} - bool JointTrajectoryController::validate_trajectory_point_field( - size_t joint_names_size, const std::vector & vector_field, - const std::string & string_for_vector_field, size_t i, bool allow_empty) const +bool JointTrajectoryController::validate_trajectory_point_field( + size_t joint_names_size, const std::vector & vector_field, + const std::string & string_for_vector_field, size_t i, bool allow_empty) const +{ + if (allow_empty && vector_field.empty()) { - if (allow_empty && vector_field.empty()) - { - return true; - } - if (joint_names_size != vector_field.size()) + return true; + } + if (joint_names_size != vector_field.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", + joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + return false; + } + return true; +} + +bool JointTrajectoryController::validate_trajectory_msg( + const trajectory_msgs::msg::JointTrajectory & trajectory) const +{ + // If partial joints goals are not allowed, goal should specify all controller joints + if (!allow_partial_joints_goal_) + { + if (trajectory.joint_names.size() != dof_) { RCLCPP_ERROR( - get_node()->get_logger(), "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", - joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); + get_node()->get_logger(), + "Joints on incoming trajectory don't match the controller joints."); return false; } - return true; } - bool JointTrajectoryController::validate_trajectory_msg( - const trajectory_msgs::msg::JointTrajectory & trajectory) const + if (trajectory.joint_names.empty()) { - // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) + RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); + return false; + } + + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) { - if (trajectory.joint_names.size() != dof_) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Joints on incoming trajectory don't match the controller joints."); - return false; - } + trajectory_end_time += p.time_from_start; } - - if (trajectory.joint_names.empty()) + if (trajectory_end_time < get_node()->now()) { - RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non zero time start time (%f) that ends on the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; } + } - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) + // for (size_t i = 0; i < trajectory.joint_names.size(); ++i) + // { + // const std::string & incoming_joint_name = trajectory.joint_names[i]; + // + // auto it = + // std::find(command_joint_names_.begin(), command_joint_names_.end(), incoming_joint_name); + // if (it == command_joint_names_.end()) + // { + // RCLCPP_ERROR( + // get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", + // incoming_joint_name.c_str()); + // return false; + // } + // } + + rclcpp::Duration previous_traj_time(0ms); + for (size_t i = 0; i < trajectory.points.size(); ++i) + { + if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } - if (trajectory_end_time < get_node()->now()) + RCLCPP_ERROR( + get_node()->get_logger(), + "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", + i - 1, i, previous_traj_time.seconds(), + rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); + return false; + } + previous_traj_time = trajectory.points[i].time_from_start; + + const size_t joint_count = trajectory.joint_names.size(); + const auto & points = trajectory.points; + // This currently supports only position, velocity and acceleration inputs + if (allow_integration_in_goal_trajectories_) + { + const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && + points[i].accelerations.empty(); + const bool position_error = + !points[i].positions.empty() && + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); + const bool velocity_error = + !points[i].velocities.empty() && + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); + const bool acceleration_error = + !points[i].accelerations.empty() && + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, false); + if (all_empty || position_error || velocity_error || acceleration_error) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non zero time start time (%f) that ends on the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; } } - -// for (size_t i = 0; i < trajectory.joint_names.size(); ++i) -// { -// const std::string & incoming_joint_name = trajectory.joint_names[i]; -// -// auto it = -// std::find(command_joint_names_.begin(), command_joint_names_.end(), incoming_joint_name); -// if (it == command_joint_names_.end()) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", -// incoming_joint_name.c_str()); -// return false; -// } -// } - - rclcpp::Duration previous_traj_time(0ms); - for (size_t i = 0; i < trajectory.points.size(); ++i) + else if ( + !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || + !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || + !validate_trajectory_point_field( + joint_count, points[i].accelerations, "accelerations", i, true) || + !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) { - if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", - i - 1, i, previous_traj_time.seconds(), - rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); - return false; - } - previous_traj_time = trajectory.points[i].time_from_start; - - const size_t joint_count = trajectory.joint_names.size(); - const auto & points = trajectory.points; - // This currently supports only position, velocity and acceleration inputs - if (allow_integration_in_goal_trajectories_) - { - const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && - points[i].accelerations.empty(); - const bool position_error = - !points[i].positions.empty() && - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false); - const bool velocity_error = - !points[i].velocities.empty() && - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, false); - const bool acceleration_error = - !points[i].accelerations.empty() && - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, false); - if (all_empty || position_error || velocity_error || acceleration_error) - { - return false; - } - } - else if ( - !validate_trajectory_point_field(joint_count, points[i].positions, "positions", i, false) || - !validate_trajectory_point_field(joint_count, points[i].velocities, "velocities", i, true) || - !validate_trajectory_point_field( - joint_count, points[i].accelerations, "accelerations", i, true) || - !validate_trajectory_point_field(joint_count, points[i].effort, "effort", i, true)) - { - return false; - } + return false; } - return true; } + return true; +} - void JointTrajectoryController::add_new_trajectory_msg( - const std::shared_ptr & traj_msg) - { - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); - } +void JointTrajectoryController::add_new_trajectory_msg( + const std::shared_ptr & traj_msg) +{ + traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); +} - void JointTrajectoryController::preempt_active_goal() +void JointTrajectoryController::preempt_active_goal() +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { - const auto active_goal = *rt_active_goal_.readFromNonRT(); - if (active_goal) - { - set_hold_position(); - auto action_res = std::make_shared(); - action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); - action_res->set__error_string("Current goal cancelled due to new incoming action."); - active_goal->setCanceled(action_res); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); - } + set_hold_position(); + auto action_res = std::make_shared(); + action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL); + action_res->set__error_string("Current goal cancelled due to new incoming action."); + active_goal->setCanceled(action_res); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } +} - void JointTrajectoryController::set_hold_position() - { - trajectory_msgs::msg::JointTrajectory empty_msg; - empty_msg.header.stamp = rclcpp::Time(0); +void JointTrajectoryController::set_hold_position() +{ + trajectory_msgs::msg::JointTrajectory empty_msg; + empty_msg.header.stamp = rclcpp::Time(0); - auto traj_msg = std::make_shared(empty_msg); - add_new_trajectory_msg(traj_msg); - } + auto traj_msg = std::make_shared(empty_msg); + add_new_trajectory_msg(traj_msg); +} + +bool JointTrajectoryController::contains_interface_type( + const std::vector & interface_type_list, const std::string & interface_type) +{ + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); +} - bool JointTrajectoryController::contains_interface_type( - const std::vector & interface_type_list, const std::string & interface_type) +void JointTrajectoryController::resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) +{ + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) { - return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != - interface_type_list.end(); + point.velocities.resize(size, 0.0); } - - void JointTrajectoryController::resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + if (has_acceleration_state_interface_) { - point.positions.resize(size, 0.0); - if (has_velocity_state_interface_) - { - point.velocities.resize(size, 0.0); - } - if (has_acceleration_state_interface_) - { - point.accelerations.resize(size, 0.0); - } + point.accelerations.resize(size, 0.0); } +} } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) + joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface) From 9760b925747cd5f40d686e3c17a94e7003c5eb85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 21 Aug 2022 18:01:17 +0200 Subject: [PATCH 067/111] JTC should be the same as on master. --- .../joint_trajectory_controller.hpp | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 8c262fd056..79fe6bccf4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -27,7 +27,6 @@ #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -//#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/visibility_control.h" @@ -68,16 +67,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JointTrajectoryController(); /** - * @brief command_interface_configuration This controller requires the position command - * interfaces for the controlled joints - */ + * @brief command_interface_configuration This controller requires the position command + * interfaces for the controlled joints + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; /** - * @brief command_interface_configuration This controller requires the position and velocity - * state interfaces for the controlled joints - */ + * @brief command_interface_configuration This controller requires the position and velocity + * state interfaces for the controlled joints + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; @@ -139,7 +138,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Run the controller in open-loop, i.e., read hardware states only when starting controller. // This is useful when robot is not exactly following the commanded trajectory. bool open_loop_control_ = false; - trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Allow integration in goal trajectories to accept goals without position or velocity specified bool allow_integration_in_goal_trajectories_ = false; @@ -176,7 +174,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; - // TODO(karsten1987): eventually activate and deactivate subscriber directly when it's supported + // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; @@ -208,18 +206,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); - // joint limits for JTC - // std::vector joint_limits_; + // callback for topic interface + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void topic_callback(const std::shared_ptr msg); // callbacks for action_server_ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::GoalResponse goal_callback( + rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::CancelResponse cancel_callback( + rclcpp_action::CancelResponse goal_cancelled_callback( const std::shared_ptr> goal_handle); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void feedback_setup_callback( + void goal_accepted_callback( std::shared_ptr> goal_handle); // fill trajectory_msg so it matches joints controlled by this controller From 6ba8bccfecf03801a854ffd2a55dcb5e5298a6e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 21 Aug 2022 19:30:23 +0200 Subject: [PATCH 068/111] A bit more code cleaning and polishing. --- admittance_controller/CMakeLists.txt | 8 +- .../admittance_controller.hpp | 2 - .../admittance_controller/admittance_rule.hpp | 100 +++++++++--------- .../admittance_rule_impl.hpp | 44 ++++---- .../src/admittance_controller.cpp | 40 +++---- .../test/test_admittance_controller.cpp | 38 +++---- 6 files changed, 111 insertions(+), 121 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 09279f7509..05939673ed 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -62,7 +62,9 @@ install(TARGETS ${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(control_msgs REQUIRED) find_package(controller_manager REQUIRED) + find_package(controller_interface REQUIRED) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) @@ -84,9 +86,9 @@ if(BUILD_TESTING) endfunction() # test loading admittance controller - add_test_with_yaml_input(test_load_admittance_controller "test/test_load_admittance_controller.cpp" + add_test_with_yaml_input(test_load_admittance_controller test/test_load_admittance_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_load_admittance_controller PUBLIC "${GMOCK_INCLUDE_DIRS}") + target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) ament_target_dependencies( test_load_admittance_controller @@ -95,7 +97,7 @@ if(BUILD_TESTING) ros2_control_test_assets ) # test admittance controller function - add_test_with_yaml_input(test_admittance_controller "test/test_admittance_controller.cpp" + add_test_with_yaml_input(test_admittance_controller test/test_admittance_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) target_include_directories(test_admittance_controller PRIVATE include) target_link_libraries(test_admittance_controller admittance_controller) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 3b5dfff95b..ba77d419b8 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -96,8 +96,6 @@ class AdmittanceController : public controller_interface::ChainableControllerInt controller_interface::return_type update_reference_from_subscribers() override; - bool on_set_chained_mode(bool chained_mode) override; - size_t num_joints_ = 0; size_t state_pos_ind = -1; size_t state_vel_ind = -1; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index c9f0ab4a74..227025a9b9 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -24,8 +24,6 @@ #include #include -#include "tf2_ros/buffer.h" - #include "admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "control_toolbox/filters.hpp" @@ -33,12 +31,12 @@ #include "geometry_msgs/msg/wrench_stamped.hpp" #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" -#include "tf2_eigen/tf2_eigen.h" +#include "tf2_eigen/tf2_eigen.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -// TODO(destogl): remove this include - this is wrong! -#include "trajectory_msgs/msg/detail/joint_trajectory_point__struct.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller { @@ -103,40 +101,42 @@ class AdmittanceRule reset(num_joints_); } + /// Configure admittance rule memory using number of joints. controller_interface::return_type configure( - const std::shared_ptr & node, size_t num_joint); + const std::shared_ptr & node, const size_t num_joint); - controller_interface::return_type reset(size_t num_joints); + /// Reset all values back to default + controller_interface::return_type reset(const size_t num_joints); /** - * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does - * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation - * are calculated without an error - * \param[in] current_joint_state - * \param[in] reference_joint_state - * \param[in] success - */ + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does + * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation + * are calculated without an error + * \param[in] current_joint_state + * \param[in] reference_joint_state + * \param[in] success + */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, const AdmittanceState & admittance_state); /** - * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field - * members (ee_weight_, cog_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated - */ + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field + * members (ee_weight_, cog_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + */ void apply_parameters_update(); /** - * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and - * 'current_joint_state'. - * - * \param[in] current_joint_state - * \param[in] measured_wrench - * \param[in] reference_joint_state - * \param[in] period - * \param[out] desired_joint_state - */ + * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * 'current_joint_state'. + * + * \param[in] current_joint_state + * \param[in] measured_wrench + * \param[in] reference_joint_state + * \param[in] period + * \param[out] desired_joint_state + */ controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, @@ -145,10 +145,10 @@ class AdmittanceRule trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); /** - * Fill fields of `state_message` from current admittance controller state. - * - * \param[in] state_message - */ + * Fill fields of `state_message` from current admittance controller state. + * + * \param[in] state_message + */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); public: @@ -158,29 +158,29 @@ class AdmittanceRule protected: /** - * Calculate the admittance rule from given the robot's current joint angles. the control frame rotation, the - * current force torque transform, the reference force torque transform, the force torque sensor frame id, - * the time delta, and the current admittance controller state. The admittance controller state input is updated - * with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin calls - * failed. - * \param[in] current_joint_positions - * \param[in] base_control - * \param[in] base_ft - * \param[in] ref_base_ft - * \param[in] ft_sensor_frame - * \param[in] dt - * \param[in] admittance_state - * \param[out] success - */ + * Calculate the admittance rule from given the robot's current joint angles. the control frame rotation, the + * current force torque transform, the reference force torque transform, the force torque sensor frame id, + * the time delta, and the current admittance controller state. The admittance controller state input is updated + * with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin calls + * failed. + * \param[in] current_joint_positions + * \param[in] base_control + * \param[in] base_ft + * \param[in] ref_base_ft + * \param[in] ft_sensor_frame + * \param[in] dt + * \param[in] admittance_state + * \param[out] success + */ bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement. The `wrench_world_` - * estimate includes gravity compensation - * \param[in] measured_wrench - * \param[in] sensor_rot - * \param[in] cog_rot - */ + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement. The `wrench_world_` + * estimate includes gravity compensation + * \param[in] measured_wrench + * \param[in] sensor_rot + * \param[in] cog_rot + */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, const Eigen::Matrix & sensor_rot, const Eigen::Matrix & cog_rot); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index c1e22d0c10..865e1f11ef 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,18 +22,16 @@ #include #include -#include "tf2_ros/transform_listener.h" - #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" +#include "tf2_ros/transform_listener.h" namespace admittance_controller { +/// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( - const std::shared_ptr & node, size_t num_joints) + const std::shared_ptr & node, const size_t num_joints) { - // configure admittance rule using num_joints and load kinematics interface - num_joints_ = num_joints; // initialize ROS functions @@ -79,16 +77,14 @@ controller_interface::return_type AdmittanceRule::configure( return controller_interface::return_type::OK; } -controller_interface::return_type AdmittanceRule::reset(size_t num_joints) +controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) { - // reset all values back to default - // reset state message fields state_message_.joint_state.name.assign(num_joints, ""); state_message_.joint_state.position.assign(num_joints, 0); state_message_.joint_state.velocity.assign(num_joints, 0); state_message_.joint_state.effort.assign(num_joints, 0); - for (auto i = 0ul; i < parameters_.joints.size(); i++) + for (size_t i = 0; i < parameters_.joints.size(); ++i) { state_message_.joint_state.name = parameters_.joints; } @@ -127,7 +123,7 @@ void AdmittanceRule::apply_parameters_update() vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); - for (auto i = 0ul; i < 6; ++i) + for (size_t i = 0; i < 6; ++i) { admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * @@ -138,7 +134,7 @@ void AdmittanceRule::apply_parameters_update() bool AdmittanceRule::get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, - const AdmittanceState & admittance_state) + const AdmittanceState & /*admittance_state*/) { // get all reference transforms bool success = kinematics_->calculate_link_transform( @@ -180,7 +176,7 @@ controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { - double dt = period.seconds(); + const double dt = period.seconds(); if (parameters_.enable_parameter_update_without_reactivation) { @@ -222,16 +218,16 @@ controller_interface::return_type AdmittanceRule::update( } // update joint desired joint state - for (auto i = 0ul; i < reference_joint_state.positions.size(); i++) + for (size_t i = 0; i < reference_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; } - for (auto i = 0ul; i < reference_joint_state.velocities.size(); i++) + for (size_t i = 0; i < reference_joint_state.velocities.size(); ++i) { desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; } - for (auto i = 0ul; i < reference_joint_state.accelerations.size(); i++) + for (size_t i = 0; i < reference_joint_state.accelerations.size(); ++i) { desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; } @@ -334,7 +330,7 @@ void AdmittanceRule::process_wrench_measurements( new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight_); // apply smoothing filter - for (auto i = 0; i < 6; i++) + for (size_t i = 0; i < 6; ++i) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); @@ -343,35 +339,35 @@ void AdmittanceRule::process_wrench_measurements( const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() { - for (auto i = 0ul; i < parameters_.joints.size(); i++) + for (size_t i = 0; i < parameters_.joints.size(); ++i) { state_message_.joint_state.name[i] = parameters_.joints[i]; } - for (auto i = 0l; i < admittance_state_.joint_pos.size(); i++) + for (size_t i = 0; i < admittance_state_.joint_pos.size(); ++i) { state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; } - for (auto i = 0l; i < admittance_state_.joint_vel.size(); i++) + for (size_t i = 0; i < admittance_state_.joint_vel.size(); ++i) { state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; } - for (auto i = 0l; i < admittance_state_.joint_acc.size(); i++) + for (size_t i = 0; i < admittance_state_.joint_acc.size(); ++i) { state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; } - for (auto i = 0l; i < admittance_state_.stiffness.size(); i++) + for (size_t i = 0; i < admittance_state_.stiffness.size(); ++i) { state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; } - for (auto i = 0l; i < admittance_state_.damping.size(); i++) + for (size_t i = 0; i < admittance_state_.damping.size(); ++i) { state_message_.damping.data[i] = admittance_state_.damping[i]; } - for (auto i = 0l; i < admittance_state_.selected_axes.size(); i++) + for (size_t i = 0; i < admittance_state_.selected_axes.size(); ++i) { state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); } - for (auto i = 0l; i < admittance_state_.mass.size(); i++) + for (size_t i = 0; i < admittance_state_.mass.size(); ++i) { state_message_.mass.data[i] = admittance_state_.mass[i]; } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 51b726f35e..ae8310479b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -91,7 +91,7 @@ controller_interface::InterfaceConfiguration AdmittanceController::state_interfa } std::vector state_interfaces_config_names; - for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) + for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) { const auto & interface = admittance_->parameters_.state_interfaces[i]; for (const auto & joint : admittance_->parameters_.joints) @@ -151,7 +151,7 @@ AdmittanceController::on_export_reference_interfaces() } controller_interface::CallbackReturn AdmittanceController::on_configure( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State & /*previous_state*/) { try { @@ -235,11 +235,11 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( return controller_interface::CallbackReturn::ERROR; } - return LifecycleNodeInterface::on_configure(previous_state); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn AdmittanceController::on_activate( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State & /*previous_state*/) { // on_activate is called when the lifecycle node activates. if (!admittance_) @@ -258,7 +258,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( {hardware_interface::HW_IF_POSITION, -1}, {hardware_interface::HW_IF_VELOCITY, -1}, {hardware_interface::HW_IF_ACCELERATION, -1}}; - for (auto i = 0ul; i < admittance_->parameters_.state_interfaces.size(); i++) + for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) { const auto & interface = admittance_->parameters_.state_interfaces[i]; inter_to_ind[interface] = i; @@ -272,7 +272,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( {hardware_interface::HW_IF_POSITION, -1}, {hardware_interface::HW_IF_VELOCITY, -1}, {hardware_interface::HW_IF_ACCELERATION, -1}}; - for (auto i = 0ul; i < admittance_->parameters_.command_interfaces.size(); i++) + for (size_t i = 0; i < admittance_->parameters_.command_interfaces.size(); ++i) { const auto & interface = admittance_->parameters_.command_interfaces[i]; inter_to_ind[interface] = i; @@ -314,11 +314,11 @@ controller_interface::return_type AdmittanceController::update_reference_from_su // if message exists, load values into references if (joint_command_msg_.get()) { - for (auto i = 0ul; i < joint_command_msg_->positions.size(); i++) + for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) { position_reference_[i].get() = joint_command_msg_->positions[i]; } - for (auto i = 0ul; i < joint_command_msg_->velocities.size(); i++) + for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) { velocity_reference_[i].get() = joint_command_msg_->velocities[i]; } @@ -328,7 +328,7 @@ controller_interface::return_type AdmittanceController::update_reference_from_su } controller_interface::return_type AdmittanceController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // Realtime constraints are required in this function if (!admittance_) @@ -365,13 +365,13 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( } controller_interface::CallbackReturn AdmittanceController::on_cleanup( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn AdmittanceController::on_error( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State & /*previous_state*/) { if (!admittance_) { @@ -389,9 +389,9 @@ void AdmittanceController::read_state_from_hardware( // not exist or the values are nan, the corresponding state field will be set to empty. // if any interface has nan values, assume state_reference is the last command state - for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { - for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) + for (size_t inter_ind = 0; inter_ind < 3; ++inter_ind) { auto ind = joint_ind + num_joints_ * inter_ind; if (inter_ind == state_pos_ind) @@ -440,9 +440,9 @@ void AdmittanceController::write_state_to_hardware( // not exist or the values are nan, the corresponding state field will be set to empty. // if any interface has nan values, assume state_reference is the last command state - for (auto joint_ind = 0ul; joint_ind < num_joints_; joint_ind++) + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { - for (auto inter_ind = 0ul; inter_ind < 3; inter_ind++) + for (size_t inter_ind = 0; inter_ind < 3; ++inter_ind) { auto ind = joint_ind + num_joints_ * inter_ind; if (inter_ind == command_pos_ind) @@ -470,7 +470,7 @@ void AdmittanceController::read_state_reference_interfaces( // If the interface does not exist or // the values are nan, the corresponding field will be set to empty - for (auto i = 0ul; i < position_reference_.size(); i++) + for (size_t i = 0; i < position_reference_.size(); ++i) { if (std::isnan(position_reference_[i])) { @@ -480,7 +480,7 @@ void AdmittanceController::read_state_reference_interfaces( } last_reference_.positions = state_reference.positions; - for (auto i = 0ul; i < velocity_reference_.size(); i++) + for (size_t i = 0; i < velocity_reference_.size(); ++i) { if (std::isnan(velocity_reference_[i])) { @@ -491,12 +491,6 @@ void AdmittanceController::read_state_reference_interfaces( last_reference_.velocities = state_reference.velocities; } -bool AdmittanceController::on_set_chained_mode(bool chained_mode) -{ - // this method sets the chained mode to value of argument chained_mode if true is returned. - return true; -} - } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp" diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 55173bea33..d056e0406c 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -224,23 +224,23 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - // Check that wrench command are all zero since not used - ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); - ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); - ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); - ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); - ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); - ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); - ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); - - // Check joint command message - for (auto i = 0ul; i < joint_names_.size(); i++) - { - ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); - ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); - ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); - ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); - } + // // Check that wrench command are all zero since not used + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + + // // Check joint command message + // for (auto i = 0ul; i < joint_names_.size(); i++) + // { + // ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); + // ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); + // } } TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) @@ -264,8 +264,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); - ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); From 9a08f7735d6c79203757e420d30368c5700dcd41 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 07:59:28 -0600 Subject: [PATCH 069/111] update control_msgs repo --- ros2_controllers.foxy.repos | 4 ++-- ros2_controllers.galactic.repos | 4 ++-- ros2_controllers.humble.repos | 4 ++-- ros2_controllers.rolling.repos | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 09ec6e5387..b4a0f2c851 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -5,8 +5,8 @@ repositories: version: foxy control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel + url: git@github.com:pac48/control_msgs.git + version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 2011f0c31a..40abfe6471 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -5,8 +5,8 @@ repositories: version: galactic control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel + url: git@github.com:pac48/control_msgs.git + version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 3a667e7d9a..da1a926722 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -5,8 +5,8 @@ repositories: version: master control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel + url: git@github.com:pac48/control_msgs.git + version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 3a667e7d9a..da1a926722 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -5,8 +5,8 @@ repositories: version: master control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel + url: git@github.com:pac48/control_msgs.git + version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From 4b0b4b28ec043d20bca29c0d03980ebbad16cdb9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:09:59 -0600 Subject: [PATCH 070/111] remove joint limits from package --- admittance_controller/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9155b9c36b..441f3e54cf 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -18,7 +18,6 @@ filters geometry_msgs hardware_interface - joint_limits joint_trajectory_controller pluginlib rclcpp From 8f85eaddc04671e3bdf4a18df9eb1855e2c220a9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:14:16 -0600 Subject: [PATCH 071/111] add kinematics_interface to not released .repo files --- ros2_controllers-not-released.foxy.repos | 4 ++++ ros2_controllers-not-released.galactic.repos | 4 ++++ ros2_controllers-not-released.humble.repos | 4 ++++ ros2_controllers-not-released.rolling.repos | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 1b3910e7e7..350d54c864 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 1b3910e7e7..350d54c864 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 1b3910e7e7..350d54c864 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 1b3910e7e7..350d54c864 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master From 742b252969009a5e63203da0cd038777fe4c5e87 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:22:26 -0600 Subject: [PATCH 072/111] add tf2_kdl to admittance package.xml --- admittance_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 441f3e54cf..3e355fdc89 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -26,6 +26,7 @@ tf2 tf2_eigen tf2_geometry_msgs + tf2_kdl tf2_ros trajectory_msgs generate_parameter_library From 97745847565377c8dd0a9f8bdebb95f603171475 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:27:59 -0600 Subject: [PATCH 073/111] add kinematics_interface to all .repo files --- ros2_controllers.foxy.repos | 4 ++++ ros2_controllers.galactic.repos | 4 ++++ ros2_controllers.humble.repos | 4 ++++ ros2_controllers.rolling.repos | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 3ddaa5d63b..2736dcb1a7 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -15,3 +15,7 @@ repositories: type: git url: https://github.com/ros/angles.git version: ros2 + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index c2c0d3ba67..142c379adb 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -15,3 +15,7 @@ repositories: type: git url: https://github.com/ros/angles.git version: ros2 + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 82f0ae706c..b10bad748d 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -11,3 +11,7 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 82f0ae706c..b10bad748d 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -11,3 +11,7 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master From a225c175fc3b8517d1fb350b8c306e9ced79a6a0 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:30:58 -0600 Subject: [PATCH 074/111] add generate_parameter_library to .repos --- ros2_controllers-not-released.foxy.repos | 4 ++++ ros2_controllers-not-released.galactic.repos | 4 ++++ ros2_controllers-not-released.humble.repos | 4 ++++ ros2_controllers-not-released.rolling.repos | 4 ++++ ros2_controllers.foxy.repos | 4 ++++ ros2_controllers.galactic.repos | 4 ++++ ros2_controllers.humble.repos | 4 ++++ ros2_controllers.rolling.repos | 4 ++++ 8 files changed, 32 insertions(+) diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 350d54c864..a54eb685ed 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -8,3 +8,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 350d54c864..a54eb685ed 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -8,3 +8,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 350d54c864..a54eb685ed 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -8,3 +8,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 350d54c864..a54eb685ed 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -8,3 +8,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 2736dcb1a7..dcf9dcc05c 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -19,3 +19,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 142c379adb..ed4796151e 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -19,3 +19,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index b10bad748d..5e03daa2b0 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -15,3 +15,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main \ No newline at end of file diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index b10bad748d..ccc76bca3e 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -15,3 +15,7 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From dc1641bef5d34fa6edc0987330b000fd52ccc9dd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:35:26 -0600 Subject: [PATCH 075/111] fix url --- ros2_controllers-not-released.foxy.repos | 2 +- ros2_controllers-not-released.galactic.repos | 2 +- ros2_controllers-not-released.humble.repos | 2 +- ros2_controllers-not-released.rolling.repos | 2 +- ros2_controllers.foxy.repos | 4 ++-- ros2_controllers.galactic.repos | 4 ++-- ros2_controllers.humble.repos | 4 ++-- ros2_controllers.rolling.repos | 2 +- 8 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index a54eb685ed..2e43246a0c 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -11,4 +11,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index a54eb685ed..2e43246a0c 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -11,4 +11,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index a54eb685ed..2e43246a0c 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -11,4 +11,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index a54eb685ed..2e43246a0c 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -11,4 +11,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index dcf9dcc05c..8c8883593a 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -5,7 +5,7 @@ repositories: version: foxy control_msgs: type: git - url: https://github.com:pac48/control_msgs.git + url: https://github.com/pac48/control_msgs.git version: add-admittance-controller realtime_tools: type: git @@ -22,4 +22,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index ed4796151e..79d546feba 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -5,7 +5,7 @@ repositories: version: galactic control_msgs: type: git - url: https://github.com:pac48/control_msgs.git + url: https://github.com/pac48/control_msgs.git version: add-admittance-controller realtime_tools: type: git @@ -22,4 +22,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 5e03daa2b0..379e4a15be 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -5,7 +5,7 @@ repositories: version: master control_msgs: type: git - url: https://github.com:pac48/control_msgs.git + url: https://github.com/pac48/control_msgs.git version: add-admittance-controller realtime_tools: type: git @@ -18,4 +18,4 @@ repositories: generate_parameter_library: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main \ No newline at end of file + version: main diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index ccc76bca3e..379e4a15be 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -5,7 +5,7 @@ repositories: version: master control_msgs: type: git - url: https://github.com:pac48/control_msgs.git + url: https://github.com/pac48/control_msgs.git version: add-admittance-controller realtime_tools: type: git From 84fa4fe897f57b524bf8c0ce425a86fbee9f65a5 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 08:42:00 -0600 Subject: [PATCH 076/111] add control_msgs to not-released .repos --- ros2_controllers-not-released.foxy.repos | 4 ++++ ros2_controllers-not-released.galactic.repos | 4 ++++ ros2_controllers-not-released.humble.repos | 4 ++++ ros2_controllers-not-released.rolling.repos | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 2e43246a0c..c4265f9bbc 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git version: main + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller \ No newline at end of file diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 2e43246a0c..c4265f9bbc 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git version: main + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller \ No newline at end of file diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 2e43246a0c..c4265f9bbc 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git version: main + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller \ No newline at end of file diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 2e43246a0c..c4265f9bbc 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/picknikrobotics/generate_parameter_library.git version: main + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller \ No newline at end of file From 8ae50905bc5fe475a38b31c8dbe5c1a8a740c5be Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 10:44:35 -0600 Subject: [PATCH 077/111] update kinematics plugin name --- admittance_controller/test/test_params.yaml | 2 +- ros2_controllers-not-released.foxy.repos | 2 +- ros2_controllers-not-released.galactic.repos | 2 +- ros2_controllers-not-released.humble.repos | 2 +- ros2_controllers-not-released.rolling.repos | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index e7676b6c91..22cbda2df9 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -40,7 +40,7 @@ test_admittance_controller: kinematics: plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL - plugin_package: kinematics_interface_kdl + plugin_package: kinematics_interface base: base_link # Assumed to be stationary tip: tool0 group_name: arm diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index c4265f9bbc..ff76947d75 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -15,4 +15,4 @@ repositories: control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller \ No newline at end of file + version: add-admittance-controller diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index c4265f9bbc..ff76947d75 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -15,4 +15,4 @@ repositories: control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller \ No newline at end of file + version: add-admittance-controller diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index c4265f9bbc..ff76947d75 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -15,4 +15,4 @@ repositories: control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller \ No newline at end of file + version: add-admittance-controller diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index c4265f9bbc..ff76947d75 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -15,4 +15,4 @@ repositories: control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller \ No newline at end of file + version: add-admittance-controller From e323d14d34b1dd8ebc6e831e2db570a9b1e804b1 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 13:18:57 -0600 Subject: [PATCH 078/111] fix parameter update on restart --- .../src/admittance_controller.cpp | 5 +---- ros2_controllers-not-released.foxy.repos | 4 ---- ros2_controllers-not-released.galactic.repos | 4 ---- ros2_controllers-not-released.humble.repos | 4 ---- ros2_controllers-not-released.rolling.repos | 4 ---- ros2_controllers.foxy.repos | 14 +------------- ros2_controllers.galactic.repos | 14 +------------- ros2_controllers.humble.repos | 14 +------------- ros2_controllers.rolling.repos | 14 +------------- 9 files changed, 5 insertions(+), 72 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index ae8310479b..fde12d7108 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -248,10 +248,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } // update parameters if any have changed - if (admittance_->parameter_handler_->is_old(admittance_->parameters_)) - { - admittance_->parameters_ = admittance_->parameter_handler_->get_params(); - } + admittance_->apply_parameters_update(); // get state interface inds std::unordered_map inter_to_ind = { diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index ff76947d75..50c6532c38 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -8,10 +8,6 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main control_msgs: type: git url: https://github.com/pac48/control_msgs.git diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index ff76947d75..50c6532c38 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -8,10 +8,6 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main control_msgs: type: git url: https://github.com/pac48/control_msgs.git diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index ff76947d75..50c6532c38 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -8,10 +8,6 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main control_msgs: type: git url: https://github.com/pac48/control_msgs.git diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index ff76947d75..50c6532c38 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -8,10 +8,6 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main control_msgs: type: git url: https://github.com/pac48/control_msgs.git diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 8c8883593a..cec7f0ea22 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -3,10 +3,6 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: foxy - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git @@ -14,12 +10,4 @@ repositories: angles: type: git url: https://github.com/ros/angles.git - version: ros2 - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + version: ros2 \ No newline at end of file diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 79d546feba..0e29d4b03f 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -3,10 +3,6 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: galactic - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git @@ -14,12 +10,4 @@ repositories: angles: type: git url: https://github.com/ros/angles.git - version: ros2 - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + version: ros2 \ No newline at end of file diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 379e4a15be..49e7604299 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -3,19 +3,7 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + version: master \ No newline at end of file diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 379e4a15be..49e7604299 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -3,19 +3,7 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + version: master \ No newline at end of file From 9c148a0adf3fff87a494e3e88b77428970034007 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 15:52:20 -0600 Subject: [PATCH 079/111] update repos --- ros2_controllers-not-released.foxy.repos | 4 ++++ ros2_controllers-not-released.galactic.repos | 4 ++++ ros2_controllers-not-released.humble.repos | 4 ++++ ros2_controllers-not-released.rolling.repos | 4 ++++ 4 files changed, 16 insertions(+) diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 50c6532c38..2122cb257c 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/pac48/control_msgs.git version: add-admittance-controller + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master \ No newline at end of file diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 50c6532c38..2122cb257c 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/pac48/control_msgs.git version: add-admittance-controller + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master \ No newline at end of file diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 50c6532c38..2122cb257c 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/pac48/control_msgs.git version: add-admittance-controller + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master \ No newline at end of file diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 50c6532c38..a4b64fa732 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/pac48/control_msgs.git version: add-admittance-controller + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master From 795cdce7dac92452043e0c4f9c7c4a18f8fa296c Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 15:54:50 -0600 Subject: [PATCH 080/111] Precommit --- admittance_controller/src/admittance_controller.cpp | 2 +- ros2_controllers-not-released.foxy.repos | 2 +- ros2_controllers-not-released.galactic.repos | 2 +- ros2_controllers-not-released.humble.repos | 2 +- ros2_controllers.foxy.repos | 2 +- ros2_controllers.galactic.repos | 2 +- ros2_controllers.humble.repos | 2 +- ros2_controllers.rolling.repos | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index fde12d7108..506ec6e7e6 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -248,7 +248,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } // update parameters if any have changed - admittance_->apply_parameters_update(); + admittance_->apply_parameters_update(); // get state interface inds std::unordered_map inter_to_ind = { diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 2122cb257c..a4b64fa732 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -15,4 +15,4 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master \ No newline at end of file + version: master diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 2122cb257c..a4b64fa732 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -15,4 +15,4 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master \ No newline at end of file + version: master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 2122cb257c..a4b64fa732 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -15,4 +15,4 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master \ No newline at end of file + version: master diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index cec7f0ea22..a769bf4ca2 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -10,4 +10,4 @@ repositories: angles: type: git url: https://github.com/ros/angles.git - version: ros2 \ No newline at end of file + version: ros2 diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 0e29d4b03f..5dcca82b05 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -10,4 +10,4 @@ repositories: angles: type: git url: https://github.com/ros/angles.git - version: ros2 \ No newline at end of file + version: ros2 diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 49e7604299..3c4bdace94 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -6,4 +6,4 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master \ No newline at end of file + version: master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 49e7604299..3c4bdace94 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -6,4 +6,4 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master \ No newline at end of file + version: master From 2f609be4c8d931814536ab4ecd8bd9311db10f7e Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 16:21:16 -0600 Subject: [PATCH 081/111] update repos --- ros2_controllers-not-released.foxy.repos | 4 ++++ ros2_controllers-not-released.galactic.repos | 4 ++++ ros2_controllers-not-released.humble.repos | 4 ++++ ros2_controllers-not-released.rolling.repos | 8 ++++++++ ros2_controllers.foxy.repos | 14 +++++++++++--- ros2_controllers.galactic.repos | 14 +++++++++++--- ros2_controllers.humble.repos | 12 ++++++++++++ ros2_controllers.rolling.repos | 16 ++++++++++++++++ 8 files changed, 70 insertions(+), 6 deletions(-) diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index a4b64fa732..88a934f5c4 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -16,3 +16,7 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index a4b64fa732..88a934f5c4 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -16,3 +16,7 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index a4b64fa732..88a934f5c4 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -16,3 +16,7 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index a4b64fa732..743ab42a6a 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -16,3 +16,11 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index a769bf4ca2..d33679017c 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -7,7 +7,15 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: + kinematics_interface: type: git - url: https://github.com/ros/angles.git - version: ros2 + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 5dcca82b05..281ac0c6e6 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -7,7 +7,15 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: + kinematics_interface: type: git - url: https://github.com/ros/angles.git - version: ros2 + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 3c4bdace94..d5ac4379aa 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -7,3 +7,15 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 3c4bdace94..69f0dee836 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -7,3 +7,19 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/pac48/control_msgs.git + version: add-admittance-controller + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From 77104489ef177b96bd00055f66a2ed8c1e4188a4 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 18:05:36 -0600 Subject: [PATCH 082/111] fix test output --- admittance_controller/CMakeLists.txt | 2 +- .../include/admittance_controller/admittance_rule.hpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 05939673ed..dbb9a6164c 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -80,7 +80,7 @@ if(BUILD_TESTING) ${TARGET} COMMAND ${executable} --ros-args --params-file ${YAML_FILE} --gtest_output=xml:${result_file} - OUTPUT_FILE ${CMAKE_BINARY_DIR}/ament_cmake_gmock/test_load_admittance_controller.txt + OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt RESULT_FILE ${result_file} ) endfunction() diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 227025a9b9..8888c9fb9d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -52,9 +52,6 @@ struct Transforms struct AdmittanceState { - // TODO(destogl): does this constructor makes sense at all? - AdmittanceState() = default; - explicit AdmittanceState(size_t num_joints) { admittance_velocity.setZero(); @@ -200,7 +197,7 @@ class AdmittanceRule Eigen::Matrix wrench_world_; // admittance controllers internal state - AdmittanceState admittance_state_; + AdmittanceState admittance_state_{0}; // transforms needed for admittance update Transforms trans_; From a5847a207c2637a63609b6c86b165eb564c66e5f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 18:18:55 -0600 Subject: [PATCH 083/111] move parameter initialization to controller class --- .../include/admittance_controller/admittance_controller.hpp | 3 +++ .../include/admittance_controller/admittance_rule.hpp | 5 ++--- admittance_controller/src/admittance_controller.cpp | 6 ++++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index ba77d419b8..ec421b99bd 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -120,6 +120,9 @@ class AdmittanceController : public controller_interface::ChainableControllerInt input_joint_command_subscriber_; rclcpp::Publisher::SharedPtr s_publisher_; + // admittance parameters + std::shared_ptr parameter_handler_; + // ROS messages std::shared_ptr joint_command_msg_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 8888c9fb9d..67a76670a7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -88,10 +88,9 @@ struct AdmittanceState class AdmittanceRule { public: - explicit AdmittanceRule(const std::shared_ptr & lifecycl_node) + explicit AdmittanceRule(const std::shared_ptr& parameter_handler) { - parameter_handler_ = std::make_shared( - lifecycl_node->get_node_parameters_interface()); + parameter_handler_ = parameter_handler; parameters_ = parameter_handler_->get_params(); num_joints_ = parameters_.joints.size(); admittance_state_ = AdmittanceState(num_joints_); diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 506ec6e7e6..04b2d20e97 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -37,7 +37,8 @@ controller_interface::CallbackReturn AdmittanceController::on_init() // initialize controller config try { - admittance_ = std::make_unique(get_node()); + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); } catch (const std::exception & e) { @@ -155,7 +156,8 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( { try { - admittance_ = std::make_unique(get_node()); + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); num_joints_ = admittance_->parameters_.joints.size(); } catch (const std::exception & e) From d8674cdf88b75eb0058b428ae3e3a46858a066c9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 18:32:11 -0600 Subject: [PATCH 084/111] use single admittance transforms struct --- .../admittance_controller/admittance_rule.hpp | 16 +++--- .../admittance_rule_impl.hpp | 53 ++++++++----------- 2 files changed, 30 insertions(+), 39 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 67a76670a7..24dea84519 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -40,10 +40,11 @@ namespace admittance_controller { -struct Transforms +struct AdmittanceTransforms { - Eigen::Isometry3d base_control_; + Eigen::Isometry3d ref_base_ft_; Eigen::Isometry3d base_ft_; + Eigen::Isometry3d base_control_; Eigen::Isometry3d base_tip_; Eigen::Isometry3d world_base_; Eigen::Isometry3d base_sensor_; @@ -88,7 +89,8 @@ struct AdmittanceState class AdmittanceRule { public: - explicit AdmittanceRule(const std::shared_ptr& parameter_handler) + explicit AdmittanceRule( + const std::shared_ptr & parameter_handler) { parameter_handler_ = parameter_handler; parameters_ = parameter_handler_->get_params(); @@ -110,12 +112,11 @@ class AdmittanceRule * are calculated without an error * \param[in] current_joint_state * \param[in] reference_joint_state - * \param[in] success + * \param[out] success */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, - const AdmittanceState & admittance_state); + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field @@ -199,8 +200,7 @@ class AdmittanceRule AdmittanceState admittance_state_{0}; // transforms needed for admittance update - Transforms trans_; - Transforms trans_ref_; + AdmittanceTransforms admittance_transforms_; // position of center of gravity in cog_frame Eigen::Vector3d cog_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 865e1f11ef..6bc244d85e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -97,8 +97,7 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) admittance_state_ = AdmittanceState(num_joints); // reset transforms and rotations - trans_ = Transforms(); - trans_ref_ = Transforms(); + admittance_transforms_ = AdmittanceTransforms(); // reset forces wrench_world_.setZero(); @@ -133,38 +132,30 @@ void AdmittanceRule::apply_parameters_update() bool AdmittanceRule::get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, - const AdmittanceState & /*admittance_state*/) + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) { - // get all reference transforms + // get reference transforms bool success = kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.ft_sensor.frame.id, trans_ref_.base_ft_); - success &= kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.kinematics.tip, trans_ref_.base_tip_); - success &= kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.fixed_world_frame.frame.id, - trans_ref_.world_base_); - success &= kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.ft_sensor.frame.id, trans_ref_.base_sensor_); - success &= kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.gravity_compensation.frame.id, - trans_ref_.base_cog_); - success &= kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.control.frame.id, trans_ref_.base_control_); + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); - // get all current transforms + // get transforms at current configuration success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.ft_sensor.frame.id, trans_.base_ft_); + current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.kinematics.tip, trans_.base_tip_); + current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.fixed_world_frame.frame.id, trans_.world_base_); + current_joint_state.positions, parameters_.fixed_world_frame.frame.id, + admittance_transforms_.world_base_); success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.ft_sensor.frame.id, trans_.base_sensor_); + current_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.base_sensor_); success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.gravity_compensation.frame.id, trans_.base_cog_); + current_joint_state.positions, parameters_.gravity_compensation.frame.id, + admittance_transforms_.base_cog_); success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.control.frame.id, trans_.base_control_); + current_joint_state.positions, parameters_.control.frame.id, + admittance_transforms_.base_control_); return success; } @@ -183,13 +174,13 @@ controller_interface::return_type AdmittanceRule::update( apply_parameters_update(); } - bool success = get_all_transforms(current_joint_state, reference_joint_state, admittance_state_); + bool success = get_all_transforms(current_joint_state, reference_joint_state); // calculate needed rotations - Eigen::Matrix rot_base_sensor = trans_.base_sensor_.rotation(); - Eigen::Matrix rot_world_base = trans_.world_base_.rotation(); - Eigen::Matrix rot_base_cog = trans_.base_cog_.rotation(); - Eigen::Matrix rot_base_control = trans_.base_control_.rotation(); + Eigen::Matrix rot_base_sensor = admittance_transforms_.base_sensor_.rotation(); + Eigen::Matrix rot_world_base = admittance_transforms_.world_base_.rotation(); + Eigen::Matrix rot_base_cog = admittance_transforms_.base_cog_.rotation(); + Eigen::Matrix rot_base_control = admittance_transforms_.base_control_.rotation(); // apply filter and update wrench_world_ vector Eigen::Matrix rot_world_sensor = rot_world_base * rot_base_sensor; @@ -205,7 +196,7 @@ controller_interface::return_type AdmittanceRule::update( // Compute admittance control law vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); admittance_state_.rot_base_control = rot_base_control; - admittance_state_.ref_trans_base_ft = trans_ref_.base_ft_; + admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; success &= calculate_admittance_rule(admittance_state_, dt); From d1d42df1f5dc2db5fbceaf93075a27edb3b93b74 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 22 Aug 2022 18:36:54 -0600 Subject: [PATCH 085/111] rename cog_ and ee_weight_ variables --- .../include/admittance_controller/admittance_rule.hpp | 6 +++--- .../admittance_controller/admittance_rule_impl.hpp | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 24dea84519..d73044f8e0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -120,7 +120,7 @@ class AdmittanceRule /** * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field - * members (ee_weight_, cog_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated */ void apply_parameters_update(); @@ -203,10 +203,10 @@ class AdmittanceRule AdmittanceTransforms admittance_transforms_; // position of center of gravity in cog_frame - Eigen::Vector3d cog_; + Eigen::Vector3d cog_pos_; // force applied to sensor due to weight of end effector - Eigen::Vector3d ee_weight_; + Eigen::Vector3d end_effector_weight_; // ROS control_msgs::msg::AdmittanceControllerState state_message_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 6bc244d85e..45e91151cd 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -101,7 +101,7 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) // reset forces wrench_world_.setZero(); - ee_weight_.setZero(); + end_effector_weight_.setZero(); // load/initialize Eigen types from parameters apply_parameters_update(); @@ -116,8 +116,8 @@ void AdmittanceRule::apply_parameters_update() parameters_ = parameter_handler_->get_params(); } // update param values - ee_weight_[2] = -parameters_.gravity_compensation.CoG.force; - vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_); + end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force; + vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_); vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); @@ -317,8 +317,8 @@ void AdmittanceRule::process_wrench_measurements( Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; // apply gravity compensation - new_wrench_base(2, 0) -= ee_weight_[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_).cross(ee_weight_); + new_wrench_base(2, 0) -= end_effector_weight_[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); // apply smoothing filter for (size_t i = 0; i < 6; ++i) From d6050dd9ef7f8287aa3c0087659c9c7e3f75143e Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 07:51:26 -0600 Subject: [PATCH 086/111] Update admittance_controller/include/admittance_controller/admittance_rule_impl.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../admittance_rule_impl.hpp | 21 ------------------- 1 file changed, 21 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 45e91151cd..edfb198d5f 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -333,33 +333,12 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control for (size_t i = 0; i < parameters_.joints.size(); ++i) { state_message_.joint_state.name[i] = parameters_.joints[i]; - } - for (size_t i = 0; i < admittance_state_.joint_pos.size(); ++i) - { state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; - } - for (size_t i = 0; i < admittance_state_.joint_vel.size(); ++i) - { state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; - } - for (size_t i = 0; i < admittance_state_.joint_acc.size(); ++i) - { state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; - } - for (size_t i = 0; i < admittance_state_.stiffness.size(); ++i) - { state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - } - for (size_t i = 0; i < admittance_state_.damping.size(); ++i) - { state_message_.damping.data[i] = admittance_state_.damping[i]; - } - for (size_t i = 0; i < admittance_state_.selected_axes.size(); ++i) - { state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - } - for (size_t i = 0; i < admittance_state_.mass.size(); ++i) - { state_message_.mass.data[i] = admittance_state_.mass[i]; } From 93bd6273c0a460e86235fdf0f4931576b3fb31a1 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 09:00:22 -0600 Subject: [PATCH 087/111] reset on values deactivate --- .../admittance_controller.hpp | 14 +- .../admittance_rule_impl.hpp | 14 +- .../src/admittance_controller.cpp | 151 ++++++++++-------- 3 files changed, 103 insertions(+), 76 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index ec421b99bd..ae34e4ccb8 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -142,12 +142,22 @@ class AdmittanceController : public controller_interface::ChainableControllerInt trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; geometry_msgs::msg::Wrench ft_values_; - // helper methods + /** + * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values + */ void read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint & state_current, geometry_msgs::msg::Wrench & ft_values); + + /** + * @brief Set fields of state_reference with values from controllers exported position and velocity references + */ void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); - void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded); + + /** +* @brief Write values from state_command to claimed hardware interfaces +*/ + void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index edfb198d5f..cdc2a5a21a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -209,18 +209,14 @@ controller_interface::return_type AdmittanceRule::update( } // update joint desired joint state - for (size_t i = 0; i < reference_joint_state.positions.size(); ++i) + for (size_t i = 0; i < num_joints_; ++i) { desired_joint_state.positions[i] = reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; - } - for (size_t i = 0; i < reference_joint_state.velocities.size(); ++i) - { - desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; - } - for (size_t i = 0; i < reference_joint_state.accelerations.size(); ++i) - { - desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; + desired_joint_state.velocities[i] = + reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + desired_joint_state.accelerations[i] = + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; } return controller_interface::return_type::OK; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 04b2d20e97..ff0e482984 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -18,16 +18,14 @@ #include #include -#include #include #include #include -#include "tf2_ros/buffer.h" - #include "admittance_controller/admittance_rule_impl.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "rcutils/logging_macros.h" +#include "tf2_ros/buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller @@ -252,49 +250,54 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( // update parameters if any have changed admittance_->apply_parameters_update(); - // get state interface inds - std::unordered_map inter_to_ind = { - {hardware_interface::HW_IF_POSITION, -1}, - {hardware_interface::HW_IF_VELOCITY, -1}, - {hardware_interface::HW_IF_ACCELERATION, -1}}; + // get indices for state and command interfaces for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) { const auto & interface = admittance_->parameters_.state_interfaces[i]; - inter_to_ind[interface] = i; + if (interface == hardware_interface::HW_IF_POSITION) + { + state_pos_ind = i; + } + else if (interface == hardware_interface::HW_IF_VELOCITY) + { + state_vel_ind = i; + } + else if (interface == hardware_interface::HW_IF_ACCELERATION) + { + state_acc_ind = i; + } } - state_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; - state_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; - state_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; - - // get state interface inds - inter_to_ind = { - {hardware_interface::HW_IF_POSITION, -1}, - {hardware_interface::HW_IF_VELOCITY, -1}, - {hardware_interface::HW_IF_ACCELERATION, -1}}; for (size_t i = 0; i < admittance_->parameters_.command_interfaces.size(); ++i) { const auto & interface = admittance_->parameters_.command_interfaces[i]; - inter_to_ind[interface] = i; + if (interface == hardware_interface::HW_IF_POSITION) + { + command_pos_ind = i; + } + else if (interface == hardware_interface::HW_IF_VELOCITY) + { + command_vel_ind = i; + } + else if (interface == hardware_interface::HW_IF_ACCELERATION) + { + command_acc_ind = i; + } } - command_pos_ind = inter_to_ind[hardware_interface::HW_IF_POSITION]; - command_vel_ind = inter_to_ind[hardware_interface::HW_IF_VELOCITY]; - command_acc_ind = inter_to_ind[hardware_interface::HW_IF_ACCELERATION]; - // initialize interface of the FTS semantic semantic component + // initialize interface of the FTS semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); // handle state after restart or initial startups read_state_from_hardware(joint_state_, ft_values_); - // initialize states from last read state reference + // initialize states + for (auto val : joint_state_.positions) + { + RCLCPP_ERROR(get_node()->get_logger(), "val: %f \n", val); + } last_commanded_ = joint_state_; last_reference_ = joint_state_; - read_state_reference_interfaces(reference_); - - // reset dynamic fields in case non-zero - reference_.velocities.assign(num_joints_, 0.0); - reference_.accelerations.assign(num_joints_, 0.0); reference_admittance_ = reference_; return controller_interface::CallbackReturn::SUCCESS; @@ -358,8 +361,29 @@ controller_interface::return_type AdmittanceController::update_and_write_command controller_interface::CallbackReturn AdmittanceController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { + // release force torque sensor interface force_torque_sensor_->release_interfaces(); + // reset indices + state_pos_ind = -1; + state_vel_ind = -1; + state_acc_ind = -1; + command_pos_ind = -1; + command_vel_ind = -1; + command_acc_ind = -1; + + // reset to prevent stale references + auto assign_vector = [&](std::vector & vec, double assigned_value) + { + for (auto & val : vec) + { + val = assigned_value; + } + }; + assign_vector(reference_.positions, std::numeric_limits::quiet_NaN()); + assign_vector(reference_.velocities, std::numeric_limits::quiet_NaN()); + assign_vector(reference_.accelerations, std::numeric_limits::quiet_NaN()); + return LifecycleNodeInterface::on_deactivate(previous_state); } @@ -384,44 +408,46 @@ void AdmittanceController::read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint & state_current, geometry_msgs::msg::Wrench & ft_values) { - // Fill fields of state_reference argument from hardware state interfaces. If the hardware does - // not exist or the values are nan, the corresponding state field will be set to empty. - - // if any interface has nan values, assume state_reference is the last command state + // if any interface has nan values, assume state_current is the last command state + bool nan_position = false; + bool nan_velocity = false; + bool nan_acceleration = false; for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { for (size_t inter_ind = 0; inter_ind < 3; ++inter_ind) { - auto ind = joint_ind + num_joints_ * inter_ind; + auto interface_value = state_interfaces_[joint_ind + num_joints_ * inter_ind].get_value(); if (inter_ind == state_pos_ind) { - state_current.positions[joint_ind] = state_interfaces_[ind].get_value(); - if (std::isnan(state_current.positions[joint_ind])) - { - state_current.positions = last_commanded_.positions; - break; - } + state_current.positions[joint_ind] = interface_value; + nan_position |= std::isnan(interface_value); } else if (inter_ind == state_vel_ind) { - state_current.velocities[joint_ind] = state_interfaces_[ind].get_value(); - if (std::isnan(state_current.positions[joint_ind])) - { - state_current.velocities = last_commanded_.velocities; - break; - } + state_current.velocities[joint_ind] = interface_value; + nan_velocity |= std::isnan(interface_value); } else if (inter_ind == state_acc_ind) { - state_current.accelerations[joint_ind] = state_interfaces_[ind].get_value(); - if (std::isnan(state_current.positions[joint_ind])) - { - state_current.accelerations = last_commanded_.accelerations; - break; - } + state_current.accelerations[joint_ind] = interface_value; + nan_acceleration |= std::isnan(interface_value); } } } + if (nan_position) + { + state_current.positions = last_commanded_.positions; + } + if (nan_velocity) + { + state_current.velocities = last_commanded_.velocities; + } + if (nan_acceleration) + { + state_current.accelerations = last_commanded_.accelerations; + } + + // if any ft_values are nan, assume values are zero force_torque_sensor_->get_values_as_message(ft_values); if ( std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || @@ -435,10 +461,7 @@ void AdmittanceController::read_state_from_hardware( void AdmittanceController::write_state_to_hardware( const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded) { - // Fill fields of state_reference argument from hardware state interfaces. If the hardware does - // not exist or the values are nan, the corresponding state field will be set to empty. - - // if any interface has nan values, assume state_reference is the last command state + // if any interface has nan values, assume state_commanded is the last command state for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { for (size_t inter_ind = 0; inter_ind < 3; ++inter_ind) @@ -458,35 +481,33 @@ void AdmittanceController::write_state_to_hardware( } } } - last_commanded_ = reference_admittance_; + last_commanded_ = state_commanded; } void AdmittanceController::read_state_reference_interfaces( trajectory_msgs::msg::JointTrajectoryPoint & state_reference) { // TODO(destogl): check why is this here? - // Fill fields of state_reference argument from controller references. - // If the interface does not exist or - // the values are nan, the corresponding field will be set to empty - for (size_t i = 0; i < position_reference_.size(); ++i) + // if any interface has nan values, assume state_reference is the last set reference + for (size_t i = 0; i < num_joints_; ++i) { + // update position if (std::isnan(position_reference_[i])) { position_reference_[i].get() = last_reference_.positions[i]; } state_reference.positions[i] = position_reference_[i]; - } - last_reference_.positions = state_reference.positions; - for (size_t i = 0; i < velocity_reference_.size(); ++i) - { + // update velocity if (std::isnan(velocity_reference_[i])) { velocity_reference_[i].get() = last_reference_.velocities[i]; } state_reference.velocities[i] = velocity_reference_[i]; } + + last_reference_.positions = state_reference.positions; last_reference_.velocities = state_reference.velocities; } From ac307cea2133cdcf32f8bcff0beda1f24dbcbab8 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 10:14:32 -0600 Subject: [PATCH 088/111] fix admittance reset --- .../src/admittance_controller.cpp | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index ff0e482984..a18dfb5e0a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -287,16 +287,19 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( // initialize interface of the FTS semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); - // handle state after restart or initial startups - read_state_from_hardware(joint_state_, ft_values_); - // initialize states - for (auto val : joint_state_.positions) - { - RCLCPP_ERROR(get_node()->get_logger(), "val: %f \n", val); + read_state_from_hardware(joint_state_, ft_values_); + for (auto val : joint_state_.positions) { + if (std::isnan(val)) { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); + return controller_interface::CallbackReturn::ERROR; + } } - last_commanded_ = joint_state_; + last_reference_ = joint_state_; + last_commanded_ = joint_state_; + read_state_reference_interfaces(reference_); reference_admittance_ = reference_; @@ -373,16 +376,12 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( command_acc_ind = -1; // reset to prevent stale references - auto assign_vector = [&](std::vector & vec, double assigned_value) - { - for (auto & val : vec) - { - val = assigned_value; - } - }; - assign_vector(reference_.positions, std::numeric_limits::quiet_NaN()); - assign_vector(reference_.velocities, std::numeric_limits::quiet_NaN()); - assign_vector(reference_.accelerations, std::numeric_limits::quiet_NaN()); + for (size_t i = 0; i < num_joints_; i++ ){ + position_reference_[i].get() = std::numeric_limits::quiet_NaN(); + velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + } + + admittance_->reset(num_joints_); return LifecycleNodeInterface::on_deactivate(previous_state); } From ed0b0140b339468538233c00af1146ce4bdd9805 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 11:44:24 -0600 Subject: [PATCH 089/111] add joint damping parameter --- .../admittance_controller/admittance_rule_impl.hpp | 7 ++++++- .../src/admittance_controller_parameters.yaml | 12 ++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index cdc2a5a21a..3162a7587b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -281,8 +281,13 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, admittance_state.joint_acc); + // add damping if cartesian velocity falls below threshold + for (size_t i = 0; i < admittance_state.joint_acc.size(); i++){ + admittance_state.joint_acc[i] -= parameters_.admittance.joint_damping*admittance_state.joint_vel[i]; + } + // integrate motion in joint space - admittance_state.joint_vel += admittance_state.joint_acc * dt; + admittance_state.joint_vel += (admittance_state.joint_acc) * dt; admittance_state.joint_pos += admittance_state.joint_vel * dt; // calculate admittance velocity corresponding to joint velocity diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 45106f24f0..546d6ce93a 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -45,7 +45,7 @@ admittance_controller: } alpha: { type: double, - default_value: 0.0005, + default_value: 0.01, description: "specifies the damping coefficient for the Jacobian pseudo inverse" } @@ -61,7 +61,7 @@ admittance_controller: } filter_coefficient: { type: double, - default_value: 0.005, + default_value: 0.05, description: "specifies the coefficient for the sensor's exponential filter" } @@ -132,6 +132,14 @@ admittance_controller: element_bounds<>: [ 0.0, 100000000.0 ] } } + joint_damping: { + type: double, + description: "specifies joint damping applied when the cartesian velocity norm is less than min_velocity", + default_value: 5, + validation: { + lower_bounds: [ 0.0 ] + } + } # general settings robot_description: { From ba73aeee61ad089ce30146a226308aeaa509372f Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 12:07:07 -0600 Subject: [PATCH 090/111] update documenation in admittance_rule --- .../admittance_controller/admittance_rule.hpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index d73044f8e0..76ce716711 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -155,32 +155,26 @@ class AdmittanceRule protected: /** - * Calculate the admittance rule from given the robot's current joint angles. the control frame rotation, the - * current force torque transform, the reference force torque transform, the force torque sensor frame id, - * the time delta, and the current admittance controller state. The admittance controller state input is updated - * with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin calls - * failed. - * \param[in] current_joint_positions - * \param[in] base_control - * \param[in] base_ft - * \param[in] ref_base_ft - * \param[in] ft_sensor_frame - * \param[in] dt + * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input + * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin + * calls failed. * \param[in] admittance_state + * \param[in] dt * \param[out] success */ bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement. The `wrench_world_` - * estimate includes gravity compensation + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, + * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. + * The `wrench_world_` estimate includes gravity compensation * \param[in] measured_wrench - * \param[in] sensor_rot - * \param[in] cog_rot + * \param[in] sensor_world_rot + * \param[in] cog_world_rot */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, - const Eigen::Matrix & sensor_rot, const Eigen::Matrix & cog_rot); + const Eigen::Matrix & sensor_world_rot, const Eigen::Matrix & cog_world_rot); template void vec_to_eigen(const std::vector & data, T2 & matrix); From c61217b20a73bd4ef415712a1e80d1ec224a4a72 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 12:30:37 -0600 Subject: [PATCH 091/111] Update comments --- admittance_controller/CMakeLists.txt | 2 -- .../admittance_controller/admittance_rule.hpp | 7 ++++--- .../admittance_rule_impl.hpp | 12 ++++++----- .../src/admittance_controller.cpp | 20 ++++++++++--------- 4 files changed, 22 insertions(+), 19 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index dbb9a6164c..84aa025da9 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -38,8 +38,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() - -# The admittance controller add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include) generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 76ce716711..a1a0658d64 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -142,9 +142,9 @@ class AdmittanceRule trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); /** - * Fill fields of `state_message` from current admittance controller state. + * Set fields of `state_message` from current admittance controller state. * - * \param[in] state_message + * \param[out] state_message */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); @@ -174,7 +174,8 @@ class AdmittanceRule */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, - const Eigen::Matrix & sensor_world_rot, const Eigen::Matrix & cog_world_rot); + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot); template void vec_to_eigen(const std::vector & data, T2 & matrix); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 3162a7587b..49279fe49f 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -176,7 +176,7 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); - // calculate needed rotations + // rotations needed for calculation Eigen::Matrix rot_base_sensor = admittance_transforms_.base_sensor_.rotation(); Eigen::Matrix rot_world_base = admittance_transforms_.world_base_.rotation(); Eigen::Matrix rot_base_cog = admittance_transforms_.base_cog_.rotation(); @@ -263,8 +263,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat // get admittance relative velocity auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); - // get external force - auto F_base = Eigen::Matrix(admittance_state.wrench_base.data()); + // external force expressed in the base frame + auto F_base = admittance_state.wrench_base; // zero out any forces in the control frame Eigen::Matrix F_control; @@ -282,8 +282,10 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat admittance_state.joint_acc); // add damping if cartesian velocity falls below threshold - for (size_t i = 0; i < admittance_state.joint_acc.size(); i++){ - admittance_state.joint_acc[i] -= parameters_.admittance.joint_damping*admittance_state.joint_vel[i]; + for (size_t i = 0; i < admittance_state.joint_acc.size(); i++) + { + admittance_state.joint_acc[i] -= + parameters_.admittance.joint_damping * admittance_state.joint_vel[i]; } // integrate motion in joint space diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a18dfb5e0a..8729062d05 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -289,10 +289,11 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( // initialize states read_state_from_hardware(joint_state_, ft_values_); - for (auto val : joint_state_.positions) { - if (std::isnan(val)) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); + for (auto val : joint_state_.positions) + { + if (std::isnan(val)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); return controller_interface::CallbackReturn::ERROR; } } @@ -376,12 +377,13 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( command_acc_ind = -1; // reset to prevent stale references - for (size_t i = 0; i < num_joints_; i++ ){ - position_reference_[i].get() = std::numeric_limits::quiet_NaN(); - velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); - } + for (size_t i = 0; i < num_joints_; i++) + { + position_reference_[i].get() = std::numeric_limits::quiet_NaN(); + velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + } - admittance_->reset(num_joints_); + admittance_->reset(num_joints_); return LifecycleNodeInterface::on_deactivate(previous_state); } From dacd8bbe1213be5555c073ae2e77793a73fd18dd Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 13:19:23 -0600 Subject: [PATCH 092/111] update descriptions for the admittance parameters --- .../src/admittance_controller_parameters.yaml | 52 +++++++++---------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 546d6ce93a..d98921d2d9 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -1,94 +1,94 @@ admittance_controller: joints: { type: string_array, - description: "specifies which joints will be used by the controller", + description: "Specifies which joints will be used by the controller. ", read_only: true } command_interfaces: { type: string_array, - description: "specifies which command interfaces to claim", + description: "Specifies which command interfaces the controller will claim.", read_only: true } state_interfaces: { type: string_array, - description: "specifies which state interfaces to claim", + description: "Specifies which state interfaces the controller will claim.", read_only: true } chainable_command_interfaces: { type: string_array, - description: "specifies which chainable interfaces to claim", + description: "Specifies which reference interfaces the controller will export. Normally, the position and velocity are used.", read_only: true } kinematics: plugin_name: { type: string, - description: "specifies which kinematics plugin to load" + description: "Specifies the name of the kinematics plugin to load." } plugin_package: { type: string, - description: "specifies the package to load the kinematics plugin from" + description: "Specifies the package name that contains the kinematics plugin." } base: { type: string, - description: "specifies the base link of the robot description used by the kinematics plugin" + description: "Specifies the base link of the robot description used by the kinematics plugin." } tip: { type: string, - description: "specifies the end effector link of the robot description used by the kinematics plugin" + description: "Specifies the end effector link of the robot description used by the kinematics plugin." } alpha: { type: double, default_value: 0.01, - description: "specifies the damping coefficient for the Jacobian pseudo inverse" + description: "Specifies the damping coefficient for the Jacobian pseudo inverse." } ft_sensor: name: { type: string, - description: "name of the force torque sensor in the robot description" + description: "Specifies the name of the force torque sensor in the robot description which will be used in the admittance calculation." } frame: id: { type: string, - description: "frame of the force torque sensor" + description: "Specifies the frame/link name of the force torque sensor." } filter_coefficient: { type: double, default_value: 0.05, - description: "specifies the coefficient for the sensor's exponential filter" + description: "Specifies the filter coefficient for the sensor's exponential filter." } control: frame: id: { type: string, - description: "control frame used for admittance control" + description: "Specifies the control frame used for admittance calculation." } fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) frame: id: { type: string, - description: "world frame, gravity points down (neg. Z) in this frame" + description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame." } gravity_compensation: frame: id: { type: string, - description: "frame which center of gravity (CoG) is defined in" + description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used." } - CoG: # specifies the center of gravity of the end effector + CoG: pos: { type: double_array, - description: "position of the center of gravity (CoG) in its frame", + description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.", validation: { fixed_size<>: 3 } @@ -96,21 +96,21 @@ admittance_controller: force: { type: double, default_value: 0.0, - description: "weight of the end effector, e.g mass * 9.81" + description: "Specifies the weight of the end effector, e.g mass * 9.81." } admittance: selected_axes: { type: bool_array, - description: "specifies if the axes x, y, z, rx, ry, and rz are enabled", + description: "Specifies whether the axes x, y, z, rx, ry, and rz should be included in the admittance calculation.", validation: { fixed_size<>: 6 } } mass: { type: double_array, - description: "specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation", + description: "Specifies the mass values for x, y, z, rx, ry, and rz used in the admittance calculation.", validation: { fixed_size<>: 6, element_bounds<>: [ 0.0001, 1000000.0 ] @@ -118,15 +118,15 @@ admittance_controller: } damping_ratio: { type: double_array, - description: "specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. - The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S ))", + description: "Specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. + The damping ratio is defined as: zeta = D / (2 * sqrt( M * S )).", validation: { fixed_size<>: 6 } } stiffness: { type: double_array, - description: "specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation", + description: "Specifies the stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation.", validation: { fixed_size<>: 6, element_bounds<>: [ 0.0, 100000000.0 ] @@ -134,7 +134,7 @@ admittance_controller: } joint_damping: { type: double, - description: "specifies joint damping applied when the cartesian velocity norm is less than min_velocity", + description: "Specifies the joint damping applied used in the admittance calculation.", default_value: 5, validation: { lower_bounds: [ 0.0 ] @@ -144,11 +144,11 @@ admittance_controller: # general settings robot_description: { type: string, - description: "Contains robot description in URDF format. The description is used to perform forward and inverse kinematics.", + description: "Contains robot description in URDF format. The description is used for forward and inverse kinematics.", read_only: true } enable_parameter_update_without_reactivation: { type: bool, default_value: true, - description: "if enabled, parameters will be dynamically updated in the control loop" + description: "If enabled, the parameters will be dynamically updated while the controller is running." } From ce583b014aa3ab2931799fe6e782ff8a386d8743 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 14:27:26 -0600 Subject: [PATCH 093/111] refactor indexing to be like JTC --- .../admittance_controller.hpp | 36 ++- .../src/admittance_controller.cpp | 219 ++++++++++++------ .../src/admittance_controller_parameters.yaml | 7 +- 3 files changed, 182 insertions(+), 80 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index ae34e4ccb8..db7d121489 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -97,15 +97,37 @@ class AdmittanceController : public controller_interface::ChainableControllerInt controller_interface::return_type update_reference_from_subscribers() override; size_t num_joints_ = 0; - size_t state_pos_ind = -1; - size_t state_vel_ind = -1; - size_t state_acc_ind = -1; - size_t command_pos_ind = -1; - size_t command_vel_ind = -1; - size_t command_acc_ind = -1; + std::vector command_joint_names_; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_state_interface_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION, + hardware_interface::HW_IF_EFFORT, + }; // internal reference values - const std::vector reference_interfaces_types_ = {"position", "velocity"}; + const std::vector reference_interfaces_types_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; std::vector> position_reference_; std::vector> velocity_reference_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 8729062d05..1e692e7852 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -44,6 +45,8 @@ controller_interface::CallbackReturn AdmittanceController::on_init() get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } + + // number of joints in controllers is fixed after initialization num_joints_ = admittance_->parameters_.joints.size(); // allocate dynamic memory @@ -156,7 +159,6 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( { parameter_handler_ = std::make_shared(get_node()); admittance_ = std::make_unique(parameter_handler_); - num_joints_ = admittance_->parameters_.joints.size(); } catch (const std::exception & e) { @@ -173,6 +175,8 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( { RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); } + + // validate exported interfaces for (const auto & tmp : admittance_->parameters_.chainable_command_interfaces) { if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) @@ -190,6 +194,76 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( } } + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + auto contains_interface_type = + [](const std::vector & interface_type_list, const std::string & interface_type) + { + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); + }; + + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Command interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_EFFORT); + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + // Note: 'effort' storage is also here, but never used. Still, for this is OK. + joint_state_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.state_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + + command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) + { + command_joint_names_ = admittance_->parameters_.joints; + RCLCPP_INFO( + get_node()->get_logger(), + "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != num_joints_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + auto get_interface_list = [](const std::vector & interface_types) { std::stringstream ss_command_interfaces; @@ -247,43 +321,40 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( return controller_interface::CallbackReturn::ERROR; } - // update parameters if any have changed - admittance_->apply_parameters_update(); - - // get indices for state and command interfaces - for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) + // order all joints in the storage + for (const auto & interface : admittance_->parameters_.command_interfaces) { - const auto & interface = admittance_->parameters_.state_interfaces[i]; - if (interface == hardware_interface::HW_IF_POSITION) - { - state_pos_ind = i; - } - else if (interface == hardware_interface::HW_IF_VELOCITY) + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { - state_vel_ind = i; - } - else if (interface == hardware_interface::HW_IF_ACCELERATION) - { - state_acc_ind = i; + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_, + interface.c_str(), joint_command_interface_[index].size()); + return CallbackReturn::ERROR; } } - for (size_t i = 0; i < admittance_->parameters_.command_interfaces.size(); ++i) + for (const auto & interface : admittance_->parameters_.state_interfaces) { - const auto & interface = admittance_->parameters_.command_interfaces[i]; - if (interface == hardware_interface::HW_IF_POSITION) - { - command_pos_ind = i; - } - else if (interface == hardware_interface::HW_IF_VELOCITY) - { - command_vel_ind = i; - } - else if (interface == hardware_interface::HW_IF_ACCELERATION) + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, admittance_->parameters_.joints, interface, + joint_state_interface_[index])) { - command_acc_ind = i; + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, + interface.c_str(), joint_state_interface_[index].size()); + return CallbackReturn::ERROR; } } + // update parameters if any have changed + admittance_->apply_parameters_update(); + // initialize interface of the FTS semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); @@ -298,11 +369,11 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } } + // Use current joint_state as a default reference last_reference_ = joint_state_; last_commanded_ = joint_state_; - - read_state_reference_interfaces(reference_); - reference_admittance_ = reference_; + reference_ = joint_state_; + reference_admittance_ = joint_state_; return controller_interface::CallbackReturn::SUCCESS; } @@ -365,17 +436,14 @@ controller_interface::return_type AdmittanceController::update_and_write_command controller_interface::CallbackReturn AdmittanceController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + // release force torque sensor interface force_torque_sensor_->release_interfaces(); - // reset indices - state_pos_ind = -1; - state_vel_ind = -1; - state_acc_ind = -1; - command_pos_ind = -1; - command_vel_ind = -1; - command_acc_ind = -1; - // reset to prevent stale references for (size_t i = 0; i < num_joints_; i++) { @@ -384,8 +452,9 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( } admittance_->reset(num_joints_); + release_interfaces(); - return LifecycleNodeInterface::on_deactivate(previous_state); + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn AdmittanceController::on_cleanup( @@ -413,28 +482,32 @@ void AdmittanceController::read_state_from_hardware( bool nan_position = false; bool nan_velocity = false; bool nan_acceleration = false; + + size_t pos_ind = 0; + size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t acc_ind = vel_ind + has_acceleration_state_interface_; for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { - for (size_t inter_ind = 0; inter_ind < 3; ++inter_ind) + if (has_position_state_interface_) { - auto interface_value = state_interfaces_[joint_ind + num_joints_ * inter_ind].get_value(); - if (inter_ind == state_pos_ind) - { - state_current.positions[joint_ind] = interface_value; - nan_position |= std::isnan(interface_value); - } - else if (inter_ind == state_vel_ind) - { - state_current.velocities[joint_ind] = interface_value; - nan_velocity |= std::isnan(interface_value); - } - else if (inter_ind == state_acc_ind) - { - state_current.accelerations[joint_ind] = interface_value; - nan_acceleration |= std::isnan(interface_value); - } + state_current.positions[joint_ind] = + state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); + nan_position |= std::isnan(state_current.positions[joint_ind]); + } + else if (has_velocity_state_interface_) + { + state_current.velocities[joint_ind] = + state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); + nan_velocity |= std::isnan(state_current.velocities[joint_ind]); + } + else if (has_acceleration_state_interface_) + { + state_current.accelerations[joint_ind] = + state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); + nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); } } + if (nan_position) { state_current.positions = last_commanded_.positions; @@ -463,23 +536,25 @@ void AdmittanceController::write_state_to_hardware( const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded) { // if any interface has nan values, assume state_commanded is the last command state + size_t pos_ind = 0; + size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t acc_ind = vel_ind + has_acceleration_state_interface_; for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { - for (size_t inter_ind = 0; inter_ind < 3; ++inter_ind) + if (has_position_command_interface_) { - auto ind = joint_ind + num_joints_ * inter_ind; - if (inter_ind == command_pos_ind) - { - command_interfaces_[ind].set_value(state_commanded.positions[joint_ind]); - } - else if (inter_ind == command_vel_ind) - { - command_interfaces_[ind].set_value(state_commanded.velocities[joint_ind]); - } - else if (inter_ind == command_acc_ind) - { - command_interfaces_[ind].set_value(state_commanded.accelerations[joint_ind]); - } + command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_velocity_command_interface_) + { + command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_acceleration_command_interface_) + { + command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); } } last_commanded_ = state_commanded; diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index d98921d2d9..5bdc40e398 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -4,7 +4,12 @@ admittance_controller: description: "Specifies which joints will be used by the controller. ", read_only: true } - + command_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies the joints for writing into another controllers reference. This parameter is only relevant when chaining the output of this controller to the input of another controller.", + read_only: true + } command_interfaces: { type: string_array, From d814028c976caa91ef34a3ff416953aee1a12e05 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 23 Aug 2022 19:20:14 -0600 Subject: [PATCH 094/111] clear joint_command_interface_ and joint_state_interface_ in deactivate --- .../src/admittance_controller.cpp | 65 ++++++++++--------- 1 file changed, 36 insertions(+), 29 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1e692e7852..517a4c351d 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -72,7 +72,7 @@ controller_interface::InterfaceConfiguration AdmittanceController::command_inter std::vector command_interfaces_config_names; for (const auto & interface : admittance_->parameters_.command_interfaces) { - for (const auto & joint : admittance_->parameters_.joints) + for (const auto & joint : command_joint_names_) { auto full_name = joint + "/" + interface; command_interfaces_config_names.push_back(full_name); @@ -166,6 +166,24 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } + + command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) + { + command_joint_names_ = admittance_->parameters_.joints; + RCLCPP_INFO( + get_node()->get_logger(), + "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != num_joints_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + + // print and validate interface types for (const auto & tmp : admittance_->parameters_.state_interfaces) { @@ -227,7 +245,6 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation - // Note: 'effort' storage is also here, but never used. Still, for this is OK. joint_state_interface_.resize(allowed_interface_types_.size()); for (const auto & interface : admittance_->parameters_.state_interfaces) { @@ -248,22 +265,6 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( has_acceleration_state_interface_ = contains_interface_type( admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); - command_joint_names_ = admittance_->parameters_.command_joints; - if (command_joint_names_.empty()) - { - command_joint_names_ = admittance_->parameters_.joints; - RCLCPP_INFO( - get_node()->get_logger(), - "No specific joint names are used for command interfaces. Using 'joints' parameter."); - } - else if (command_joint_names_.size() != num_joints_) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "'command_joints' parameter has to have the same size as 'joints' parameter."); - return CallbackReturn::FAILURE; - } - auto get_interface_list = [](const std::vector & interface_types) { std::stringstream ss_command_interfaces; @@ -322,36 +323,37 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } // order all joints in the storage - for (const auto & interface : admittance_->parameters_.command_interfaces) + for (const auto & interface : admittance_->parameters_.state_interfaces) { auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + state_interfaces_, admittance_->parameters_.joints, interface, + joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_, - interface.c_str(), joint_command_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, + interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } - for (const auto & interface : admittance_->parameters_.state_interfaces) + for (const auto & interface : admittance_->parameters_.command_interfaces) { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - state_interfaces_, admittance_->parameters_.joints, interface, - joint_state_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, - interface.c_str(), joint_state_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_, + interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } + // update parameters if any have changed admittance_->apply_parameters_update(); @@ -451,8 +453,13 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); } - admittance_->reset(num_joints_); + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } release_interfaces(); + admittance_->reset(num_joints_); return CallbackReturn::SUCCESS; } From c4ae3dacf61eeada003e84d6f36ac89c798a1f2b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 15:34:39 -0600 Subject: [PATCH 095/111] fix include for rolling --- .../admittance_controller/admittance_rule.hpp | 2 +- .../src/admittance_controller.cpp | 20 +++++++++---------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a1a0658d64..b05a924f36 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -33,7 +33,7 @@ #include "pluginlib/class_loader.hpp" #include "tf2_eigen/tf2_eigen.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_kdl/tf2_kdl/tf2_kdl.hpp" +#include "tf2_kdl/tf2_kdl.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 517a4c351d..39d923bf23 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -172,18 +172,17 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( { command_joint_names_ = admittance_->parameters_.joints; RCLCPP_INFO( - get_node()->get_logger(), - "No specific joint names are used for command interfaces. Using 'joints' parameter."); + get_node()->get_logger(), + "No specific joint names are used for command interfaces. Using 'joints' parameter."); } else if (command_joint_names_.size() != num_joints_) { RCLCPP_ERROR( - get_node()->get_logger(), - "'command_joints' parameter has to have the same size as 'joints' parameter."); + get_node()->get_logger(), + "'command_joints' parameter has to have the same size as 'joints' parameter."); return CallbackReturn::FAILURE; } - // print and validate interface types for (const auto & tmp : admittance_->parameters_.state_interfaces) { @@ -326,15 +325,15 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( for (const auto & interface : admittance_->parameters_.state_interfaces) { auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - state_interfaces_, admittance_->parameters_.joints, interface, - joint_state_interface_[index])) + state_interfaces_, admittance_->parameters_.joints, interface, + joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, - interface.c_str(), joint_state_interface_[index].size()); + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, + interface.c_str(), joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -353,7 +352,6 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } } - // update parameters if any have changed admittance_->apply_parameters_update(); From 3dc1816439e3f628cc6c470d217b3ad1135278f7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:03:57 -0600 Subject: [PATCH 096/111] update README --- admittance_controller/README.md | 138 +------------------------------- 1 file changed, 1 insertion(+), 137 deletions(-) diff --git a/admittance_controller/README.md b/admittance_controller/README.md index f8f35f8b4c..72e3582f40 100644 --- a/admittance_controller/README.md +++ b/admittance_controller/README.md @@ -7,140 +7,4 @@ In the non-chained mode, the controller listens to joint commands on the `~/join The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. ## Parameters: -admittance.damping_ratio -``` -Type: double array -Description: specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. The values are calculated as damping can be used instead: zeta = D / (2 * sqrt( M * S )) -``` - -admittance.mass -``` -Type: double array -Description: specifies mass values for x, y, z, rx, ry, and rz used in the admittance calculation -``` - -admittance.selected_axes -``` -Type: double array -Description: specifies if the axes x, y, z, rx, ry, and rz are enabled -``` - -admittance.stiffness -``` -Type: double array -Description: specifies stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation -``` - -chainable_command_interfaces -``` -Type: string array -Description: specifies which chainable interfaces to claim -``` - -command_interfaces -``` -Type: string array -Description: specifies which command interfaces to claim -``` - -control.frame.id -``` -Type: string -Description: control frame used for admittance control -``` - -enable_parameter_update_without_reactivation -``` -Type: boolean -Description: if enabled, parameters will be dynamically updated in the control loop -``` - -fixed_world_frame.frame.id -``` -Type: string -Description: world frame, gravity points down (neg. Z) in this frame -``` - -ft_sensor.filter_coefficient -``` -Type: double -Description: specifies the coefficient for the sensor's exponential filter -``` - -ft_sensor.frame.id -``` -Type: string -Description: frame of the force torque senso -``` - -ft_sensor.name -``` -Type: string -Description: name of the force torque sensor in the robot description -``` - -gravity_compensation.CoG.force -``` -Type: double -Description: weight of the end effector, e.g mass * 9.81 -``` - -gravity_compensation.CoG.pos -``` -Type: double array -Description: position of the center of gravity (CoG) in its frame -``` - -gravity_compensation.frame.id -``` -Type: string -Description: rame which center of gravity (CoG) is defined in -``` - -joints -``` -Type: string array -Description: specifies which joints will be used by the controller -``` - -kinematics.alpha -``` -Type: double -Description: specifies the damping coefficient for the Jacobian pseudo inverse -``` - -kinematics.base -``` -Type: string -Description: specifies the base link of the robot description used by the kinematics plugin -``` - -kinematics.plugin_name -``` -Type: string -Description: specifies which kinematics plugin to load -``` - -kinematics.plugin_package -``` -Type: string -Description: specifies the package to load the kinematics plugin from -``` - -kinematics.tip -``` -Type: string -Description: specifies the end effector link of the robot description used by the kinematics plugin -``` - -robot_description -``` -Type: string -Description: Contains robot description in URDF format. The description is used to perform forward and inverse kinematics. -``` - -state_interfaces -``` -Type: string array -Description: specifies which state interfaces to claim -``` +The admittance controller's uses the [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter definition file, located [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller. From 32877408031f216af824ffd13ad6a228b55773f6 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:19:58 -0600 Subject: [PATCH 097/111] update comments --- .../admittance_controller.hpp | 6 ++++-- .../admittance_controller/admittance_rule.hpp | 4 ---- .../admittance_rule_impl.hpp | 15 ++++++--------- 3 files changed, 10 insertions(+), 15 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index db7d121489..e69e9af612 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -22,6 +22,9 @@ #include #include +// include generated parameter library +#include "admittance_controller_parameters.hpp" + #include "admittance_controller/admittance_rule.hpp" #include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" @@ -121,8 +124,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt const std::vector allowed_interface_types_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, - hardware_interface::HW_IF_EFFORT, + hardware_interface::HW_IF_ACCELERATION }; // internal reference values diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index b05a924f36..00f2698329 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -24,7 +24,6 @@ #include #include -#include "admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "control_toolbox/filters.hpp" #include "controller_interface/controller_interface.hpp" @@ -205,9 +204,6 @@ class AdmittanceRule // ROS control_msgs::msg::AdmittanceControllerState state_message_; - std::shared_ptr clock_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 49279fe49f..9cd9c1c3ba 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -34,11 +34,6 @@ controller_interface::return_type AdmittanceRule::configure( { num_joints_ = num_joints; - // initialize ROS functions - clock_ = node->get_clock(); - tf_buffer_ = std::make_shared(clock_); - tf_listener_ = std::make_shared(*tf_buffer_); - // initialize memory and values to zero (non-realtime function) reset(num_joints); @@ -224,7 +219,8 @@ controller_interface::return_type AdmittanceRule::update( bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) { - // create stiffness matrix + // Create stiffness matrix in base frame. The values of admittance_state.stiffness correspond to the six diagonal + // elements of the stiffness matrix expressed in the control frame auto rot_base_control = admittance_state.rot_base_control; Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); @@ -236,7 +232,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat K.block<3, 3>(0, 0) = K_pos; K.block<3, 3>(3, 3) = K_rot; - // create damping matrix + // Create stiffness damping in base frame. The values of admittance_state.damping correspond to the six diagonal + // elements of the damping matrix expressed in the control frame. Eigen::Matrix D = Eigen::Matrix::Zero(); Eigen::Matrix D_pos = Eigen::Matrix::Zero(); Eigen::Matrix D_rot = Eigen::Matrix::Zero(); @@ -247,7 +244,7 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat D.block<3, 3>(0, 0) = D_pos; D.block<3, 3>(3, 3) = D_rot; - // calculate admittance relative offset + // calculate admittance relative offset in base frame Eigen::Isometry3d desired_trans_base_ft; kinematics_->calculate_link_transform( admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); @@ -274,7 +271,7 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); - // Compute admittance control law: F = M*a + D*v + S*x + // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x Eigen::Matrix X_ddot = admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( From bde3d3946bee597a04d7cb7774203751005e3be3 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:20:50 -0600 Subject: [PATCH 098/111] Update admittance_controller/include/admittance_controller/admittance_rule_impl.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../include/admittance_controller/admittance_rule_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 9cd9c1c3ba..94a9c64d25 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -289,7 +289,7 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat admittance_state.joint_vel += (admittance_state.joint_acc) * dt; admittance_state.joint_pos += admittance_state.joint_vel * dt; - // calculate admittance velocity corresponding to joint velocity + // calculate admittance velocity corresponding to joint velocity ("base_link" frame) success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( admittance_state.current_joint_pos, admittance_state.joint_vel, admittance_state.ft_sensor_frame, admittance_state.admittance_velocity); From f23b6a630e4c89c27d98a9a3221d4ba091ff5d7b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:21:29 -0600 Subject: [PATCH 099/111] Update admittance_controller/src/admittance_controller.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- admittance_controller/src/admittance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 39d923bf23..8dae583e55 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -121,7 +121,7 @@ AdmittanceController::on_export_reference_interfaces() } std::vector chainable_command_interfaces; - auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * + const auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); // allocate dynamic memory From bbdc506ab6654f9b5331a33a2be9b98ed8624cde Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:21:45 -0600 Subject: [PATCH 100/111] Update admittance_controller/src/admittance_controller.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- admittance_controller/src/admittance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 8dae583e55..11491ca1a1 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -142,7 +142,7 @@ AdmittanceController::on_export_reference_interfaces() { velocity_reference_.emplace_back(reference_interfaces_[index]); } - auto full_name = joint + "/" + interface; + const auto full_name = joint + "/" + interface; chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index++)); } From c45b668b1ea5146863a6c8db4f80c1273929b284 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:25:32 -0600 Subject: [PATCH 101/111] move dynamic allocations in state message to reset method --- .../admittance_rule_impl.hpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 94a9c64d25..1d88595bb6 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -87,6 +87,11 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) state_message_.selected_axes.data.resize(6, 0); state_message_.damping.data.resize(6, 0); state_message_.stiffness.data.resize(6, 0); + state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.child_frame_id = "admittance_offset"; // reset admittance state admittance_state_ = AdmittanceState(num_joints); @@ -342,8 +347,6 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control state_message_.mass.data[i] = admittance_state_.mass[i]; } - state_message_.wrench_base.header.frame_id = - parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; @@ -351,8 +354,6 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; - state_message_.admittance_velocity.header.frame_id = - parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; @@ -360,8 +361,6 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; - state_message_.admittance_acceleration.header.frame_id = - parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here state_message_.admittance_acceleration.twist.linear.x = admittance_state_.admittance_acceleration[0]; state_message_.admittance_acceleration.twist.linear.y = @@ -375,10 +374,6 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control state_message_.admittance_acceleration.twist.angular.z = admittance_state_.admittance_acceleration[5]; - state_message_.admittance_position.header.frame_id = - parameters_.kinematics.base; // TODO(anyone) remove dynamic allocation here - state_message_.admittance_position.child_frame_id = - "admittance_offset"; // TODO(anyone) remove dynamic allocation here state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; From ea590769e97e2e6ae9aef7ba2ac3969c26bcacf1 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:28:27 -0600 Subject: [PATCH 102/111] Update admittance_controller/src/admittance_controller.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- admittance_controller/src/admittance_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 11491ca1a1..c841ad41c2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -144,7 +144,9 @@ AdmittanceController::on_export_reference_interfaces() } const auto full_name = joint + "/" + interface; chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( - std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index++)); + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + + index++; } } From f94ca25bc4b27f461b7a59c649211054fc9ccad9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:29:17 -0600 Subject: [PATCH 103/111] Update admittance_controller/include/admittance_controller/admittance_controller.hpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../include/admittance_controller/admittance_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index e69e9af612..c0206a6d93 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -128,7 +128,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt }; // internal reference values - const std::vector reference_interfaces_types_ = { + const std::vector allowed_reference_interfaces_types_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; std::vector> position_reference_; std::vector> velocity_reference_; From 6a0281fbaaf1ed1fc46125a92d0e4447910c8196 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:39:10 -0600 Subject: [PATCH 104/111] Remove unneeded code and chnage field name --- admittance_controller/src/admittance_controller.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c841ad41c2..ee7db22df8 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -64,10 +64,6 @@ controller_interface::CallbackReturn AdmittanceController::on_init() controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() const { - if (!admittance_) - { - return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; - } std::vector command_interfaces_config_names; for (const auto & interface : admittance_->parameters_.command_interfaces) @@ -87,10 +83,6 @@ controller_interface::InterfaceConfiguration AdmittanceController::command_inter controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() const { - if (!admittance_) - { - return {controller_interface::interface_configuration_type::INDIVIDUAL, {}}; - } std::vector state_interfaces_config_names; for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) @@ -132,7 +124,7 @@ AdmittanceController::on_export_reference_interfaces() // assign reference interfaces auto index = 0ul; - for (const auto & interface : reference_interfaces_types_) + for (const auto & interface : allowed_reference_interfaces_types_) { for (const auto & joint : admittance_->parameters_.joints) { @@ -151,7 +143,7 @@ AdmittanceController::on_export_reference_interfaces() } return chainable_command_interfaces; - ; + } controller_interface::CallbackReturn AdmittanceController::on_configure( From 4346e3de40e3b6fcc51cf1f6e4fa89072fdd9324 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 16:55:10 -0600 Subject: [PATCH 105/111] update comments/docs --- admittance_controller/README.md | 2 +- .../admittance_controller.hpp | 6 +-- .../admittance_controller/admittance_rule.hpp | 37 +++++++++++-------- .../admittance_rule_impl.hpp | 5 +-- .../src/admittance_controller.cpp | 8 ++-- 5 files changed, 28 insertions(+), 30 deletions(-) diff --git a/admittance_controller/README.md b/admittance_controller/README.md index 72e3582f40..0b643e4049 100644 --- a/admittance_controller/README.md +++ b/admittance_controller/README.md @@ -7,4 +7,4 @@ In the non-chained mode, the controller listens to joint commands on the `~/join The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. ## Parameters: -The admittance controller's uses the [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter definition file, located [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller. +The admittance controller's uses the [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter definition file, located [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index c0206a6d93..f776646d42 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -122,10 +122,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants const std::vector allowed_interface_types_ = { - hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION - }; + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION}; // internal reference values const std::vector allowed_reference_interfaces_types_ = { diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 00f2698329..7f2355b2b2 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -41,13 +41,18 @@ namespace admittance_controller { struct AdmittanceTransforms { + // transformation from force torque sensor frame to base link frame at reference joint angles Eigen::Isometry3d ref_base_ft_; + // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles Eigen::Isometry3d base_ft_; + // transformation from control frame to base link frame at reference + admittance offset joint angles Eigen::Isometry3d base_control_; + // transformation from end effector frame to base link frame at reference + admittance offset joint angles Eigen::Isometry3d base_tip_; - Eigen::Isometry3d world_base_; - Eigen::Isometry3d base_sensor_; + // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles Eigen::Isometry3d base_cog_; + // transformation from world frame to base link frame + Eigen::Isometry3d world_base_; }; struct AdmittanceState @@ -109,9 +114,9 @@ class AdmittanceRule * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation * are calculated without an error - * \param[in] current_joint_state - * \param[in] reference_joint_state - * \param[out] success + * \param[in] current_joint_state current joint state of the robot + * \param[in] reference_joint_state input joint state reference + * \param[out] success true if no calls to the kinematics interface fail */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, @@ -127,11 +132,11 @@ class AdmittanceRule * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and * 'current_joint_state'. * - * \param[in] current_joint_state - * \param[in] measured_wrench - * \param[in] reference_joint_state - * \param[in] period - * \param[out] desired_joint_state + * \param[in] current_joint_state current joint state of the robot + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] reference_joint_state input joint state reference + * \param[in] period time in seconds since last controller update + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference */ controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, @@ -157,9 +162,9 @@ class AdmittanceRule * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin * calls failed. - * \param[in] admittance_state - * \param[in] dt - * \param[out] success + * \param[in] admittance_state contains all the information needed to calculate the admittance offset + * \param[in] dt controller period + * \param[out] success true if no calls to the kinematics interface fail */ bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); @@ -167,9 +172,9 @@ class AdmittanceRule * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. * The `wrench_world_` estimate includes gravity compensation - * \param[in] measured_wrench - * \param[in] sensor_world_rot - * \param[in] cog_world_rot + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame + * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 1d88595bb6..27639b0f1c 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -147,9 +147,6 @@ bool AdmittanceRule::get_all_transforms( success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.fixed_world_frame.frame.id, admittance_transforms_.world_base_); - success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.ft_sensor.frame.id, - admittance_transforms_.base_sensor_); success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.gravity_compensation.frame.id, admittance_transforms_.base_cog_); @@ -177,7 +174,7 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); // rotations needed for calculation - Eigen::Matrix rot_base_sensor = admittance_transforms_.base_sensor_.rotation(); + Eigen::Matrix rot_base_sensor = admittance_transforms_.base_ft_.rotation(); Eigen::Matrix rot_world_base = admittance_transforms_.world_base_.rotation(); Eigen::Matrix rot_base_cog = admittance_transforms_.base_cog_.rotation(); Eigen::Matrix rot_base_control = admittance_transforms_.base_control_.rotation(); diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index ee7db22df8..69d7ece1bb 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -64,7 +64,6 @@ controller_interface::CallbackReturn AdmittanceController::on_init() controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() const { - std::vector command_interfaces_config_names; for (const auto & interface : admittance_->parameters_.command_interfaces) { @@ -83,7 +82,6 @@ controller_interface::InterfaceConfiguration AdmittanceController::command_inter controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() const { - std::vector state_interfaces_config_names; for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) { @@ -113,8 +111,9 @@ AdmittanceController::on_export_reference_interfaces() } std::vector chainable_command_interfaces; - const auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * - admittance_->parameters_.joints.size(); + const auto num_chainable_interfaces = + admittance_->parameters_.chainable_command_interfaces.size() * + admittance_->parameters_.joints.size(); // allocate dynamic memory chainable_command_interfaces.reserve(num_chainable_interfaces); @@ -143,7 +142,6 @@ AdmittanceController::on_export_reference_interfaces() } return chainable_command_interfaces; - } controller_interface::CallbackReturn AdmittanceController::on_configure( From 82c0018385a732594a25b6634cb9a3443431a073 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 24 Aug 2022 17:01:05 -0600 Subject: [PATCH 106/111] remove tmp variables --- .../admittance_rule_impl.hpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 27639b0f1c..024b337876 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -173,26 +173,22 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); - // rotations needed for calculation - Eigen::Matrix rot_base_sensor = admittance_transforms_.base_ft_.rotation(); - Eigen::Matrix rot_world_base = admittance_transforms_.world_base_.rotation(); - Eigen::Matrix rot_base_cog = admittance_transforms_.base_cog_.rotation(); - Eigen::Matrix rot_base_control = admittance_transforms_.base_control_.rotation(); - // apply filter and update wrench_world_ vector - Eigen::Matrix rot_world_sensor = rot_world_base * rot_base_sensor; - Eigen::Matrix rot_world_cog = rot_world_base * rot_base_cog; + Eigen::Matrix rot_world_sensor = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); + Eigen::Matrix rot_world_cog = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); // transform wrench_world_ into base frame admittance_state_.wrench_base.block<3, 1>(0, 0) = - rot_world_base.transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); admittance_state_.wrench_base.block<3, 1>(3, 0) = - rot_world_base.transpose() * wrench_world_.block<3, 1>(3, 0); + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); // Compute admittance control law vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); - admittance_state_.rot_base_control = rot_base_control; + admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; success &= calculate_admittance_rule(admittance_state_, dt); From 2644a4e4be8171be7fd860178cbfd2ff240465d4 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 23 Sep 2022 12:48:29 -0500 Subject: [PATCH 107/111] Improve some comments --- .../include/admittance_controller/admittance_rule.hpp | 2 +- .../admittance_controller/admittance_rule_impl.hpp | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7f2355b2b2..0f0aabb063 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -148,7 +148,7 @@ class AdmittanceRule /** * Set fields of `state_message` from current admittance controller state. * - * \param[out] state_message + * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 024b337876..c12f3e030a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -217,21 +217,24 @@ controller_interface::return_type AdmittanceRule::update( bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) { - // Create stiffness matrix in base frame. The values of admittance_state.stiffness correspond to the six diagonal - // elements of the stiffness matrix expressed in the control frame + // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness correspond to the + // six diagonal elements of the stiffness matrix expressed in the control frame auto rot_base_control = admittance_state.rot_base_control; Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); Eigen::Matrix K_rot = Eigen::Matrix::Zero(); K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); + // Transform to the control frame + // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf + // Force Control by Luigi Villani and Joris De Schutter + // Page 200 K_pos = rot_base_control * K_pos * rot_base_control.transpose(); K_rot = rot_base_control * K_rot * rot_base_control.transpose(); K.block<3, 3>(0, 0) = K_pos; K.block<3, 3>(3, 3) = K_rot; - // Create stiffness damping in base frame. The values of admittance_state.damping correspond to the six diagonal - // elements of the damping matrix expressed in the control frame. + // The same for damping Eigen::Matrix D = Eigen::Matrix::Zero(); Eigen::Matrix D_pos = Eigen::Matrix::Zero(); Eigen::Matrix D_rot = Eigen::Matrix::Zero(); From 83c6e5d4ae400a96dcc46985a060c4008f51d6d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 10 Oct 2022 22:38:47 +0200 Subject: [PATCH 108/111] Updated documentation. --- admittance_controller/README.md | 10 ---- admittance_controller/doc/userdoc.rst | 57 +++++++++++++++++++ .../src/admittance_controller.cpp | 6 +- 3 files changed, 60 insertions(+), 13 deletions(-) delete mode 100644 admittance_controller/README.md create mode 100644 admittance_controller/doc/userdoc.rst diff --git a/admittance_controller/README.md b/admittance_controller/README.md deleted file mode 100644 index 0b643e4049..0000000000 --- a/admittance_controller/README.md +++ /dev/null @@ -1,10 +0,0 @@ -admittance_controller -========================================== - -The admittance controller operates in one of two modes: chained and non-chained. -In the non-chained mode, the controller listens to joint commands on the `~/joint_commands` topic to get a reference. In chained mode, the admittance controller receives its reference from another controller. In both modes, the controller applies the calculated admittance values to the received reference and write the modified reference to its command interfaces. - -The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. - -## Parameters: -The admittance controller's uses the [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter definition file, located [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller. diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst new file mode 100644 index 0000000000..447ea3e3f2 --- /dev/null +++ b/admittance_controller/doc/userdoc.rst @@ -0,0 +1,57 @@ +.. _joint_trajectory_controller_userdoc: + +Admittance Controller +====================== + +Admittance controller enables you do zero-force control from a force measured on your TCP. +The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``. + +The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. + +Parameters: + + + +ROS2 interface of the controller +--------------------------------- + +Parameters +^^^^^^^^^^^ + +The admittance controller's uses the [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. +The parameter definition file, located [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller. + + +Topics +^^^^^^^ + +~/reference (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] + Target joint commands when controller is not in chained mode. + +~/state (output topic) [control_msgs::msg::AdmittanceControllerState] + Topic publishing internal states. + + +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +The controller has ``position`` and ``velocity`` reference interfaces exported in the format: +``//[position|velocity]`` + + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +If some interface is not provided, the last commanded interface will be used for calculation. + +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. + + +Commands +^^^^^^^^^ +The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 69d7ece1bb..e6cf9c8b90 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -280,9 +280,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( { input_joint_command_.writeFromNonRT(msg); }; input_joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_commands", rclcpp::SystemDefaultsQoS(), joint_command_callback); + "~/reference", rclcpp::SystemDefaultsQoS(), joint_command_callback); s_publisher_ = get_node()->create_publisher( - "~/state", rclcpp::SystemDefaultsQoS()); + "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique>(s_publisher_); @@ -426,7 +426,7 @@ controller_interface::return_type AdmittanceController::update_and_write_command } controller_interface::CallbackReturn AdmittanceController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) + const rclcpp_lifecycle::State & /*previous_state*/) { if (!admittance_) { From 2e57e4aca552a6dfd70520f95b9aeb5fafd96df0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 10 Oct 2022 23:12:20 +0200 Subject: [PATCH 109/111] Fixing linters. --- admittance_controller/CMakeLists.txt | 44 ++++++++++--------- admittance_controller/doc/userdoc.rst | 2 +- .../admittance_rule_impl.hpp | 6 +-- .../src/admittance_controller.cpp | 2 +- .../test/test_admittance_controller.hpp | 16 ++++--- ros2_controllers-not-released.foxy.repos | 16 ------- ros2_controllers-not-released.galactic.repos | 16 ------- ros2_controllers-not-released.humble.repos | 14 +----- ros2_controllers-not-released.rolling.repos | 18 +------- ros2_controllers.foxy.repos | 16 ++----- ros2_controllers.galactic.repos | 16 ++----- ros2_controllers.humble.repos | 2 +- ros2_controllers.rolling.repos | 6 +-- 13 files changed, 50 insertions(+), 124 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 84aa025da9..bbfe19deda 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -43,9 +43,11 @@ target_include_directories(${PROJECT_NAME} PRIVATE include) generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") + pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) install(DIRECTORY include/ @@ -66,25 +68,25 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - # create custom test function to pass yaml file into test main - function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) - add_executable(${TARGET} ${SOURCES}) - _ament_cmake_gmock_find_gmock() - target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") - target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) - set(executable "$") - set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") - ament_add_test( - ${TARGET} - COMMAND ${executable} --ros-args --params-file ${YAML_FILE} - --gtest_output=xml:${result_file} - OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt - RESULT_FILE ${result_file} - ) - endfunction() + ## create custom test function to pass yaml file into test main + #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) + #add_executable(${TARGET} ${SOURCES}) + #_ament_cmake_gmock_find_gmock() + #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") + #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) + #set(executable "$") + #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") + #ament_add_test( + #${TARGET} + #COMMAND ${executable} --ros-args --params-file ${YAML_FILE} + #--gtest_output=xml:${result_file} + #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt + #RESULT_FILE ${result_file} + #) + #endfunction() # test loading admittance controller - add_test_with_yaml_input(test_load_admittance_controller test/test_load_admittance_controller.cpp + add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) @@ -95,7 +97,7 @@ if(BUILD_TESTING) ros2_control_test_assets ) # test admittance controller function - add_test_with_yaml_input(test_admittance_controller test/test_admittance_controller.cpp + add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) target_include_directories(test_admittance_controller PRIVATE include) target_link_libraries(test_admittance_controller admittance_controller) @@ -108,12 +110,12 @@ if(BUILD_TESTING) ) endif() -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) ament_export_include_directories( include ) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) ament_export_libraries( ${PROJECT_NAME} ) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 447ea3e3f2..760fd7ec6f 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -25,7 +25,7 @@ The parameter definition file, located [here](../admittance_controller/src/admit Topics ^^^^^^^ -~/reference (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] +~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] Target joint commands when controller is not in chained mode. ~/state (output topic) [control_msgs::msg::AdmittanceControllerState] diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index c12f3e030a..87c16e4787 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -217,8 +217,8 @@ controller_interface::return_type AdmittanceRule::update( bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) { - // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness correspond to the - // six diagonal elements of the stiffness matrix expressed in the control frame + // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness + // correspond to the six diagonal elements of the stiffness matrix expressed in the control frame auto rot_base_control = admittance_state.rot_base_control; Eigen::Matrix K = Eigen::Matrix::Zero(); Eigen::Matrix K_pos = Eigen::Matrix::Zero(); @@ -280,7 +280,7 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat admittance_state.joint_acc); // add damping if cartesian velocity falls below threshold - for (size_t i = 0; i < admittance_state.joint_acc.size(); i++) + for (int64_t i = 0; i < admittance_state.joint_acc.size(); ++i) { admittance_state.joint_acc[i] -= parameters_.admittance.joint_damping * admittance_state.joint_vel[i]; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index e6cf9c8b90..1af4e1317f 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -280,7 +280,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( { input_joint_command_.writeFromNonRT(msg); }; input_joint_command_subscriber_ = get_node()->create_subscription( - "~/reference", rclcpp::SystemDefaultsQoS(), joint_command_callback); + "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 40e96bd8b1..be78f05bb9 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -18,8 +18,12 @@ #define TEST_ADMITTANCE_CONTROLLER_HPP_ #include +#include #include #include +#include +#include +#include #include "gmock/gmock.h" @@ -139,11 +143,11 @@ class AdmittanceControllerTest : public ::testing::Test command_publisher_node_ = std::make_shared("command_publisher"); force_command_publisher_ = command_publisher_node_->create_publisher( - "/test_admittance_controller/force_commands", rclcpp::SystemDefaultsQoS()); - // pose_command_publisher_ = command_publisher_node_->create_publisher( - // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + "/test_admittance_controller/force_references", rclcpp::SystemDefaultsQoS()); + // pose_command_publisher_ =command_publisher_node_->create_publisher( + // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); joint_command_publisher_ = command_publisher_node_->create_publisher( - "/test_admittance_controller/joint_commands", rclcpp::SystemDefaultsQoS()); + "/test_admittance_controller/joint_references", rclcpp::SystemDefaultsQoS()); test_subscription_node_ = std::make_shared("test_subscription_node"); test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); @@ -275,7 +279,7 @@ class AdmittanceControllerTest : public ::testing::Test // create a new subscriber auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; auto subscription = test_subscription_node_->create_subscription( - "/test_admittance_controller/state", 10, subs_callback); + "/test_admittance_controller/status", 10, subs_callback); // call update to publish the test value ASSERT_EQ( @@ -450,7 +454,7 @@ class AdmittanceControllerTestParameterizedInvalidParameters static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } protected: - void SetUpController(bool val = false) + void SetUpController() { AdmittanceControllerTest::SetUpController("test_admittance_controller"); } diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 88a934f5c4..1b3910e7e7 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -4,19 +4,3 @@ repositories: # type: git # url: git@github.com:/.git # version: master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: master - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 88a934f5c4..1b3910e7e7 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -4,19 +4,3 @@ repositories: # type: git # url: git@github.com:/.git # version: master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: master - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 88a934f5c4..375781a422 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,19 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: master - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master + version: humble diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 743ab42a6a..375781a422 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -4,23 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: master - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + version: humble diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index d33679017c..d9a1c841a5 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -3,19 +3,11 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: foxy + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: foxy-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 281ac0c6e6..b9b8674fc9 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -3,19 +3,11 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: galactic + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: foxy-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - kinematics_interface: - type: git - url: https://github.com/ros-controls/kinematics_interface.git - version: master - control_msgs: - type: git - url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller - control_toolbox: - type: git - url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index d5ac4379aa..e6761e0ff1 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -14,7 +14,7 @@ repositories: control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller + version: humble control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index a566350538..3c942bd9d0 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -10,7 +10,7 @@ repositories: control_msgs: type: git url: https://github.com/pac48/control_msgs.git - version: add-admittance-controller + version: humble control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git @@ -20,7 +20,3 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master - generate_parameter_library: - type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main From 28c56651f730e931c3e57fb1e2e1275de6b0eba5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 10 Oct 2022 23:21:21 +0200 Subject: [PATCH 110/111] Small polishing --- admittance_controller/doc/userdoc.rst | 7 ++++--- admittance_controller/package.xml | 3 +-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 760fd7ec6f..60837457f9 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -6,7 +6,7 @@ Admittance Controller Admittance controller enables you do zero-force control from a force measured on your TCP. The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``. -The controller requires an external kinematics plugin to function. The [kinematics_interface](https://github.com/ros-controls/kinematics_interface) repository provides an interface and an implementation that the admittance controller can use. +The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. Parameters: @@ -18,8 +18,9 @@ ROS2 interface of the controller Parameters ^^^^^^^^^^^ -The admittance controller's uses the [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. -The parameter definition file, located [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller. +The admittance controller's 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. +An example parameter file can be found in the `test folder of the controller `_ Topics diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 3e355fdc89..e352dcfa91 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -10,12 +10,12 @@ ament_cmake - backward_ros control_msgs control_toolbox controller_interface kinematics_interface filters + generate_parameter_library geometry_msgs hardware_interface joint_trajectory_controller @@ -29,7 +29,6 @@ tf2_kdl tf2_ros trajectory_msgs - generate_parameter_library ament_cmake_gmock From e9c7432c640c1e986a6727231f47cd8ff52fdf39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 10 Oct 2022 23:23:28 +0200 Subject: [PATCH 111/111] Update upstream repo. --- ros2_controllers-not-released.humble.repos | 2 +- ros2_controllers-not-released.rolling.repos | 2 +- ros2_controllers.humble.repos | 2 +- ros2_controllers.rolling.repos | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 375781a422..b9cdead153 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -6,5 +6,5 @@ repositories: # version: master control_msgs: type: git - url: https://github.com/pac48/control_msgs.git + url: https://github.com/ros-controls/control_msgs.git version: humble diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 375781a422..b9cdead153 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -6,5 +6,5 @@ repositories: # version: master control_msgs: type: git - url: https://github.com/pac48/control_msgs.git + url: https://github.com/ros-controls/control_msgs.git version: humble diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index e6761e0ff1..8c0ba1ff8a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -13,7 +13,7 @@ repositories: version: master control_msgs: type: git - url: https://github.com/pac48/control_msgs.git + url: https://github.com/ros-controls/control_msgs.git version: humble control_toolbox: type: git diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 3c942bd9d0..f62ba87bf3 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -9,7 +9,7 @@ repositories: version: master control_msgs: type: git - url: https://github.com/pac48/control_msgs.git + url: https://github.com/ros-controls/control_msgs.git version: humble control_toolbox: type: git