From 79d5ea7e0dfe76db7c3f4c0492abfc68ca65dfec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 21 Apr 2021 23:01:17 +0200 Subject: [PATCH 01/99] Add shell of the controller with a test --- admittance_controller/CMakeLists.txt | 70 ++++++++ admittance_controller/README.md | 7 + .../admittance_controller.xml | 8 + .../admittance_controller.hpp | 64 ++++++++ .../visibility_control.h | 49 ++++++ admittance_controller/package.xml | 26 +++ .../src/admittance_controller.cpp | 150 ++++++++++++++++++ .../test/test_load_admittance_controller.cpp | 41 +++++ ros2_controllers/package.xml | 1 + 9 files changed, 416 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/visibility_control.h create mode 100644 admittance_controller/package.xml create mode 100644 admittance_controller/src/admittance_controller.cpp 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..843fd536fa --- /dev/null +++ b/admittance_controller/CMakeLists.txt @@ -0,0 +1,70 @@ +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(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +add_library( + admittance_controller + SHARED + src/admittance_controller.cpp +) +target_include_directories( + admittance_controller + PUBLIC + include +) +ament_target_dependencies( + admittance_controller + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) +# prevent pluginlib from using boost +target_compile_definitions(admittance_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +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 +) + +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_admittance_controller test/test_load_admittance_controller.cpp) + target_include_directories(test_admittance_controller PRIVATE include) + ament_target_dependencies( + test_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) +endif() + +ament_package() diff --git a/admittance_controller/README.md b/admittance_controller/README.md new file mode 100644 index 0000000000..a72be1a9fb --- /dev/null +++ b/admittance_controller/README.md @@ -0,0 +1,7 @@ +admittance_controller +========================================== + +Implemenation 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/admittance_controller.xml b/admittance_controller/admittance_controller.xml new file mode 100644 index 0000000000..944ba0ad75 --- /dev/null +++ b/admittance_controller/admittance_controller.xml @@ -0,0 +1,8 @@ + + + + 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..0bdf27ed71 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -0,0 +1,64 @@ +// 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__ADMITTANCE_CONTROLLER_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ + +#include +#include + +#include "admittance_controller/visibility_control.h" +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace admittance_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class AdmittanceController : public controller_interface::ControllerInterface +{ +public: + ADMITTANCE_CONTROLLER_PUBLIC + AdmittanceController(); + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::return_type init(const std::string & controller_name) 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 + controller_interface::return_type update() override; + +protected: + std::vector joint_names_; + std::vector interface_names_; +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_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..df84552b9a --- /dev/null +++ b/admittance_controller/package.xml @@ -0,0 +1,26 @@ + + + + admittance_controller + 0.0.0 + Implemenation of admittance controllers for different input and output interface. + Denis Štogl + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + ament_cmake_gmock + 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..958b377615 --- /dev/null +++ b/admittance_controller/src/admittance_controller.cpp @@ -0,0 +1,150 @@ +// 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. +// +/// \author: Denis Stogl + +#include +#include + +#include "admittance_controller/admittance_controller.hpp" + +namespace admittance_controller +{ +AdmittanceController::AdmittanceController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::return_type AdmittanceController::init(const std::string & controller_name) +{ + auto ret = ControllerInterface::init(controller_name); + if (ret != controller_interface::return_type::OK) { + return ret; + } + + try { + auto node = get_node(); + node->declare_parameter>("joints", {}); + node->declare_parameter("interface_name", ""); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +CallbackReturn AdmittanceController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joint_names_ = node_->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return CallbackReturn::ERROR; + } + + // Specialized, child controllers set interfaces before calling init function. + if (interface_names_.empty()) { + interface_names_ = node_->get_parameter("interface_names").as_string_array(); + } + + if (interface_names_.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + return CallbackReturn::ERROR; + } + + RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() +const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & joint : joint_names_) { + for (const auto & interface : interface_names_) { + command_interfaces_config.names.push_back(joint + "/" + interface); + } + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() +const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & joint : joint_names_) { + for (const auto & interface : interface_names_) { + state_interfaces_config.names.push_back(joint + "/" + interface); + } + } + + return state_interfaces_config; +} + +// Fill ordered_interfaces with references to the matching interfaces +// in the same order as in joint_names +template +bool get_ordered_interfaces( + std::vector & unordered_interfaces, + const std::vector & joint_names, + const std::string & interface_type, + std::vector> & ordered_interfaces) +{ + for (const auto & joint_name : joint_names) { + for (auto & command_interface : unordered_interfaces) { + if ((command_interface.get_name() == joint_name) && + (command_interface.get_interface_name() == interface_type)) + { + ordered_interfaces.push_back(std::ref(command_interface)); + } + } + } + + return joint_names.size() == ordered_interfaces.size(); +} + +CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return CallbackReturn::SUCCESS; +} + +CallbackReturn AdmittanceController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type AdmittanceController::update() +{ + for (auto index = 0ul; index < command_interfaces_.size(); ++index) { + command_interfaces_[index].set_value(state_interfaces_[index].get_value()); + } + + return controller_interface::return_type::OK; +} + +} // namespace admittance_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + admittance_controller::AdmittanceController, + controller_interface::ControllerInterface) 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")); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3ed8250aaf..9812d7488e 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -11,6 +11,7 @@ ament_cmake + admittance_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster From cf6f9191e44c31830866e0b2d0b846b0964dde0c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 09:53:10 -0500 Subject: [PATCH 02/99] Outline the admittance control pipeline Outline of a class for IK calculations More outline of incremental IK Begin hooking up with MoveIt --- admittance_controller/CMakeLists.txt | 5 +++ .../admittance_controller.hpp | 8 ++-- .../incremental_ik_calculator.hpp | 44 +++++++++++++++++++ admittance_controller/package.xml | 4 ++ .../src/admittance_controller.cpp | 28 ++++++++++++ .../src/incremental_ik_calculator.cpp | 25 +++++++++++ 6 files changed, 111 insertions(+), 3 deletions(-) create mode 100644 admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp create mode 100644 admittance_controller/src/incremental_ik_calculator.cpp diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 843fd536fa..2fa2b117cf 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -13,6 +13,8 @@ endif() find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) find_package(hardware_interface REQUIRED) +find_package(moveit_ros_move_group REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -21,6 +23,7 @@ add_library( admittance_controller SHARED src/admittance_controller.cpp + src/incremental_ik_calculator.cpp ) target_include_directories( admittance_controller @@ -31,6 +34,8 @@ ament_target_dependencies( admittance_controller controller_interface hardware_interface + moveit_ros_move_group + moveit_ros_planning_interface pluginlib rclcpp rclcpp_lifecycle diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 0bdf27ed71..047d523735 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -11,13 +11,13 @@ // 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 ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include -#include - +#include "admittance_controller/incremental_ik_calculator.hpp" #include "admittance_controller/visibility_control.h" #include "controller_interface/controller_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -57,6 +57,8 @@ class AdmittanceController : public controller_interface::ControllerInterface protected: std::vector joint_names_; std::vector interface_names_; + + IncrementalIKCalculator ik_; }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp new file mode 100644 index 0000000000..ed9c32b923 --- /dev/null +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -0,0 +1,44 @@ +// 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. +// +/// \author: Andy Zelenak + +#pragma once + +#include +#include + +namespace admittance_controller +{ + +class IncrementalIKCalculator +{ +public: + /** + * Create an object which takes Cartesian delta-x and converts to joint delta-theta. + */ + IncrementalIKCalculator(); + + bool convertCartesianDeltasToJointDeltas() + { + return true; + } + +private: + // MoveIt setup, required to retrieve the Jacobian + const moveit::core::JointModelGroupPtr joint_model_group_; + moveit::core::RobotStatePtr kinematic_state_; +}; + +} // namespace admittance_controller diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index df84552b9a..43c58b078f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -11,6 +11,8 @@ controller_interface hardware_interface + moveit_ros_move_group + moveit_ros_planning_interface pluginlib rclcpp rclcpp_lifecycle @@ -18,6 +20,8 @@ ament_cmake_gmock controller_manager hardware_interface + moveit_ros_move_group + moveit_ros_planning_interface ros2_control_test_assets diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 958b377615..267bff1783 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -134,6 +134,34 @@ CallbackReturn AdmittanceController::on_deactivate( controller_interface::return_type AdmittanceController::update() { + // Task space admittance pipeline: + // Get wrench measurement + // Calculate the desired Cartesian displacement of the robot with the admittance equation. + // The basic form is: F = K * (x_d - x) + // (wrench) = (stiffness matrix) * (desired_Cartesian_position minus current_Cartesian_position) + // Damping terms can be added too. + // Get current robot joint angles + // Convert Cartesian deltas to joint angle deltas via Jacobian + // Write new joint angles to robot + + + // Get wrench measurement + + // Calculate desired Cartesian displacement of the robot + + // Get current robot joint angles + + // Convert Cartesian deltas to joint angle deltas via Jacobian + if (!ik_.convertCartesianDeltasToJointDeltas()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); + return controller_interface::return_type::ERROR; + } + + // Write new joint angles to the robot + + + // TODO: below is just a skeleton for (auto index = 0ul; index < command_interfaces_.size(); ++index) { command_interfaces_[index].set_value(state_interfaces_[index].get_value()); } diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp new file mode 100644 index 0000000000..01e40564b9 --- /dev/null +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -0,0 +1,25 @@ +// 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. +// +/// \author: Andy Zelenak + +#include "admittance_controller/incremental_ik_calculator.hpp" + +namespace admittance_controller +{ +IncrementalIKCalculator::IncrementalIKCalculator() +{ + ; +} +} // namespace admittance_controller From f3497e31528c65c4809f8c81e99d2afacb202d6b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 11:28:25 -0500 Subject: [PATCH 03/99] Hook up with MoveIt --- .../admittance_controller/admittance_controller.hpp | 2 +- .../incremental_ik_calculator.hpp | 6 ++++-- admittance_controller/src/admittance_controller.cpp | 3 ++- .../src/incremental_ik_calculator.cpp | 10 ++++++++-- 4 files changed, 15 insertions(+), 6 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 047d523735..c9aed654e5 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -58,7 +58,7 @@ class AdmittanceController : public controller_interface::ControllerInterface std::vector joint_names_; std::vector interface_names_; - IncrementalIKCalculator ik_; + std::shared_ptr ik_; }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index ed9c32b923..ff46cf2ca9 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -27,8 +27,9 @@ class IncrementalIKCalculator public: /** * Create an object which takes Cartesian delta-x and converts to joint delta-theta. + * It uses the Jacobian from MoveIt. */ - IncrementalIKCalculator(); + IncrementalIKCalculator(std::shared_ptr& node); bool convertCartesianDeltasToJointDeltas() { @@ -37,8 +38,9 @@ class IncrementalIKCalculator private: // MoveIt setup, required to retrieve the Jacobian - const moveit::core::JointModelGroupPtr joint_model_group_; + const moveit::core::JointModelGroup* joint_model_group_; moveit::core::RobotStatePtr kinematic_state_; + std::shared_ptr node_; }; } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 267bff1783..7a475061a8 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -37,6 +37,7 @@ controller_interface::return_type AdmittanceController::init(const std::string & auto node = get_node(); node->declare_parameter>("joints", {}); node->declare_parameter("interface_name", ""); + ik_ = std::make_shared(node); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; @@ -152,7 +153,7 @@ controller_interface::return_type AdmittanceController::update() // Get current robot joint angles // Convert Cartesian deltas to joint angle deltas via Jacobian - if (!ik_.convertCartesianDeltasToJointDeltas()) + if (!ik_->convertCartesianDeltasToJointDeltas()) { RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); return controller_interface::return_type::ERROR; diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index 01e40564b9..a0437d70d6 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -18,8 +18,14 @@ namespace admittance_controller { -IncrementalIKCalculator::IncrementalIKCalculator() +IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& node) : node_(node) { - ; + // TODO(andyz): Parameterize robot description and joint group + std::unique_ptr model_loader_ptr = + std::unique_ptr(new robot_model_loader::RobotModelLoader(node_, "/robot_description", false /* do not load kinematics plugins */)); + const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); + // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe? + joint_model_group_ = kinematic_model->getJointModelGroup("gemini"); + kinematic_state_ = std::make_shared(kinematic_model); } } // namespace admittance_controller From 0fc5ed739773982c69d84cd9089e5d5adc6d0c3b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 11:48:39 -0500 Subject: [PATCH 04/99] Link against Eigen --- admittance_controller/CMakeLists.txt | 3 +++ .../admittance_controller/incremental_ik_calculator.hpp | 7 +++---- admittance_controller/src/admittance_controller.cpp | 6 +++++- admittance_controller/src/incremental_ik_calculator.cpp | 5 +++++ 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 2fa2b117cf..b7a72ba779 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -12,6 +12,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) +find_package(Eigen3 REQUIRED) find_package(hardware_interface REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) @@ -28,11 +29,13 @@ add_library( target_include_directories( admittance_controller PUBLIC + ${EIGEN3_INCLUDE_DIR} include ) ament_target_dependencies( admittance_controller controller_interface + ${Eigen_LIBRARIES} hardware_interface moveit_ros_move_group moveit_ros_planning_interface diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index ff46cf2ca9..22fe69cda3 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -16,6 +16,7 @@ #pragma once +#include #include #include @@ -31,10 +32,8 @@ class IncrementalIKCalculator */ IncrementalIKCalculator(std::shared_ptr& node); - bool convertCartesianDeltasToJointDeltas() - { - return true; - } + + bool convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, Eigen::VectorXd& delta_theta); private: // MoveIt setup, required to retrieve the Jacobian diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 7a475061a8..fe6853b178 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -18,6 +18,7 @@ #include #include "admittance_controller/admittance_controller.hpp" +#include "Eigen/Core" namespace admittance_controller { @@ -149,11 +150,13 @@ controller_interface::return_type AdmittanceController::update() // Get wrench measurement // Calculate desired Cartesian displacement of the robot + Eigen::VectorXd delta_x(6); // Get current robot joint angles // Convert Cartesian deltas to joint angle deltas via Jacobian - if (!ik_->convertCartesianDeltasToJointDeltas()) + Eigen::VectorXd delta_theta; + if (!ik_->convertCartesianDeltasToJointDeltas(delta_x, delta_theta)) { RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); return controller_interface::return_type::ERROR; @@ -162,6 +165,7 @@ controller_interface::return_type AdmittanceController::update() // Write new joint angles to the robot + // TODO: below is just a skeleton for (auto index = 0ul; index < command_interfaces_.size(); ++index) { command_interfaces_[index].set_value(state_interfaces_[index].get_value()); diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index a0437d70d6..a16bf82cd1 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -28,4 +28,9 @@ IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& joint_model_group_ = kinematic_model->getJointModelGroup("gemini"); kinematic_state_ = std::make_shared(kinematic_model); } + +bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, Eigen::VectorXd& delta_theta) +{ + return true; +} } // namespace admittance_controller From 968e5dbc6cc48b0df98e5d46fb0208710ce86722 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 12:36:49 -0500 Subject: [PATCH 05/99] Do the pseudoinverse calculation --- .../admittance_controller.hpp | 5 +++++ .../incremental_ik_calculator.hpp | 18 +++++++++++++++--- .../src/admittance_controller.cpp | 7 ++++--- .../src/incremental_ik_calculator.cpp | 15 ++++++++++++++- 4 files changed, 38 insertions(+), 7 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index c9aed654e5..b8032213f9 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -59,6 +59,11 @@ class AdmittanceController : public controller_interface::ControllerInterface std::vector interface_names_; std::shared_ptr ik_; + + Eigen::VectorXd delta_x_; + Eigen::VectorXd delta_theta_; + // TODO(andyz): parameterize this + std::string force_torque_sensor_frame_; }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index 22fe69cda3..3bce16be7b 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -27,19 +27,31 @@ class IncrementalIKCalculator { public: /** - * Create an object which takes Cartesian delta-x and converts to joint delta-theta. + * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ IncrementalIKCalculator(std::shared_ptr& node); - - bool convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, Eigen::VectorXd& delta_theta); + /** + * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. + * \return true if successful + */ + bool convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta); private: // MoveIt setup, required to retrieve the Jacobian const moveit::core::JointModelGroup* joint_model_group_; moveit::core::RobotStatePtr kinematic_state_; std::shared_ptr node_; + + // Pre-allocate for speed + Eigen::MatrixXd jacobian_; + Eigen::JacobiSVD svd_; + Eigen::MatrixXd matrix_s_; + Eigen::MatrixXd pseudo_inverse_; + + // TF frames + std::string moveit_jacobian_frame_; }; } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index fe6853b178..c71d979bfe 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -39,6 +39,8 @@ controller_interface::return_type AdmittanceController::init(const std::string & node->declare_parameter>("joints", {}); node->declare_parameter("interface_name", ""); ik_ = std::make_shared(node); + delta_x_ = Eigen::VectorXd(6); + force_torque_sensor_frame_ = "force_sensor"; } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; @@ -150,13 +152,12 @@ controller_interface::return_type AdmittanceController::update() // Get wrench measurement // Calculate desired Cartesian displacement of the robot - Eigen::VectorXd delta_x(6); + //delta_x_ = ... // Get current robot joint angles // Convert Cartesian deltas to joint angle deltas via Jacobian - Eigen::VectorXd delta_theta; - if (!ik_->convertCartesianDeltasToJointDeltas(delta_x, delta_theta)) + if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, force_torque_sensor_frame_, delta_theta_)) { RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); return controller_interface::return_type::ERROR; diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index a16bf82cd1..0a7060ec1e 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -27,10 +27,23 @@ IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe? joint_model_group_ = kinematic_model->getJointModelGroup("gemini"); kinematic_state_ = std::make_shared(kinematic_model); + + // By default, the MoveIt Jacobian frame is the last link + // TODO(andyz): parameterize this + moveit_jacobian_frame_ = "end_effector"; } -bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, Eigen::VectorXd& delta_theta) +bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta) { + // Transform delta_x to the moveit_jacobian_frame + + // Multiply with the pseudoinverse to get delta_theta + jacobian_ = kinematic_state_->getJacobian(joint_model_group_); + svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); + matrix_s_ = svd_.singularValues().asDiagonal(); + pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); + delta_theta = pseudo_inverse_ * delta_x; + return true; } } // namespace admittance_controller From 690562a1850b6990d994840990589a72f78d4d2a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 13:07:48 -0500 Subject: [PATCH 06/99] Set up transform buffer stuff --- .../incremental_ik_calculator.hpp | 4 ++++ .../src/incremental_ik_calculator.cpp | 13 ++++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index 3bce16be7b..a74f307a34 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace admittance_controller { @@ -51,7 +52,10 @@ class IncrementalIKCalculator Eigen::MatrixXd pseudo_inverse_; // TF frames + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; std::string moveit_jacobian_frame_; + geometry_msgs::msg::TransformStamped wrench_to_jacobian_transform_; }; } // namespace admittance_controller diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index 0a7060ec1e..1d43100332 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -29,13 +29,24 @@ IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& kinematic_state_ = std::make_shared(kinematic_model); // By default, the MoveIt Jacobian frame is the last link - // TODO(andyz): parameterize this + // TODO(andyz): parameterize this or retrieve it via API moveit_jacobian_frame_ = "end_effector"; + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); } bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta) { // Transform delta_x to the moveit_jacobian_frame + try + { + wrench_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero); + } + catch (tf2::TransformException & ex) + { + RCLCPP_ERROR(node_->get_logger(), "Transformation of wrench failed."); + return false; + } // Multiply with the pseudoinverse to get delta_theta jacobian_ = kinematic_state_->getJacobian(joint_model_group_); From 6ea8b7685c8df69cd6352770a73f9cf892c3e9ac Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 13:37:35 -0500 Subject: [PATCH 07/99] Transform the incoming wrench --- .../incremental_ik_calculator.hpp | 6 +++++- .../src/incremental_ik_calculator.cpp | 14 +++++++++++++- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index a74f307a34..84e99326dd 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include namespace admittance_controller @@ -35,9 +36,12 @@ class IncrementalIKCalculator /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. + * \param delta_x input Cartesian deltas + * \param delta_x_frame input name of the delta_x tf frame + * \param delta_theta output * \return true if successful */ - bool convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta); + bool convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta); private: // MoveIt setup, required to retrieve the Jacobian diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index 1d43100332..58f506de7d 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -35,12 +35,24 @@ IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& tf_listener_ = std::make_shared(*tf_buffer_); } -bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta) +bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta) { // Transform delta_x to the moveit_jacobian_frame + Eigen::Vector3d translation = delta_x.head(3); + Eigen::Vector3d rotation = delta_x.tail(3); try { wrench_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero); + + tf2::doTransform(translation, translation, wrench_to_jacobian_transform_); + tf2::doTransform(rotation, rotation, wrench_to_jacobian_transform_); + // Back to 6x1 vector + delta_x[0] = translation[0]; + delta_x[1] = translation[1]; + delta_x[2] = translation[2]; + delta_x[0] = translation[0]; + delta_x[1] = translation[1]; + delta_x[2] = translation[2]; } catch (tf2::TransformException & ex) { From 9511ea0b33a7d61ffc54237fcdb661a4e9aa46b2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 15:01:46 -0500 Subject: [PATCH 08/99] Correct the wrench transformation --- .../incremental_ik_calculator.hpp | 2 +- .../src/incremental_ik_calculator.cpp | 31 ++++++++++++------- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index 84e99326dd..8533e45eaf 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -59,7 +59,7 @@ class IncrementalIKCalculator std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; std::string moveit_jacobian_frame_; - geometry_msgs::msg::TransformStamped wrench_to_jacobian_transform_; + geometry_msgs::msg::TransformStamped delta_x_to_jacobian_transform_; }; } // namespace admittance_controller diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index 58f506de7d..02488298d1 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -38,21 +38,28 @@ IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta) { // Transform delta_x to the moveit_jacobian_frame - Eigen::Vector3d translation = delta_x.head(3); - Eigen::Vector3d rotation = delta_x.tail(3); try { - wrench_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero); + // 4x4 transformation matrix + delta_x_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero); + Eigen::Isometry3d affine_transform = tf2::transformToEigen(delta_x_to_jacobian_transform_); - tf2::doTransform(translation, translation, wrench_to_jacobian_transform_); - tf2::doTransform(rotation, rotation, wrench_to_jacobian_transform_); - // Back to 6x1 vector - delta_x[0] = translation[0]; - delta_x[1] = translation[1]; - delta_x[2] = translation[2]; - delta_x[0] = translation[0]; - delta_x[1] = translation[1]; - delta_x[2] = translation[2]; + // Build the 6x6 adjoint of the transformation matrix + Eigen::MatrixXd twist_transform(6,6); + // upper left 3x3 block is the rotation part + twist_transform.block(0,0,3,3) = affine_transform.rotation(); + // upper right 3x3 block is all zeros + twist_transform.block(0,4,3,3) = Eigen::MatrixXd::Zero(3,3); + // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf + Eigen::MatrixXd adjoint(3,3); + adjoint(0,0) = 0; adjoint(0,1) = -affine_transform.translation().z(); adjoint(0,2) = affine_transform.translation().y(); + adjoint(1, 0) = affine_transform.translation().z(); adjoint(1,1) = 0; adjoint(1,2) = -affine_transform.translation().x(); + adjoint(2, 0) = -affine_transform.translation().y(); adjoint(2,1) = affine_transform.translation().x(); adjoint(1,2) = 0; + twist_transform.block(3,0,3,3) = adjoint * affine_transform.rotation(); + // lower right 3x3 block is the rotation part + twist_transform.block(3,3,3,3) = affine_transform.rotation(); + + delta_x = twist_transform * delta_x; } catch (tf2::TransformException & ex) { From b766b29a29fa8edb4206914306adc3cbd82f9a2d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 28 Apr 2021 15:19:43 -0500 Subject: [PATCH 09/99] Minor renaming to be technically correct --- .../src/incremental_ik_calculator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index 02488298d1..ace07c39ee 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -44,18 +44,18 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorX delta_x_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero); Eigen::Isometry3d affine_transform = tf2::transformToEigen(delta_x_to_jacobian_transform_); - // Build the 6x6 adjoint of the transformation matrix + // Build the 6x6 transformation matrix Eigen::MatrixXd twist_transform(6,6); // upper left 3x3 block is the rotation part twist_transform.block(0,0,3,3) = affine_transform.rotation(); // upper right 3x3 block is all zeros twist_transform.block(0,4,3,3) = Eigen::MatrixXd::Zero(3,3); // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf - Eigen::MatrixXd adjoint(3,3); - adjoint(0,0) = 0; adjoint(0,1) = -affine_transform.translation().z(); adjoint(0,2) = affine_transform.translation().y(); - adjoint(1, 0) = affine_transform.translation().z(); adjoint(1,1) = 0; adjoint(1,2) = -affine_transform.translation().x(); - adjoint(2, 0) = -affine_transform.translation().y(); adjoint(2,1) = affine_transform.translation().x(); adjoint(1,2) = 0; - twist_transform.block(3,0,3,3) = adjoint * affine_transform.rotation(); + Eigen::MatrixXd pos_vector_3x3(3,3); + pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y(); + pos_vector_3x3(1, 0) = affine_transform.translation().z(); pos_vector_3x3(1,1) = 0; pos_vector_3x3(1,2) = -affine_transform.translation().x(); + pos_vector_3x3(2, 0) = -affine_transform.translation().y(); pos_vector_3x3(2,1) = affine_transform.translation().x(); pos_vector_3x3(1,2) = 0; + twist_transform.block(3,0,3,3) = pos_vector_3x3 * affine_transform.rotation(); // lower right 3x3 block is the rotation part twist_transform.block(3,3,3,3) = affine_transform.rotation(); From 86914536b76028887998e0fd871e18d7396ead0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 6 May 2021 10:16:36 +0200 Subject: [PATCH 10/99] Testable version of Admittance Controller. IK not yet enables, so there is not output. --- admittance_controller/CMakeLists.txt | 58 ++- .../admittance_controller.hpp | 73 +++- .../admittance_controller/admittance_rule.hpp | 162 ++++++++ .../incremental_ik_calculator.hpp | 3 +- admittance_controller/package.xml | 11 +- .../src/admittance_controller.cpp | 379 ++++++++++++++--- admittance_controller/src/admittance_rule.cpp | 286 +++++++++++++ .../src/incremental_ik_calculator.cpp | 2 + .../test/test_admittance_controller.cpp | 368 +++++++++++++++++ .../test/test_admittance_controller.hpp | 389 ++++++++++++++++++ doc/admittance_controller.rst | 58 +++ 11 files changed, 1710 insertions(+), 79 deletions(-) create mode 100644 admittance_controller/include/admittance_controller/admittance_rule.hpp create mode 100644 admittance_controller/src/admittance_rule.cpp create mode 100644 admittance_controller/test/test_admittance_controller.cpp create mode 100644 admittance_controller/test/test_admittance_controller.hpp create mode 100644 doc/admittance_controller.rst diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index b7a72ba779..797942911c 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -11,37 +11,55 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) +find_package(iirob_filters REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_ros_planning_interface 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) add_library( admittance_controller SHARED src/admittance_controller.cpp + src/admittance_rule.cpp src/incremental_ik_calculator.cpp ) target_include_directories( admittance_controller PUBLIC + $ + $ ${EIGEN3_INCLUDE_DIR} - include ) ament_target_dependencies( admittance_controller + control_msgs controller_interface ${Eigen_LIBRARIES} + geometry_msgs hardware_interface + iirob_filters moveit_ros_move_group moveit_ros_planning_interface pluginlib rclcpp rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros ) # prevent pluginlib from using boost target_compile_definitions(admittance_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") @@ -65,14 +83,46 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_admittance_controller test/test_load_admittance_controller.cpp) - target_include_directories(test_admittance_controller PRIVATE include) + 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_admittance_controller + 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 + ) endif() +ament_export_include_directories( + include +) +ament_export_libraries( + admittance_controller +) +ament_export_dependencies( + control_msgs + controller_interface + geometry_msgs + hardware_interface + iirob_filters + 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 b8032213f9..fc815e14e2 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -17,11 +17,25 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include "admittance_controller/incremental_ik_calculator.hpp" +#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/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 "iirob_filters/low_pass_filter.h" +#include "iirob_filters/gravity_compensation.h" +#include "semantic_components/force_torque_sensor.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" namespace admittance_controller { @@ -56,14 +70,59 @@ class AdmittanceController : public controller_interface::ControllerInterface protected: std::vector joint_names_; - std::vector interface_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + std::string ft_sensor_name_; + + // Internal variables + std::unique_ptr force_torque_sensor_; + + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + rclcpp::Time previous_time_; + + // Command subscribers and Controller State publisher + using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; + using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; + + rclcpp::Subscription::SharedPtr + input_force_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + input_pose_command_subscriber_ = nullptr; + + realtime_tools::RealtimeBuffer> + input_force_command_; + realtime_tools::RealtimeBuffer> + input_pose_command_; + + using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // Internal access to sorted interfaces + + // 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, + }; + + // 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_; - std::shared_ptr ik_; + bool has_velocity_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; - Eigen::VectorXd delta_x_; - Eigen::VectorXd delta_theta_; - // TODO(andyz): parameterize this - std::string force_torque_sensor_frame_; }; } // namespace admittance_controller 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..f21bf98c8f --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -0,0 +1,162 @@ +// 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. +// +/// \author: Denis Stogl + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ + +#include "admittance_controller/incremental_ik_calculator.hpp" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +namespace admittance_controller +{ +class AdmittanceRule +{ +public: + AdmittanceRule() = default; + + controller_interface::return_type configure(rclcpp::Clock::SharedPtr clock); + + controller_interface::return_type update( + const std::vector & current_joint_state, + const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::PoseStamped & target_pose, + const geometry_msgs::msg::WrenchStamped & target_force, + const rclcpp::Duration & period + ); + + controller_interface::return_type update( + const std::vector & current_joint_state, + const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::PoseStamped & target_pose, + const rclcpp::Duration & period); + +// controller_interface::return_type update( +// const geometry_msgs::msg::WrenchStamped & measured_force, +// const geometry_msgs::msg::PoseStamped & target_pose, +// const geometry_msgs::msg::PoseStamped & current_pose, +// const rclcpp::Duration & period, +// geometry_msgs::msg::TransformStamped & relative_desired_pose_vec +// ); + + controller_interface::return_type get_controller_state( + control_msgs::msg::AdmittanceControllerState & state_message + ); + + controller_interface::return_type reset(); + +public: + // IK related parameters + std::string ik_base_frame_; + std::string ik_tip_frame_; + std::string ik_group_name_; + + // Controller frames + std::string control_frame_; + std::string endeffector_frame_; + std::string fixed_world_frame_; + std::string sensor_frame_; + + // Admittance parameters + // bool unified_mode_ = false; + std::array selected_axes_; + std::array mass_; + std::array damping_; + std::array stiffness_; + +protected: + // IK variables + std::shared_ptr ik_; + Eigen::VectorXd delta_x_; + Eigen::VectorXd delta_theta_; + + // Transformation variables + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + geometry_msgs::msg::WrenchStamped measured_force_; + geometry_msgs::msg::WrenchStamped measured_force_control_frame_; + + geometry_msgs::msg::PoseStamped current_origin_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::PoseStamped current_pose_control_frame_; + + geometry_msgs::msg::WrenchStamped target_force_control_frame_; + geometry_msgs::msg::PoseStamped target_pose_control_frame_; + + geometry_msgs::msg::PoseStamped desired_pose_; + geometry_msgs::msg::TransformStamped relative_desired_pose_; + + // Pre-reserved update-loop variables + std::array measured_force_control_frame_vec_; + std::array target_pose_control_frame_vec_; + std::array current_pose_control_frame_vec_; + + std::array angles_error_; + + std::array relative_desired_pose_vec_; + std::array desired_velocity_vec_; + std::array desired_velocity_previous_vec_; + std::array desired_acceleration_previous_vec_; + +private: + // TODO: implement doTransform for WrenchStamped + template +// using MsgType = geometry_msgs::msg::PoseStamped; + void transform_message_to_control_frame(const MsgType & message_in, MsgType & message_out) + { + if (control_frame_ != message_in.header.frame_id) { + geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( + control_frame_, message_in.header.frame_id, tf2::TimePointZero); + tf2::doTransform(message_in, message_out, transform); + } else { + message_out = message_in; + } + } + +// using MsgType2 = geometry_msgs::msg::WrenchStamped; +// void transform_message_to_control_frame(const MsgType2 & message_in, MsgType2 & message_out) +// { +// if (control_frame_ != message_in.header.frame_id) { +// geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( +// control_frame_, message_in.header.frame_id, tf2::TimePointZero); +// +// geometry_msgs::msg::Vector3Stamped vec_in; +// geometry_msgs::msg::Vector3Stamped vec_out; +// +// vec_in.vector = message_in.wrench.force; +// vec_in.header = message_in.header; +// tf2::doTransform(vec_in, vec_out, transform); +// message_out.wrench.force = vec_out.vector; +// +// vec_in.vector = message_in.wrench.torque; +// tf2::doTransform(vec_in, vec_out, transform); +// message_out.wrench.torque = vec_out.vector; +// } else { +// message_out = message_in; +// } +// } +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index 8533e45eaf..2bfd17a4b8 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include namespace admittance_controller @@ -32,7 +31,7 @@ class IncrementalIKCalculator * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ - IncrementalIKCalculator(std::shared_ptr& node); + IncrementalIKCalculator(std::shared_ptr & node); /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 43c58b078f..456296e972 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -9,19 +9,26 @@ ament_cmake + control_msgs controller_interface + geometry_msgs hardware_interface + iirob_filters moveit_ros_move_group moveit_ros_planning_interface pluginlib rclcpp rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros ament_cmake_gmock + control_msgs controller_manager hardware_interface - moveit_ros_move_group - moveit_ros_planning_interface ros2_control_test_assets diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c71d979bfe..78d270cb36 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -14,11 +14,16 @@ // /// \author: Denis Stogl +#include +#include #include #include +// #include "Eigen/Core" + #include "admittance_controller/admittance_controller.hpp" -#include "Eigen/Core" +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" namespace admittance_controller { @@ -35,40 +40,249 @@ controller_interface::return_type AdmittanceController::init(const std::string & } try { - auto node = get_node(); - node->declare_parameter>("joints", {}); - node->declare_parameter("interface_name", ""); - ik_ = std::make_shared(node); - delta_x_ = Eigen::VectorXd(6); - force_torque_sensor_frame_ = "force_sensor"; + get_node()->declare_parameter>("joints", {}); + get_node()->declare_parameter>("command_interfaces", {}); + get_node()->declare_parameter>("state_interfaces", {}); + get_node()->declare_parameter("ft_sensor_name", ""); + + get_node()->declare_parameter("IK.base", ""); + get_node()->declare_parameter("IK.tip", ""); + // TODO(destogl): enable when IK support is added +// get_node()->declare_parameter("IK.plugin", ""); +// get_node()->declare_parameter("IK.group_name", ""); + + get_node()->declare_parameter("control_frame", ""); + get_node()->declare_parameter("endeffector_frame", ""); + get_node()->declare_parameter("fixed_world_frame", ""); + get_node()->declare_parameter("sensor_frame", ""); + + get_node()->declare_parameter>("gravity_compensation.masses", {}); + get_node()->declare_parameter>("gravity_compensation.center_of_masses", {}); + + // TODO(destogl): enable when force/position control is implemented +// get_node()->declare_parameter("admitance.unified_mode", false); + get_node()->declare_parameter("admittance.selected_axes.x", false); + get_node()->declare_parameter("admittance.selected_axes.y", false); + get_node()->declare_parameter("admittance.selected_axes.z", false); + get_node()->declare_parameter("admittance.selected_axes.rx", false); + get_node()->declare_parameter("admittance.selected_axes.ry", false); + get_node()->declare_parameter("admittance.selected_axes.rz", false); + + get_node()->declare_parameter("admittance.mass.x", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.y", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.z", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.rx", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.ry", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.mass.rz", std::numeric_limits::quiet_NaN()); + + get_node()->declare_parameter("admittance.damping.x", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.y", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.z", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.rx", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.ry", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.damping.rz", std::numeric_limits::quiet_NaN()); + + get_node()->declare_parameter("admittance.stiffness.x", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.y", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.z", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.rx", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.ry", std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("admittance.stiffness.rz", std::numeric_limits::quiet_NaN()); + +// get_node()->declare_parameter>("") } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } + admittance_ = std::make_unique(); + + auto node = get_node(); +// ik_ = std::make_shared(node); +// delta_x_ = Eigen::VectorXd(6); + return controller_interface::return_type::OK; } CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - joint_names_ = node_->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + auto error_if_empty = [&](const auto & parameter, const char * parameter_name) { + if (parameter.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was empty", parameter_name); + return true; + } + return false; + }; + + auto get_string_array_param_and_error_if_empty = [&]( + std::vector & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string_array(); + return error_if_empty(parameter, parameter_name); + }; + + auto get_string_param_and_error_if_empty = [&]( + std::string & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string(); + return error_if_empty(parameter, parameter_name); + }; + + // TODO(destogl): If we would use C++20 than we can use templates here + auto get_bool_param_and_error_if_empty = [&]( + bool & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).get_value(); + return false; // TODO(destogl): how to check "if_empty" for bool? + }; + + auto get_double_param_and_error_if_empty = [&]( + double & parameter, const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).get_value(); + if (std::isnan(parameter)) { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was not set", parameter_name); + return true; + } + return false; + }; + + if ( + get_string_array_param_and_error_if_empty(joint_names_, "joints") || + get_string_array_param_and_error_if_empty(command_interface_types_, "command_interfaces") || + get_string_array_param_and_error_if_empty(state_interface_types_, "state_interfaces") || + get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || + get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || + get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || + // TODO: Enable this when IK plugin use is implemented + // get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || + get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") || + get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") || + get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") || + get_string_param_and_error_if_empty(admittance_->sensor_frame_, "sensor_frame") || + // TODO(destogl): add unified mode considering target force +// get_bool_param_and_error_if_empty(unified_mode_, "admittance.unified_mode") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[0], "admittance.selected_axes.x") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[1], "admittance.selected_axes.y") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[2], "admittance.selected_axes.z") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[3], "admittance.selected_axes.rx") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[4], "admittance.selected_axes.ry") || + get_bool_param_and_error_if_empty(admittance_->selected_axes_[5], "admittance.selected_axes.rz") || + + get_double_param_and_error_if_empty(admittance_->mass_[0], "admittance.mass.x") || + get_double_param_and_error_if_empty(admittance_->mass_[1], "admittance.mass.y") || + get_double_param_and_error_if_empty(admittance_->mass_[2], "admittance.mass.z") || + get_double_param_and_error_if_empty(admittance_->mass_[3], "admittance.mass.rx") || + get_double_param_and_error_if_empty(admittance_->mass_[4], "admittance.mass.ry") || + get_double_param_and_error_if_empty(admittance_->mass_[5], "admittance.mass.rz") || + + get_double_param_and_error_if_empty(admittance_->damping_[0], "admittance.damping.x") || + get_double_param_and_error_if_empty(admittance_->damping_[1], "admittance.damping.y") || + get_double_param_and_error_if_empty(admittance_->damping_[2], "admittance.damping.z") || + get_double_param_and_error_if_empty(admittance_->damping_[3], "admittance.damping.rx") || + get_double_param_and_error_if_empty(admittance_->damping_[4], "admittance.damping.ry") || + get_double_param_and_error_if_empty(admittance_->damping_[5], "admittance.damping.rz") || + + get_double_param_and_error_if_empty(admittance_->stiffness_[0], "admittance.stiffness.x") || + get_double_param_and_error_if_empty(admittance_->stiffness_[1], "admittance.stiffness.y") || + get_double_param_and_error_if_empty(admittance_->stiffness_[2], "admittance.stiffness.z") || + get_double_param_and_error_if_empty(admittance_->stiffness_[3], "admittance.stiffness.rx") || + get_double_param_and_error_if_empty(admittance_->stiffness_[4], "admittance.stiffness.ry") || + get_double_param_and_error_if_empty(admittance_->stiffness_[5], "admittance.stiffness.rz") + ) + { return CallbackReturn::ERROR; } - // Specialized, child controllers set interfaces before calling init function. - if (interface_names_.empty()) { - interface_names_ = node_->get_parameter("interface_names").as_string_array(); + // 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(get_node()->get_logger(), "Command interface type '" + interface + "' not allowed!"); + return CallbackReturn::ERROR; + } + } + + if (controller_interface::interface_list_contains_interface_type( + command_interface_types_, hardware_interface::HW_IF_POSITION)) { + has_position_command_interface_ = true; + } + if (controller_interface::interface_list_contains_interface_type( + command_interface_types_, hardware_interface::HW_IF_VELOCITY)) { + has_velocity_command_interface_ = true; } - if (interface_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + 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(get_node()->get_logger(), "State interface type '" + interface + "' not allowed!"); + return CallbackReturn::ERROR; + } + } + + if (!controller_interface::interface_list_contains_interface_type( + state_interface_types_, hardware_interface::HW_IF_POSITION)) { + RCLCPP_ERROR(get_node()->get_logger(), "State interface type '" + std::string(hardware_interface::HW_IF_POSITION) + "' has to be always present allowed!"); return CallbackReturn::ERROR; } + if (controller_interface::interface_list_contains_interface_type( + state_interface_types_, hardware_interface::HW_IF_VELOCITY)) { + has_velocity_state_interface_ = true; + } + + 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(); + }; + + // Print output so users can be sure the interface setup is correct + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(command_interface_types_).c_str(), + get_interface_list(state_interface_types_).c_str()); + + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(ft_sensor_name_)); + + // Subscribers and callbacks + auto callback_input_force = [&](const std::shared_ptr msg) + -> void + { + input_force_command_.writeFromNonRT(msg); + }; + input_force_command_subscriber_ = get_node()->create_subscription( + "~/force_commands", rclcpp::SystemDefaultsQoS(), callback_input_force); + + auto callback_input_pose = [&](const std::shared_ptr msg) + -> void + { + input_pose_command_.writeFromNonRT(msg); + }; + input_pose_command_subscriber_ = get_node()->create_subscription( + "~/pose_commands", rclcpp::SystemDefaultsQoS(), callback_input_pose); + + // TODO(destogl): Add subscriber for velocity scaling + + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + + // Configure AdmittanceRule + admittance_->configure(get_node()->get_clock()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -79,8 +293,9 @@ const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(joint_names_.size() * command_interface_types_.size()); for (const auto & joint : joint_names_) { - for (const auto & interface : interface_names_) { + for (const auto & interface : command_interface_types_) { command_interfaces_config.names.push_back(joint + "/" + interface); } } @@ -94,8 +309,12 @@ const controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.reserve(6 + joint_names_.size() * state_interface_types_.size()); + + state_interfaces_config.names = force_torque_sensor_->get_state_interface_names(); + for (const auto & joint : joint_names_) { - for (const auto & interface : interface_names_) { + for (const auto & interface : state_interface_types_) { state_interfaces_config.names.push_back(joint + "/" + interface); } } @@ -103,74 +322,106 @@ const return state_interfaces_config; } -// Fill ordered_interfaces with references to the matching interfaces -// in the same order as in joint_names -template -bool get_ordered_interfaces( - std::vector & unordered_interfaces, - const std::vector & joint_names, - const std::string & interface_type, - std::vector> & ordered_interfaces) +CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - for (const auto & joint_name : joint_names) { - for (auto & command_interface : unordered_interfaces) { - if ((command_interface.get_name() == joint_name) && - (command_interface.get_interface_name() == interface_type)) - { - ordered_interfaces.push_back(std::ref(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_, joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + node_->get_logger(), "Expected %u '%s' command interfaces, got %u.", + joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + 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_ERROR( + node_->get_logger(), "Expected %u '%s' state interfaces, got %u.", + joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - return joint_names.size() == ordered_interfaces.size(); -} + // 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(); + previous_time_ = get_node()->now(); + + // Set initial command values + std::shared_ptr msg_force = std::make_shared(); + msg_force->header.frame_id = admittance_->control_frame_; + input_force_command_.writeFromNonRT(msg_force); + + std::shared_ptr msg_pose = std::make_shared(); + msg_pose->header.frame_id = admittance_->control_frame_; + input_pose_command_.writeFromNonRT(msg_pose); -CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -{ return CallbackReturn::SUCCESS; } CallbackReturn AdmittanceController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + // Stop the robot + for (auto index = 0ul; index < joint_names_.size(); ++index) { + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); + } + + for (auto index = 0ul; index < allowed_interface_types_.size(); ++index) { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); + + force_torque_sensor_->release_interfaces(); + return CallbackReturn::SUCCESS; } controller_interface::return_type AdmittanceController::update() { - // Task space admittance pipeline: - // Get wrench measurement - // Calculate the desired Cartesian displacement of the robot with the admittance equation. - // The basic form is: F = K * (x_d - x) - // (wrench) = (stiffness matrix) * (desired_Cartesian_position minus current_Cartesian_position) - // Damping terms can be added too. - // Get current robot joint angles - // Convert Cartesian deltas to joint angle deltas via Jacobian - // Write new joint angles to robot - - - // Get wrench measurement - - // Calculate desired Cartesian displacement of the robot - //delta_x_ = ... + // get input commands + // TODO(destogl): Enable this when unified mode is used + auto input_force_cmd = input_force_command_.readFromRT(); + auto input_pose_cmd = input_pose_command_.readFromRT(); - // Get current robot joint angles + // Position has to always there + auto num_joints = joint_state_interface_[0].size(); + std::vector current_joint_states(num_joints); - // Convert Cartesian deltas to joint angle deltas via Jacobian - if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, force_torque_sensor_frame_, delta_theta_)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); - return controller_interface::return_type::ERROR; + for (auto i = 0u; i < num_joints; ++i) { + current_joint_states.emplace_back(joint_state_interface_[0][i].get().get_value()); } - // Write new joint angles to the robot - + // TODO(destogl): Enable this when unified mode is used +// if (admittance_.unified_mode_) { +// admittance_->update(current_joint_states, force_torque_sensor_->get_values_as_message(), **input_pose_cmd, **input_force_cmd, get_node()->now() - previous_time_); +// } else { + admittance_->update(current_joint_states, force_torque_sensor_->get_values_as_message(), **input_pose_cmd, get_node()->now() - previous_time_); +// } + previous_time_ = get_node()->now(); + // Write new joint angles to the robot - // TODO: below is just a skeleton - for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value(state_interfaces_[index].get_value()); - } + // Publish controller state + state_publisher_->lock(); + state_publisher_->msg_.input_force_command = **input_force_cmd; + state_publisher_->msg_.input_pose_command = **input_pose_cmd; + admittance_->get_controller_state(state_publisher_->msg_); + state_publisher_->unlockAndPublish(); return controller_interface::return_type::OK; } diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp new file mode 100644 index 0000000000..54220f589f --- /dev/null +++ b/admittance_controller/src/admittance_rule.cpp @@ -0,0 +1,286 @@ +// 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. +// +/// \author: Denis Stogl + +#include "admittance_controller/admittance_rule.hpp" + +#include "angles/angles.h" + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2/utils.h" + +namespace { // Utility namespace + +void get_rpy_difference_between_two_quaternions(const geometry_msgs::msg::Quaternion quat1, + const geometry_msgs::msg::Quaternion quat2, std::array vector_out) +{ + tf2::Quaternion q1(quat1.x, quat1.y, quat1.z, quat1.w); + tf2::Quaternion q2(quat2.x, quat2.y, quat2.z, quat2.w); + + // q1 - q2 + const tf2::Quaternion q_diff = q1 * q2.inverse(); + tf2::Matrix3x3(q_diff).getRPY(vector_out[0], vector_out[1], vector_out[2]); +} + +void convert_message_to_array(const geometry_msgs::msg::Pose & msg, std::array & vector_out) +{ + vector_out[0] = msg.position.x; + vector_out[1] = msg.position.y; + vector_out[2] = msg.position.z; + tf2::Quaternion q; + tf2::fromMsg(msg.orientation, q); + q.normalize(); + tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); + for (auto i = 3u; i < 6; ++i) { + vector_out[i] = angles::normalize_angle(vector_out[i]); + } +} + +void convert_message_to_array( + const geometry_msgs::msg::PoseStamped & msg, std::array & vector_out) +{ + convert_message_to_array(msg.pose, vector_out); +} + +void convert_message_to_array( + const geometry_msgs::msg::Wrench & msg, std::array & vector_out) +{ + vector_out[0] = msg.force.x; + vector_out[1] = msg.force.y; + vector_out[2] = msg.force.z; + vector_out[3] = msg.torque.x; + vector_out[4] = msg.torque.y; + vector_out[5] = msg.torque.z; +} + +void convert_message_to_array( + const geometry_msgs::msg::WrenchStamped & msg, std::array & vector_out) +{ + convert_message_to_array(msg.wrench, vector_out); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Pose & msg_out) +{ + msg_out.position.x = vector[0]; + msg_out.position.y = vector[1]; + msg_out.position.z = vector[2]; + + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.orientation.x = q.x(); + msg_out.orientation.y = q.y(); + msg_out.orientation.z = q.z(); + msg_out.orientation.w = q.w(); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::PoseStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.pose); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Wrench & msg_out) +{ + msg_out.force.x = vector[0]; + msg_out.force.y = vector[1]; + msg_out.force.z = vector[2]; + msg_out.torque.x = vector[3]; + msg_out.torque.y = vector[4]; + msg_out.torque.z = vector[5]; + +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::WrenchStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.wrench); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Transform & msg_out) +{ + msg_out.translation.x = vector[0]; + msg_out.translation.y = vector[1]; + msg_out.translation.z = vector[2]; + + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.rotation.x = q.x(); + msg_out.rotation.y = q.y(); + msg_out.rotation.z = q.z(); + msg_out.rotation.w = q.w(); +} + +void convert_array_to_message(const std::array & vector, geometry_msgs::msg::TransformStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.transform); +} + +} // utility namespace + +namespace admittance_controller +{ + +controller_interface::return_type AdmittanceRule::configure(rclcpp::Clock::SharedPtr clock) +{ + tf_buffer_ = std::make_shared(clock); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Initialize variables used in the update loop + measured_force_.header.frame_id = sensor_frame_; + + current_origin_.header.frame_id = endeffector_frame_; + current_origin_.pose.orientation.w = 1; + current_pose_.header.frame_id = endeffector_frame_; + + relative_desired_pose_.header.frame_id = control_frame_; + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::reset() +{ + measured_force_control_frame_vec_.fill(0.0); + target_pose_control_frame_vec_.fill(0.0); + + current_pose_control_frame_vec_.fill(0.0); + + angles_error_.fill(0.0); + + desired_velocity_vec_.fill(0.0); + desired_velocity_previous_vec_.fill(0.0); + desired_acceleration_previous_vec_.fill(0.0); + + // - use tf2 instead of FK + // - transform data to Controller frame + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::update( + const std::vector & /*current_joint_state*/, + const geometry_msgs::msg::Wrench & /*measured_force*/, + const geometry_msgs::msg::PoseStamped & /*target_pose*/, + const geometry_msgs::msg::WrenchStamped & /*target_force*/, + const rclcpp::Duration & /*period*/ +) +{ + // TODO(destogl): Implement this update + // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); + // TODO(destogl) reuse ting from other update + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::update( + const std::vector & /*current_joint_state*/, + const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::PoseStamped & target_pose, + const rclcpp::Duration & period) +{ + // Convert inputs to control frame + transform_message_to_control_frame(target_pose, target_pose_control_frame_); + + // get current states, and transform those into controller frame + measured_force_.wrench = measured_force; + // TODO(destogl): Add gravity compensation of measured forces + measured_force_control_frame_ = measured_force_; + transform_message_to_control_frame(measured_force_, measured_force_control_frame_); + + // Get tool frame position - in the future use: IKSolver->getPositionFK(...) or similar (ask TODO(AndyZe)) + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::doTransform(current_origin_, current_pose_, transform); + transform_message_to_control_frame(current_pose_, current_pose_control_frame_); + + // Convert all data to arrays for simpler calculation + convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_vec_); + convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_vec_); + convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_vec_); + + // Use angle difference calculated with quaternions to avoid confusion with angles + get_rpy_difference_between_two_quaternions(target_pose_control_frame_.pose.orientation, + current_pose_control_frame_.pose.orientation, angles_error_); + + // Compute admittance control law: F = M*a + D*v + K*(x_d - x) + for (auto i = 0u; i < 6; ++i) { + if (selected_axes_[i]) { + double pose_error = target_pose_control_frame_vec_[i] - measured_force_control_frame_vec_[i]; + if (i >= 3) { + pose_error = angles_error_[i-3]; + } + + // TODO(destogl): check if velocity is measured from hardware + const double acceleration = 1 / mass_[i] * + (measured_force_control_frame_vec_[i] - damping_[i] * desired_velocity_vec_[i] - + stiffness_[i] * pose_error); + + desired_velocity_vec_[i] += (desired_acceleration_previous_vec_[i] + acceleration) * 0.5 * period.seconds(); + + relative_desired_pose_vec_[i] = (desired_velocity_previous_vec_[i] + desired_velocity_vec_[i]) * 0.5 * period.seconds(); + + desired_acceleration_previous_vec_[i] = acceleration; + desired_velocity_previous_vec_[i] = desired_velocity_vec_[i]; + } + } + + // Do clean conversion to the goal pose using transform and not messing with Euler angles + convert_array_to_message(relative_desired_pose_vec_, relative_desired_pose_); + tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); + + // Use Jacobian-based IK + // Calculate desired Cartesian displacement of the robot + //delta_x_ = relative_desired_pose_; + + // Get current robot joint angles + + // Convert Cartesian deltas to joint angle deltas via Jacobian + // if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, sensor_frame_, delta_theta_)) + // { + // RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); + // return controller_interface::return_type::ERROR; + // } + + // TODO(anyone: enable this when enabling use of IK directly + // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); + // tf2::doTransform(desired_pose_, ik_input_pose_, transform); + // ik_input_pose_.pose = endeffector_to_ik_tip(ik_input_pose_); + // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::get_controller_state( + control_msgs::msg::AdmittanceControllerState & state_message) +{ + // state_message.input_force_control_frame = target_force_control_frame_; + state_message.input_pose_control_frame = target_pose_control_frame_; + state_message.measured_force = measured_force_; + state_message.measured_force_control_frame = measured_force_control_frame_; + state_message.measured_force_endeffector_frame = measured_force_control_frame_; + state_message.desired_pose = desired_pose_; + state_message.relative_desired_pose = relative_desired_pose_; + // state_message.desired_joint_states = desired_pose_; + // state_message.actual_joint_states = desired_pose_; + // state_message.error_joint_state = desired_pose_; + + return controller_interface::return_type::OK; +} + +} // namespace admittance_controller diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index ace07c39ee..78991774de 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -16,6 +16,8 @@ #include "admittance_controller/incremental_ik_calculator.hpp" +#include "tf2_eigen/tf2_eigen.h" + namespace admittance_controller { IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& node) : node_(node) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp new file mode 100644 index 0000000000..75fc8d8e28 --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -0,0 +1,368 @@ +// Copyright (c) 2021, salfkjsadf +// 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 "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_CASE_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("IK.base"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("IK.tip"), + 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("endeffector_frame"), + rclcpp::ParameterValue("") + ), + std::make_tuple( + std::string("fixed_world_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_->ik_base_frame_, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->ik_tip_frame_, ik_tip_frame_); + // ASSERT_EQ(controller_->admittance_->ik_group_name_, ik_group_name_); + ASSERT_EQ(controller_->admittance_->endeffector_frame_, endeffector_frame_); + ASSERT_EQ(controller_->admittance_->fixed_world_frame_, fixed_world_frame_); + ASSERT_EQ(controller_->admittance_->sensor_frame_, sensor_frame_); + + ASSERT_TRUE(!controller_->admittance_->selected_axes_.empty()); + ASSERT_TRUE(controller_->admittance_->selected_axes_.size() == admittance_selected_axes_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->selected_axes_.begin(), controller_->admittance_->selected_axes_.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + + ASSERT_TRUE(!controller_->admittance_->mass_.empty()); + ASSERT_TRUE(controller_->admittance_->mass_.size() == admittance_mass_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->mass_.begin(), controller_->admittance_->mass_.end(), + admittance_mass_.begin(), admittance_mass_.end())); + + ASSERT_TRUE(!controller_->admittance_->damping_.empty()); + ASSERT_TRUE(controller_->admittance_->damping_.size() == admittance_damping_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->damping_.begin(), controller_->admittance_->damping_.end(), + admittance_damping_.begin(), admittance_damping_.end())); + + ASSERT_TRUE(!controller_->admittance_->stiffness_.empty()); + ASSERT_TRUE(controller_->admittance_->stiffness_.size() == admittance_stiffness_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->stiffness_.begin(), controller_->admittance_->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(), 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(), controller_interface::return_type::OK); +} + +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(), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // Check default parameters + ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); +} + +TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + 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(), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.input_force_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(), controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[0], 0.0); + + subscribe_and_get_messages(msg); + ASSERT_EQ(msg.input_force_command.header.frame_id, sensor_frame_); + ASSERT_EQ(msg.input_pose_command.header.frame_id, endeffector_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..172f0b22aa --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -0,0 +1,389 @@ +// Copyright (c) 2021, salfkjsadf +// 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. + +#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 "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 "tf2_ros/transform_broadcaster.h" + +// TODO(anyone): replace the state and command message types +using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +namespace +{ + 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 varibles +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); + +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) { + input_force_command_subscriber_wait_set_.add_subscription(input_force_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_force_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready && + input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_force_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()); + + 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) + { + 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({"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({"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({"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 = control_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 1; + 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 = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = endeffector_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0; + transform_stamped.child_frame_id = fixed_world_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(), 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; + } + }; + + wait_for_topic(force_command_publisher_->get_topic_name()); + wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandForceMsg 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; + + force_command_publisher_->publish(force_msg); + pose_command_publisher_->publish(pose_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"; + const std::string ik_base_frame_ = "IK_base"; + const std::string ik_tip_frame_ = "IK.tip"; + const std::string ik_group_name_ = "IK.group_name"; + 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::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/doc/admittance_controller.rst b/doc/admittance_controller.rst new file mode 100644 index 0000000000..f265c419b4 --- /dev/null +++ b/doc/admittance_controller.rst @@ -0,0 +1,58 @@ +admittance_controller +====================== + +The controller uses admittance dynamics to calculate robot movements caused by an external force. +The standard admittance control law: F = M\*a + D\*v + K\*(x_d - x) is used. + +The paramters are adjustable using ``dynamic_reconfigure`` functionality and rqt-plugin. + +The controller has following parameters: + + - ``joints`` - names of joints to control the robot. + - ``command_interfaces`` - interfaces uses for commanding the robot, e.g., position and/or velocity. + - ``state_interfaces`` - interfaces to get robot's current state. Currently only position is supported. + - ``ft_sensor_name`` - name of the sensor providing force-torque measurements. The controller uses `ForceTorqueSensor semantic component `_ which defines standard interface names for this measurements, e.g., /force.x. + + - ``IK.base`` - base link of the IK, usually the first fixed link on a robot, e.g., ``base_link``. + - ``IK.tip`` - end of the IK, usually flansh of the robot, e.g., ``tool0``. + - ``IK.group_name`` - name of the IK group defined when using a IK plugin (not implemented yet). + + - ``controller_frame`` - frame where all input and measured values will be transformed to do the calculation of admittance equation. + - ``endeffector_frame`` - frame of the endeffector, where force is influencing the robot and which pose is manipulated. + - ``fixed_world_frame`` - frame where gravity vector is oriented in ``-z`` direction. + - ``sensor_frame`` - frame where force-torque values are measured. + + - ``gravity_compensation.masses`` - list of masses of robot parts to subtract their gravity influence from force/torque measurements. + - ``gravity_compensation.center_of_masses`` - list of distances for the corresponding mass from ``sensor_frame`` (one dimensional). + + - ``admittance.selected_axes.[x|y|z|rx|ry|rz]`` - boolean value to enable admittance control for specific degree of freedom in ``controller_frame``. + - ``admittance.mass.[x|y|z|rx|ry|rz]`` - six virtual mass (M) values for degrees of freedom. + - ``admittance.damping.[x|y|z|rx|ry|rz]`` - six virtual damping (D) values for degrees of freedom. + - ``admittance.stiffness.[x|y|z|rx|ry|rz]`` - six virtual stiffness values (K) for degrees of freedom. + + +Publishers: + + - ``state`` - state message of the controller as ``control_msgs/msg/AdmittanceControllerState``: + + geometry_msgs/WrenchStamped input_force_command #commanded input force for the controller + geometry_msgs/PoseStamped input_pose_command #commanded input pose for the controller + + geometry_msgs/WrenchStamped input_force_transformed # input force transformed into control frame + geometry_msgs/PoseStamped input_pose_transformed # input pose transformed into control frame + + geometry_msgs/WrenchStamped measured_force # measured force from the sensor (sensor frame) + geometry_msgs/WrenchStamped measured_force_filtered # measured force after low-pass and gravity compensation filters in sensor frame + geometry_msgs/WrenchStamped measured_force_transformed # transformed measured force to control frame + + geometry_msgs/PoseStamped goal_pose_command # goal pose to be commanded + + string[] joint_names + trajectory_msgs/JointTrajectoryPoint desired_joint_states # result of IK from goal_pose_command + trajectory_msgs/JointTrajectoryPoint actual_joint_states # read from the hardware + trajectory_msgs/JointTrajectoryPoint error_joint_state + + +Subscribers: + + - ``input_pose_command`` - desired pose as ``geometry_msgs/msg/PoseStamped`` From ea9d49e2da2f180eaa12efd495b5d6f461e9ca12 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Thu, 6 May 2021 20:04:45 +0200 Subject: [PATCH 11/99] Try to integrate IK --- .../admittance_controller/admittance_rule.hpp | 13 +++-- .../incremental_ik_calculator.hpp | 36 +++++++------ .../src/admittance_controller.cpp | 14 ++---- admittance_controller/src/admittance_rule.cpp | 50 ++++++++++++------- .../src/incremental_ik_calculator.cpp | 28 +++++++---- .../test/test_admittance_controller.cpp | 8 +-- .../test/test_admittance_controller.hpp | 7 ++- 7 files changed, 93 insertions(+), 63 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index f21bf98c8f..a9d5554327 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -34,21 +34,22 @@ class AdmittanceRule public: AdmittanceRule() = default; - controller_interface::return_type configure(rclcpp::Clock::SharedPtr clock); + controller_interface::return_type configure(rclcpp::Node::SharedPtr node); controller_interface::return_type update( const std::vector & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const geometry_msgs::msg::WrenchStamped & target_force, - const rclcpp::Duration & period - ); + const rclcpp::Duration & period, + std::array desired_joint_states); controller_interface::return_type update( - const std::vector & current_joint_state, + const std::array & /*current_joint_state*/, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, - const rclcpp::Duration & period); + const rclcpp::Duration & period, + std::array desired_joint_states); // controller_interface::return_type update( // const geometry_msgs::msg::WrenchStamped & measured_force, @@ -118,6 +119,8 @@ class AdmittanceRule std::array desired_velocity_previous_vec_; std::array desired_acceleration_previous_vec_; + std::vector relative_desired_joint_state_vec_; + private: // TODO: implement doTransform for WrenchStamped template diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp index 2bfd17a4b8..b547275840 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp @@ -16,10 +16,11 @@ #pragma once -#include -#include -#include -#include +#include "Eigen/Core" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "moveit/robot_model_loader/robot_model_loader.h" +#include "moveit/robot_state/robot_state.h" +#include "rclcpp/utilities.hpp" namespace admittance_controller { @@ -31,16 +32,27 @@ class IncrementalIKCalculator * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ - IncrementalIKCalculator(std::shared_ptr & node); + IncrementalIKCalculator(const std::shared_ptr & node, const std::string & group_name); /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. - * \param delta_x input Cartesian deltas - * \param delta_x_frame input name of the delta_x tf frame - * \param delta_theta output + * \param delta_x_vec input Cartesian deltas (x, y, z, rx, ry, rz) + * \param ik_base_to_tip_trafo transformation between ik base and ik tip + * \param delta_theta_vec output vector with joint states * \return true if successful */ - bool convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta); + bool + convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec); + + /** + * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. + * \param[in] delta_theta_vec vector with joint states + * \param[in] ik_base_to_tip_trafo transformation between ik base to ik tip + * \param[out] delta_x_vec Cartesian deltas (x, y, z, rx, ry, rz) + * \return true if successful + */ + bool + convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec); private: // MoveIt setup, required to retrieve the Jacobian @@ -53,12 +65,6 @@ class IncrementalIKCalculator Eigen::JacobiSVD svd_; Eigen::MatrixXd matrix_s_; Eigen::MatrixXd pseudo_inverse_; - - // TF frames - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - std::string moveit_jacobian_frame_; - geometry_msgs::msg::TransformStamped delta_x_to_jacobian_transform_; }; } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 78d270cb36..010ac3d3c5 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -22,6 +22,7 @@ // #include "Eigen/Core" #include "admittance_controller/admittance_controller.hpp" +#include "admittance_controller/incremental_ik_calculator.hpp" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -47,9 +48,9 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("IK.base", ""); get_node()->declare_parameter("IK.tip", ""); - // TODO(destogl): enable when IK support is added + // TODO(destogl): enable when IK-plugin support is added // get_node()->declare_parameter("IK.plugin", ""); -// get_node()->declare_parameter("IK.group_name", ""); + get_node()->declare_parameter("IK.group_name", ""); get_node()->declare_parameter("control_frame", ""); get_node()->declare_parameter("endeffector_frame", ""); @@ -97,10 +98,6 @@ controller_interface::return_type AdmittanceController::init(const std::string & admittance_ = std::make_unique(); - auto node = get_node(); -// ik_ = std::make_shared(node); -// delta_x_ = Eigen::VectorXd(6); - return controller_interface::return_type::OK; } @@ -151,8 +148,7 @@ CallbackReturn AdmittanceController::on_configure( get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || - // TODO: Enable this when IK plugin use is implemented - // get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || + get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") || get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") || get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") || @@ -281,7 +277,7 @@ CallbackReturn AdmittanceController::on_configure( state_publisher_ = std::make_unique(s_publisher_); // Configure AdmittanceRule - admittance_->configure(get_node()->get_clock()); + admittance_->configure(get_node()); RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 54220f589f..1effcb6e7d 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -26,6 +26,7 @@ #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" #include "tf2/utils.h" +#include namespace { // Utility namespace @@ -138,9 +139,9 @@ void convert_array_to_message(const std::array & vector, geometry_msg namespace admittance_controller { -controller_interface::return_type AdmittanceRule::configure(rclcpp::Clock::SharedPtr clock) +controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::SharedPtr node) { - tf_buffer_ = std::make_shared(clock); + tf_buffer_ = std::make_shared(node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); // Initialize variables used in the update loop @@ -152,6 +153,11 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Clock::Share relative_desired_pose_.header.frame_id = control_frame_; + relative_desired_joint_state_vec_.reserve(6); + + // Initialize IK + ik_ = std::make_shared(node, ik_group_name_); + return controller_interface::return_type::OK; } @@ -175,25 +181,28 @@ controller_interface::return_type AdmittanceRule::reset() } controller_interface::return_type AdmittanceRule::update( - const std::vector & /*current_joint_state*/, + const std::array & /*current_joint_states*/, const geometry_msgs::msg::Wrench & /*measured_force*/, const geometry_msgs::msg::PoseStamped & /*target_pose*/, const geometry_msgs::msg::WrenchStamped & /*target_force*/, - const rclcpp::Duration & /*period*/ + const rclcpp::Duration & /*period*/, + std::array /*desired_joint_states*/ ) { // TODO(destogl): Implement this update // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); - // TODO(destogl) reuse ting from other update + // TODO(destogl) reuse things from other update return controller_interface::return_type::OK; } controller_interface::return_type AdmittanceRule::update( - const std::vector & /*current_joint_state*/, + const std::array & current_joint_states, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, - const rclcpp::Duration & period) + const rclcpp::Duration & period, + std::array desired_joint_states +) { // Convert inputs to control frame transform_message_to_control_frame(target_pose, target_pose_control_frame_); @@ -240,22 +249,25 @@ controller_interface::return_type AdmittanceRule::update( } } + // TODO: Add here desired vector at the IK tip and not tool!!!! + // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_vec_, relative_desired_pose_); tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); // Use Jacobian-based IK // Calculate desired Cartesian displacement of the robot - //delta_x_ = relative_desired_pose_; - - // Get current robot joint angles - - // Convert Cartesian deltas to joint angle deltas via Jacobian - // if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, sensor_frame_, delta_theta_)) - // { - // RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed."); - // return controller_interface::return_type::ERROR; - // } + transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + std::vector relative_desired_pose_v_(relative_desired_pose_vec_.begin(), relative_desired_pose_vec_.end()); + if (ik_->convertCartesianDeltasToJointDeltas( + relative_desired_pose_v_, transform, relative_desired_joint_state_vec_)){ + for (auto i = 0u; i < desired_joint_states.size(); ++i) { + desired_joint_states[i] = current_joint_states[i] + relative_desired_joint_state_vec_[i]; + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); + desired_joint_states = current_joint_states; + } // TODO(anyone: enable this when enabling use of IK directly // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); @@ -276,8 +288,8 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_force_endeffector_frame = measured_force_control_frame_; state_message.desired_pose = desired_pose_; state_message.relative_desired_pose = relative_desired_pose_; - // state_message.desired_joint_states = desired_pose_; - // state_message.actual_joint_states = desired_pose_; +// state_message.desired_joint_states = desired_pose_; +// state_message.actual_joint_states.positi = desired_pose_; // state_message.error_joint_state = desired_pose_; return controller_interface::return_type::OK; diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_ik_calculator.cpp index 78991774de..9f278769b3 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_ik_calculator.cpp @@ -16,35 +16,34 @@ #include "admittance_controller/incremental_ik_calculator.hpp" +#include "Eigen/VectorXf" #include "tf2_eigen/tf2_eigen.h" namespace admittance_controller { -IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr& node) : node_(node) +IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr & node, const std::string & group_name) : node_(node) { // TODO(andyz): Parameterize robot description and joint group std::unique_ptr model_loader_ptr = std::unique_ptr(new robot_model_loader::RobotModelLoader(node_, "/robot_description", false /* do not load kinematics plugins */)); const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe? - joint_model_group_ = kinematic_model->getJointModelGroup("gemini"); + joint_model_group_ = kinematic_model->getJointModelGroup(group_name); kinematic_state_ = std::make_shared(kinematic_model); // By default, the MoveIt Jacobian frame is the last link - // TODO(andyz): parameterize this or retrieve it via API - moveit_jacobian_frame_ = "end_effector"; - tf_buffer_ = std::make_shared(node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); } -bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta) +bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec) { + // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector + Eigen::VectorXf delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); + // Transform delta_x to the moveit_jacobian_frame try { // 4x4 transformation matrix - delta_x_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero); - Eigen::Isometry3d affine_transform = tf2::transformToEigen(delta_x_to_jacobian_transform_); + const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_trafo); // Build the 6x6 transformation matrix Eigen::MatrixXd twist_transform(6,6); @@ -74,8 +73,17 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorX svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); matrix_s_ = svd_.singularValues().asDiagonal(); pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); - delta_theta = pseudo_inverse_ * delta_x; + Eigen::VectorXf delta_theta = pseudo_inverse_ * delta_x; + + std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); + delta_theta_vec = delta_theta_v; return true; } + +bool IncrementalIKCalculator::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec) +{ + // TODO(andyz): Please add here FK implementation +} + } // namespace admittance_controller diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 75fc8d8e28..97633f47b6 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -58,10 +58,10 @@ INSTANTIATE_TEST_CASE_P( std::string("IK.tip"), rclcpp::ParameterValue("") ), -// std::make_tuple( -// std::string("IK.group_name"), -// rclcpp::ParameterValue("") -// ), + std::make_tuple( + std::string("IK.group_name"), + rclcpp::ParameterValue("") + ), std::make_tuple( std::string("control_frame"), rclcpp::ParameterValue("") diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 172f0b22aa..eda21df1e1 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -28,6 +28,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 "semantic_components/force_torque_sensor.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/utilities.hpp" @@ -155,7 +156,8 @@ class AdmittanceControllerTest : public ::testing::Test 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({"IK.group_name", ik_group_name_}); + controller_->get_node()->set_parameter({"robot_description", robot_description_}); controller_->get_node()->set_parameter({"control_frame", control_frame_}); controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_}); @@ -330,9 +332,12 @@ class AdmittanceControllerTest : public ::testing::Test const std::vector command_interface_types_ = {"position"}; const std::vector state_interface_types_ = {"position"}; const std::string ft_sensor_name_ = "ft_sensor_name"; + const std::string ik_base_frame_ = "IK_base"; const std::string ik_tip_frame_ = "IK.tip"; const std::string ik_group_name_ = "IK.group_name"; + const std::string robot_description_ = ros2_control_test_assets::6d_robot_urdf; + const std::string control_frame_ = "control_frame"; const std::string endeffector_frame_ = "endeffector_frame"; const std::string fixed_world_frame_ = "fixed_world_frame"; From aa429237bb2d907cfd3674ffde7e5296a285a3b4 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 7 May 2021 02:25:08 -0500 Subject: [PATCH 12/99] Finish the method for forward kinematics (#4) * Rename incremental_ik_calculator -> incremental_kinematics * Use Eigen double types. Rename _trafo transform variables * Fix typo in transformation matrix * Implement incremental forward kinematics --- admittance_controller/CMakeLists.txt | 2 +- .../admittance_controller/admittance_rule.hpp | 4 +- ...culator.hpp => incremental_kinematics.hpp} | 12 ++-- .../src/admittance_controller.cpp | 2 +- admittance_controller/src/admittance_rule.cpp | 2 +- ...culator.cpp => incremental_kinematics.cpp} | 64 ++++++++++++++++--- 6 files changed, 65 insertions(+), 21 deletions(-) rename admittance_controller/include/admittance_controller/{incremental_ik_calculator.hpp => incremental_kinematics.hpp} (84%) rename admittance_controller/src/{incremental_ik_calculator.cpp => incremental_kinematics.cpp} (52%) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 797942911c..f5c31bad99 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -33,7 +33,7 @@ add_library( SHARED src/admittance_controller.cpp src/admittance_rule.cpp - src/incremental_ik_calculator.cpp + src/incremental_kinematics.cpp ) target_include_directories( admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a9d5554327..6f7fa867c4 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -17,7 +17,7 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ -#include "admittance_controller/incremental_ik_calculator.hpp" +#include "admittance_controller/incremental_kinematics.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -86,7 +86,7 @@ class AdmittanceRule protected: // IK variables - std::shared_ptr ik_; + std::shared_ptr ik_; Eigen::VectorXd delta_x_; Eigen::VectorXd delta_theta_; diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp similarity index 84% rename from admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp rename to admittance_controller/include/admittance_controller/incremental_kinematics.hpp index b547275840..a3a168c5ba 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -25,34 +25,34 @@ namespace admittance_controller { -class IncrementalIKCalculator +class IncrementalKinematics { public: /** * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ - IncrementalIKCalculator(const std::shared_ptr & node, const std::string & group_name); + IncrementalKinematics(const std::shared_ptr & node, const std::string & group_name); /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. * \param delta_x_vec input Cartesian deltas (x, y, z, rx, ry, rz) - * \param ik_base_to_tip_trafo transformation between ik base and ik tip + * \param ik_base_to_tip_tf transformation between ik base and ik tip * \param delta_theta_vec output vector with joint states * \return true if successful */ bool - convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec); + convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec); /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. * \param[in] delta_theta_vec vector with joint states - * \param[in] ik_base_to_tip_trafo transformation between ik base to ik tip + * \param[in] ik_base_to_tip_tf transformation between ik base to ik tip * \param[out] delta_x_vec Cartesian deltas (x, y, z, rx, ry, rz) * \return true if successful */ bool - convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec); + convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec); private: // MoveIt setup, required to retrieve the Jacobian diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 010ac3d3c5..4e2335b350 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -22,7 +22,7 @@ // #include "Eigen/Core" #include "admittance_controller/admittance_controller.hpp" -#include "admittance_controller/incremental_ik_calculator.hpp" +#include "admittance_controller/incremental_kinematics.hpp" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 1effcb6e7d..fc87049fdf 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -156,7 +156,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared relative_desired_joint_state_vec_.reserve(6); // Initialize IK - ik_ = std::make_shared(node, ik_group_name_); + ik_ = std::make_shared(node, ik_group_name_); return controller_interface::return_type::OK; } diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_kinematics.cpp similarity index 52% rename from admittance_controller/src/incremental_ik_calculator.cpp rename to admittance_controller/src/incremental_kinematics.cpp index 9f278769b3..48bc7f6bc7 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -14,14 +14,14 @@ // /// \author: Andy Zelenak -#include "admittance_controller/incremental_ik_calculator.hpp" +#include "admittance_controller/incremental_kinematics.hpp" -#include "Eigen/VectorXf" +#include "Eigen/VectorXd" #include "tf2_eigen/tf2_eigen.h" namespace admittance_controller { -IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr & node, const std::string & group_name) : node_(node) +IncrementalKinematics::IncrementalKinematics(const std::shared_ptr & node, const std::string & group_name) : node_(node) { // TODO(andyz): Parameterize robot description and joint group std::unique_ptr model_loader_ptr = @@ -34,23 +34,25 @@ IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec) +bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector - Eigen::VectorXf delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); + Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); // Transform delta_x to the moveit_jacobian_frame + // TODO: replace when this PR to tf2_eigen is merged + // https://github.com/ros2/geometry2/pull/406 try { // 4x4 transformation matrix - const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_trafo); + const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf); // Build the 6x6 transformation matrix Eigen::MatrixXd twist_transform(6,6); // upper left 3x3 block is the rotation part twist_transform.block(0,0,3,3) = affine_transform.rotation(); // upper right 3x3 block is all zeros - twist_transform.block(0,4,3,3) = Eigen::MatrixXd::Zero(3,3); + twist_transform.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3); // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf Eigen::MatrixXd pos_vector_3x3(3,3); pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y(); @@ -73,7 +75,7 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vec svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); matrix_s_ = svd_.singularValues().asDiagonal(); pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); - Eigen::VectorXf delta_theta = pseudo_inverse_ * delta_x; + Eigen::VectorXd delta_theta = pseudo_inverse_ * delta_x; std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); delta_theta_vec = delta_theta_v; @@ -81,9 +83,51 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vec return true; } -bool IncrementalIKCalculator::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec) +bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec) { - // TODO(andyz): Please add here FK implementation + // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector + Eigen::VectorXd delta_theta = Eigen::Map(&delta_theta_vec[0], delta_theta_vec.size()); + + // Multiply with the Jacobian to get delta_x + jacobian_ = kinematic_state_->getJacobian(joint_model_group_); + // delta_x will be in the working frame of MoveIt + Eigen::VectorXd delta_x = jacobian_ * delta_theta; + + // Transform delta_x to the tip frame + // TODO: replace when this PR to tf2_eigen is merged + // https://github.com/ros2/geometry2/pull/406 + try + { + // 4x4 transformation matrix + const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf); + + // Build the 6x6 transformation matrix + Eigen::MatrixXd twist_transform(6,6); + // upper left 3x3 block is the rotation part + twist_transform.block(0,0,3,3) = affine_transform.rotation(); + // upper right 3x3 block is all zeros + twist_transform.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3); + // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf + Eigen::MatrixXd pos_vector_3x3(3,3); + pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y(); + pos_vector_3x3(1, 0) = affine_transform.translation().z(); pos_vector_3x3(1,1) = 0; pos_vector_3x3(1,2) = -affine_transform.translation().x(); + pos_vector_3x3(2, 0) = -affine_transform.translation().y(); pos_vector_3x3(2,1) = affine_transform.translation().x(); pos_vector_3x3(1,2) = 0; + twist_transform.block(3,0,3,3) = pos_vector_3x3 * affine_transform.rotation(); + // lower right 3x3 block is the rotation part + twist_transform.block(3,3,3,3) = affine_transform.rotation(); + + delta_x = twist_transform * delta_x; + } + catch (tf2::TransformException & ex) + { + RCLCPP_ERROR(node_->get_logger(), "Transformation of wrench failed."); + return false; + } + + std::vector delta_x_v(&delta_x[0], delta_x.data() + delta_x.cols() * delta_x.rows()); + delta_x_vec = delta_x_v; + + return true; } } // namespace admittance_controller From 6c07934ed4c36741dc8fa62fb7bdc746e0ea2843 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 7 May 2021 23:18:19 +0200 Subject: [PATCH 13/99] Version that could work --- admittance_controller/CMakeLists.txt | 1 + .../admittance_controller.hpp | 7 + .../admittance_controller/admittance_rule.hpp | 105 +++++++------ .../incremental_kinematics.hpp | 6 +- .../src/admittance_controller.cpp | 119 +++++++++++--- admittance_controller/src/admittance_rule.cpp | 145 ++++++++++++------ .../src/incremental_kinematics.cpp | 7 +- .../test/test_admittance_controller.cpp | 4 + .../test/test_admittance_controller.hpp | 46 ++++-- 9 files changed, 310 insertions(+), 130 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index f5c31bad99..b2b7061501 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -100,6 +100,7 @@ if(BUILD_TESTING) control_msgs controller_interface hardware_interface + ros2_control_test_assets ) endif() diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index fc815e14e2..246d101ab4 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -36,6 +36,8 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +// 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" namespace admittance_controller { @@ -73,6 +75,7 @@ class AdmittanceController : public controller_interface::ControllerInterface std::vector command_interface_types_; std::vector state_interface_types_; std::string ft_sensor_name_; + bool use_joint_commands_as_input_; // Internal variables std::unique_ptr force_torque_sensor_; @@ -84,16 +87,20 @@ class AdmittanceController : public controller_interface::ControllerInterface // Command subscribers and Controller State publisher using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; + using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory; rclcpp::Subscription::SharedPtr input_force_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr input_pose_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_force_command_; realtime_tools::RealtimeBuffer> input_pose_command_; + realtime_tools::RealtimeBuffer> + input_joint_command_; using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; using ControllerStatePublisher = realtime_tools::RealtimePublisher; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 6f7fa867c4..9a8cd65853 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -36,18 +36,27 @@ class AdmittanceRule controller_interface::return_type configure(rclcpp::Node::SharedPtr node); + controller_interface::return_type reset(); + controller_interface::return_type update( - const std::vector & current_joint_state, + const std::array & /*current_joint_state*/, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, - const geometry_msgs::msg::WrenchStamped & target_force, const rclcpp::Duration & period, std::array desired_joint_states); controller_interface::return_type update( - const std::array & /*current_joint_state*/, + const std::array & current_joint_state, + const geometry_msgs::msg::Wrench & measured_force, + const std::array & target_joint_deltas, + const rclcpp::Duration & period, + std::array desired_joint_states); + + controller_interface::return_type update( + const std::array & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, + const geometry_msgs::msg::WrenchStamped & target_force, const rclcpp::Duration & period, std::array desired_joint_states); @@ -63,7 +72,7 @@ class AdmittanceRule control_msgs::msg::AdmittanceControllerState & state_message ); - controller_interface::return_type reset(); + controller_interface::return_type get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose); public: // IK related parameters @@ -78,7 +87,8 @@ class AdmittanceRule std::string sensor_frame_; // Admittance parameters - // bool unified_mode_ = false; + // TODO(destogl): unified mode does not have to be here + bool unified_mode_ = false; // Unified mode enables simultaneous force and position goals std::array selected_axes_; std::array mass_; std::array damping_; @@ -87,17 +97,19 @@ class AdmittanceRule protected: // IK variables std::shared_ptr ik_; - Eigen::VectorXd delta_x_; - Eigen::VectorXd delta_theta_; // Transformation variables std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; + tf2::Transform ik_tip_to_endeffector_frame_tf_; + tf2::Transform endeffector_frame_to_ik_tip_tf_; + geometry_msgs::msg::WrenchStamped measured_force_; geometry_msgs::msg::WrenchStamped measured_force_control_frame_; - geometry_msgs::msg::PoseStamped current_origin_; + geometry_msgs::msg::PoseStamped origin_ik_tip_; + geometry_msgs::msg::PoseStamped origin_endeffector_; geometry_msgs::msg::PoseStamped current_pose_; geometry_msgs::msg::PoseStamped current_pose_control_frame_; @@ -108,56 +120,63 @@ class AdmittanceRule geometry_msgs::msg::TransformStamped relative_desired_pose_; // Pre-reserved update-loop variables - std::array measured_force_control_frame_vec_; - std::array target_pose_control_frame_vec_; - std::array current_pose_control_frame_vec_; + std::array measured_force_control_frame_arr_; + std::array target_pose_control_frame_arr_; + std::array current_pose_control_frame_arr_; std::array angles_error_; - std::array relative_desired_pose_vec_; - std::array desired_velocity_vec_; - std::array desired_velocity_previous_vec_; - std::array desired_acceleration_previous_vec_; + std::array relative_desired_pose_arr_; + std::array desired_velocity_arr_; + std::array desired_velocity_previous_arr_; + std::array desired_acceleration_previous_arr_; std::vector relative_desired_joint_state_vec_; private: - // TODO: implement doTransform for WrenchStamped template -// using MsgType = geometry_msgs::msg::PoseStamped; - void transform_message_to_control_frame(const MsgType & message_in, MsgType & message_out) + controller_interface::return_type + transform_message_to_control_frame(const MsgType & message_in, MsgType & message_out) { if (control_frame_ != message_in.header.frame_id) { - geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - control_frame_, message_in.header.frame_id, tf2::TimePointZero); - tf2::doTransform(message_in, message_out, transform); + try { + geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( + control_frame_, message_in.header.frame_id, tf2::TimePointZero); + tf2::doTransform(message_in, message_out, transform); + } catch (tf2::TransformException e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + message_in.header.frame_id + "'."); + return controller_interface::return_type::ERROR; + } } else { message_out = message_in; } + return controller_interface::return_type::OK; + } + + void + direct_transform(const geometry_msgs::msg::Pose & input_pose, const tf2::Transform & transform, geometry_msgs::msg::Pose & output_pose) + { + // use TF2 data types for easy math + tf2::Transform input_pose_tf, output_pose_tf; + + tf2::fromMsg(input_pose, input_pose_tf); + output_pose_tf = input_pose_tf * transform; + tf2::toMsg(output_pose_tf, output_pose); + } + + void + ik_tip_to_endeffector_frame(const geometry_msgs::msg::Pose & base_to_ik_tip, geometry_msgs::msg::Pose & base_to_toollink) + { + direct_transform(base_to_ik_tip, ik_tip_to_endeffector_frame_tf_, base_to_toollink); + } + + void + endeffector_to_ik_tip_frame(const geometry_msgs::msg::Pose & base_to_toollink, geometry_msgs::msg::Pose & base_to_ik_tip) + { + direct_transform(base_to_toollink, endeffector_frame_to_ik_tip_tf_, base_to_ik_tip); } -// using MsgType2 = geometry_msgs::msg::WrenchStamped; -// void transform_message_to_control_frame(const MsgType2 & message_in, MsgType2 & message_out) -// { -// if (control_frame_ != message_in.header.frame_id) { -// geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( -// control_frame_, message_in.header.frame_id, tf2::TimePointZero); -// -// geometry_msgs::msg::Vector3Stamped vec_in; -// geometry_msgs::msg::Vector3Stamped vec_out; -// -// vec_in.vector = message_in.wrench.force; -// vec_in.header = message_in.header; -// tf2::doTransform(vec_in, vec_out, transform); -// message_out.wrench.force = vec_out.vector; -// -// vec_in.vector = message_in.wrench.torque; -// tf2::doTransform(vec_in, vec_out, transform); -// message_out.wrench.torque = vec_out.vector; -// } else { -// message_out = message_in; -// } -// } }; } // namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index a3a168c5ba..abfa63603f 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -20,7 +20,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "moveit/robot_model_loader/robot_model_loader.h" #include "moveit/robot_state/robot_state.h" -#include "rclcpp/utilities.hpp" +#include "rclcpp/rclcpp.hpp" namespace admittance_controller { @@ -42,7 +42,7 @@ class IncrementalKinematics * \return true if successful */ bool - convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec); + convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec); /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. @@ -52,7 +52,7 @@ class IncrementalKinematics * \return true if successful */ bool - convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec); + convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec); private: // MoveIt setup, required to retrieve the Jacobian diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 4e2335b350..4462d05f29 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -19,12 +19,12 @@ #include #include -// #include "Eigen/Core" - +#include "angles/angles.h" #include "admittance_controller/admittance_controller.hpp" #include "admittance_controller/incremental_kinematics.hpp" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller { @@ -45,6 +45,7 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter>("command_interfaces", {}); get_node()->declare_parameter>("state_interfaces", {}); get_node()->declare_parameter("ft_sensor_name", ""); + get_node()->declare_parameter("use_joint_commands_as_input", false); get_node()->declare_parameter("IK.base", ""); get_node()->declare_parameter("IK.tip", ""); @@ -146,6 +147,7 @@ CallbackReturn AdmittanceController::on_configure( get_string_array_param_and_error_if_empty(command_interface_types_, "command_interfaces") || get_string_array_param_and_error_if_empty(state_interface_types_, "state_interfaces") || get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || + get_bool_param_and_error_if_empty(use_joint_commands_as_input_, "use_joint_commands_as_input") || get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || @@ -253,21 +255,33 @@ CallbackReturn AdmittanceController::on_configure( semantic_components::ForceTorqueSensor(ft_sensor_name_)); // Subscribers and callbacks - auto callback_input_force = [&](const std::shared_ptr msg) + if (admittance_->unified_mode_) { + auto callback_input_force = [&](const std::shared_ptr msg) + -> void + { + input_force_command_.writeFromNonRT(msg); + }; + input_force_command_subscriber_ = get_node()->create_subscription( + "~/force_commands", rclcpp::SystemDefaultsQoS(), callback_input_force); + } + + if (use_joint_commands_as_input_) { + auto callback_input_joint = [&](const std::shared_ptr msg) -> void { - input_force_command_.writeFromNonRT(msg); + input_joint_command_.writeFromNonRT(msg); }; - input_force_command_subscriber_ = get_node()->create_subscription( - "~/force_commands", rclcpp::SystemDefaultsQoS(), callback_input_force); - - auto callback_input_pose = [&](const std::shared_ptr msg) - -> void - { - input_pose_command_.writeFromNonRT(msg); - }; - input_pose_command_subscriber_ = get_node()->create_subscription( - "~/pose_commands", rclcpp::SystemDefaultsQoS(), callback_input_pose); + input_joint_command_subscriber_ = get_node()->create_subscription( + "~/joint_commands", rclcpp::SystemDefaultsQoS(), callback_input_joint); + } else { + auto callback_input_pose = [&](const std::shared_ptr msg) + -> void + { + input_pose_command_.writeFromNonRT(msg); + }; + input_pose_command_subscriber_ = get_node()->create_subscription( + "~/pose_commands", rclcpp::SystemDefaultsQoS(), callback_input_pose); + } // TODO(destogl): Add subscriber for velocity scaling @@ -276,6 +290,14 @@ CallbackReturn AdmittanceController::on_configure( "~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(s_publisher_); + // Initialize state message + state_publisher_->lock(); + state_publisher_->msg_.joint_names = joint_names_; + state_publisher_->msg_.actual_joint_states.positions.resize(6, 0.0); + state_publisher_->msg_.desired_joint_states.positions.resize(6, 0.0); + state_publisher_->msg_.error_joint_state.positions.resize(6, 0.0); + state_publisher_->unlock(); + // Configure AdmittanceRule admittance_->configure(get_node()); @@ -355,13 +377,27 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & admittance_->reset(); previous_time_ = get_node()->now(); - // Set initial command values + // Set initial command values - initialize all to simplify update std::shared_ptr msg_force = std::make_shared(); msg_force->header.frame_id = admittance_->control_frame_; input_force_command_.writeFromNonRT(msg_force); + std::shared_ptr msg_joint = std::make_shared(); + msg_joint->joint_names = joint_names_; + msg_joint->points.reserve(1); + + const auto num_joints = joint_names_.size(); + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; + 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_interface_[0][index].get().get_value()); + } + msg_joint->points.emplace_back(trajectory_point); + input_joint_command_.writeFromNonRT(msg_joint); std::shared_ptr msg_pose = std::make_shared(); msg_pose->header.frame_id = admittance_->control_frame_; + admittance_->get_current_pose_of_endeffector_frame(*msg_pose); input_pose_command_.writeFromNonRT(msg_pose); return CallbackReturn::SUCCESS; @@ -390,32 +426,69 @@ CallbackReturn AdmittanceController::on_deactivate( controller_interface::return_type AdmittanceController::update() { // get input commands - // TODO(destogl): Enable this when unified mode is used auto input_force_cmd = input_force_command_.readFromRT(); + auto input_joint_cmd = input_joint_command_.readFromRT(); auto input_pose_cmd = input_pose_command_.readFromRT(); // Position has to always there auto num_joints = joint_state_interface_[0].size(); - std::vector current_joint_states(num_joints); + std::array current_joint_states; + std::array desired_joint_states; - for (auto i = 0u; i < num_joints; ++i) { - current_joint_states.emplace_back(joint_state_interface_[0][i].get().get_value()); + for (auto index = 0u; index < num_joints; ++index) { + current_joint_states[index] = joint_state_interface_[0][index].get().get_value(); } + auto ft_values = force_torque_sensor_->get_values_as_message(); + auto duration_since_last_call = get_node()->now() - previous_time_; + // TODO(destogl): Enable this when unified mode is used -// if (admittance_.unified_mode_) { -// admittance_->update(current_joint_states, force_torque_sensor_->get_values_as_message(), **input_pose_cmd, **input_force_cmd, get_node()->now() - previous_time_); +// if (admittance_->unified_mode_) { + // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_force_cmd, duration_since_last_call, desired_joint_states); // } else { - admittance_->update(current_joint_states, force_torque_sensor_->get_values_as_message(), **input_pose_cmd, get_node()->now() - previous_time_); + if (use_joint_commands_as_input_) { + std::array joint_deltas; + // If there are no positions, expect velocities + // TODO(destogl): add error handling + if ((*input_joint_cmd)->points[0].positions.empty()) { + for (auto index = 0u; index < num_joints; ++index) { + joint_deltas[index] = (*input_joint_cmd)->points[0].velocities[index] * duration_since_last_call.seconds(); + } + } else { + for (auto index = 0u; index < num_joints; ++index) { + // TODO(anyone): Is here OK to use shortest_angular_distance? + joint_deltas[index] = angles::shortest_angular_distance(current_joint_states[index], (*input_joint_cmd)->points[0].positions[index]); + } + } + admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); + } else { + admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_states); + } // } previous_time_ = get_node()->now(); // Write new joint angles to the robot + for (auto index = 0u; index < num_joints; ++index) { + if (has_position_command_interface_) { + joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); + } + if (has_velocity_command_interface_) { + joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( + current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); + } + } // Publish controller state state_publisher_->lock(); state_publisher_->msg_.input_force_command = **input_force_cmd; state_publisher_->msg_.input_pose_command = **input_pose_cmd; + + state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); + state_publisher_->msg_.desired_joint_states.positions.assign(desired_joint_states.begin(), desired_joint_states.end()); + for (auto index = 0u; index < num_joints; ++index) { + state_publisher_->msg_.error_joint_state.positions[index] = angles::shortest_angular_distance( + current_joint_states[index], desired_joint_states[index]); + } admittance_->get_controller_state(state_publisher_->msg_); state_publisher_->unlockAndPublish(); @@ -425,6 +498,8 @@ controller_interface::return_type AdmittanceController::update() } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp" +#include +#include PLUGINLIB_EXPORT_CLASS( admittance_controller::AdmittanceController, diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index fc87049fdf..f2182fbdd5 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -114,7 +114,8 @@ void convert_array_to_message(const std::array & vector, geometry_msg convert_array_to_message(vector, msg_out.wrench); } -void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Transform & msg_out) +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::Transform & msg_out) { msg_out.translation.x = vector[0]; msg_out.translation.y = vector[1]; @@ -129,7 +130,8 @@ void convert_array_to_message(const std::array & vector, geometry_msg msg_out.rotation.w = q.w(); } -void convert_array_to_message(const std::array & vector, geometry_msgs::msg::TransformStamped & msg_out) +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::TransformStamped & msg_out) { convert_array_to_message(vector, msg_out.transform); } @@ -147,8 +149,6 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize variables used in the update loop measured_force_.header.frame_id = sensor_frame_; - current_origin_.header.frame_id = endeffector_frame_; - current_origin_.pose.orientation.w = 1; current_pose_.header.frame_id = endeffector_frame_; relative_desired_pose_.header.frame_id = control_frame_; @@ -163,39 +163,29 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared controller_interface::return_type AdmittanceRule::reset() { - measured_force_control_frame_vec_.fill(0.0); - target_pose_control_frame_vec_.fill(0.0); + measured_force_control_frame_arr_.fill(0.0); + target_pose_control_frame_arr_.fill(0.0); - current_pose_control_frame_vec_.fill(0.0); + current_pose_control_frame_arr_.fill(0.0); angles_error_.fill(0.0); - desired_velocity_vec_.fill(0.0); - desired_velocity_previous_vec_.fill(0.0); - desired_acceleration_previous_vec_.fill(0.0); + desired_velocity_arr_.fill(0.0); + desired_velocity_previous_arr_.fill(0.0); + desired_acceleration_previous_arr_.fill(0.0); - // - use tf2 instead of FK - // - transform data to Controller frame + // Initialize ik_tip and tool_frame transformations - those are fixed transformations + tf2::Stamped tf2_transform; + auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, endeffector_frame_, tf2::TimePointZero); - return controller_interface::return_type::OK; -} - -controller_interface::return_type AdmittanceRule::update( - const std::array & /*current_joint_states*/, - const geometry_msgs::msg::Wrench & /*measured_force*/, - const geometry_msgs::msg::PoseStamped & /*target_pose*/, - const geometry_msgs::msg::WrenchStamped & /*target_force*/, - const rclcpp::Duration & /*period*/, - std::array /*desired_joint_states*/ -) -{ - // TODO(destogl): Implement this update - // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); - // TODO(destogl) reuse things from other update + tf2::fromMsg(transform, tf2_transform); + ik_tip_to_endeffector_frame_tf_ = tf2_transform; + endeffector_frame_to_ik_tip_tf_ = tf2_transform.inverse(); return controller_interface::return_type::OK; } +// Update with target Cartesian pose - the main update method! controller_interface::return_type AdmittanceRule::update( const std::array & current_joint_states, const geometry_msgs::msg::Wrench & measured_force, @@ -213,15 +203,13 @@ controller_interface::return_type AdmittanceRule::update( measured_force_control_frame_ = measured_force_; transform_message_to_control_frame(measured_force_, measured_force_control_frame_); - // Get tool frame position - in the future use: IKSolver->getPositionFK(...) or similar (ask TODO(AndyZe)) - auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); - tf2::doTransform(current_origin_, current_pose_, transform); + get_current_pose_of_endeffector_frame(current_pose_); transform_message_to_control_frame(current_pose_, current_pose_control_frame_); // Convert all data to arrays for simpler calculation - convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_vec_); - convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_vec_); - convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_vec_); + convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_arr_); + convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_arr_); + convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); // Use angle difference calculated with quaternions to avoid confusion with angles get_rpy_difference_between_two_quaternions(target_pose_control_frame_.pose.orientation, @@ -230,43 +218,46 @@ controller_interface::return_type AdmittanceRule::update( // Compute admittance control law: F = M*a + D*v + K*(x_d - x) for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { - double pose_error = target_pose_control_frame_vec_[i] - measured_force_control_frame_vec_[i]; + double pose_error = target_pose_control_frame_arr_[i] - measured_force_control_frame_arr_[i]; if (i >= 3) { pose_error = angles_error_[i-3]; } // TODO(destogl): check if velocity is measured from hardware const double acceleration = 1 / mass_[i] * - (measured_force_control_frame_vec_[i] - damping_[i] * desired_velocity_vec_[i] - + (measured_force_control_frame_arr_[i] - damping_[i] * desired_velocity_arr_[i] - stiffness_[i] * pose_error); - desired_velocity_vec_[i] += (desired_acceleration_previous_vec_[i] + acceleration) * 0.5 * period.seconds(); + desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); - relative_desired_pose_vec_[i] = (desired_velocity_previous_vec_[i] + desired_velocity_vec_[i]) * 0.5 * period.seconds(); + relative_desired_pose_arr_[i] = (desired_velocity_previous_arr_[i] + desired_velocity_arr_[i]) * 0.5 * period.seconds(); - desired_acceleration_previous_vec_[i] = acceleration; - desired_velocity_previous_vec_[i] = desired_velocity_vec_[i]; + desired_acceleration_previous_arr_[i] = acceleration; + desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; } } // TODO: Add here desired vector at the IK tip and not tool!!!! // Do clean conversion to the goal pose using transform and not messing with Euler angles - convert_array_to_message(relative_desired_pose_vec_, relative_desired_pose_); + convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); + ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); // Use Jacobian-based IK // Calculate desired Cartesian displacement of the robot - transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); - std::vector relative_desired_pose_v_(relative_desired_pose_vec_.begin(), relative_desired_pose_vec_.end()); + // TODO: replace this with FK in the long term + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_v_, transform, relative_desired_joint_state_vec_)){ + relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_states.size(); ++i) { desired_joint_states[i] = current_joint_states[i] + relative_desired_joint_state_vec_[i]; } } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); desired_joint_states = current_joint_states; + return controller_interface::return_type::ERROR; } // TODO(anyone: enable this when enabling use of IK directly @@ -278,6 +269,63 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::OK; } +// Update from target joint deltas +controller_interface::return_type AdmittanceRule::update( + const std::array & current_joint_state, + const geometry_msgs::msg::Wrench & measured_force, + const std::array & target_joint_deltas, + const rclcpp::Duration & period, + std::array desired_joint_states) +{ + std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); + std::vector target_ik_tip_deltas_vec(6); + // TODO: replace this with FK in the long term + auto transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + + if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { + } else { + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); + desired_joint_states = current_joint_state; + return controller_interface::return_type::ERROR; + } + + // TODO(destogl): Use as class variables to avoid memory allocation + geometry_msgs::msg::PoseStamped current_ik_tip_pose; + geometry_msgs::msg::TransformStamped current_to_target_ik_pose; + geometry_msgs::msg::PoseStamped target_ik_tip_pose; + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = endeffector_frame_; + static geometry_msgs::msg::PoseStamped origin; + origin.header.frame_id = endeffector_frame_; + origin.pose.orientation.w = 1; + + // If FK this is not needed + // TODO(anyone): Can I just use values from transformation instead calling doTransform? + tf2::doTransform(origin, current_ik_tip_pose, transform_ik_base_tip); + convert_array_to_message(target_ik_tip_deltas_vec, current_to_target_ik_pose); + tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, current_to_target_ik_pose); + + ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_pose.pose); + + return update(current_joint_state, measured_force, target_pose, period, desired_joint_states); +} + +controller_interface::return_type AdmittanceRule::update( + const std::array & /*current_joint_states*/, + const geometry_msgs::msg::Wrench & /*measured_force*/, + const geometry_msgs::msg::PoseStamped & /*target_pose*/, + const geometry_msgs::msg::WrenchStamped & /*target_force*/, + const rclcpp::Duration & /*period*/, + std::array /*desired_joint_states*/ +) +{ + // TODO(destogl): Implement this update + // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); + // TODO(destogl) reuse things from other update + + return controller_interface::return_type::OK; +} + controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message) { @@ -295,4 +343,15 @@ controller_interface::return_type AdmittanceRule::get_controller_state( return controller_interface::return_type::OK; } +controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose) +{ + // Get tool frame position - in the future use: IKSolver->getPositionFK(...) + static geometry_msgs::msg::PoseStamped origin; + origin.header.frame_id = endeffector_frame_; + origin.pose.orientation.w = 1; + + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::doTransform(origin, pose, transform); +} + } // namespace admittance_controller diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 48bc7f6bc7..3014e83178 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -16,7 +16,6 @@ #include "admittance_controller/incremental_kinematics.hpp" -#include "Eigen/VectorXd" #include "tf2_eigen/tf2_eigen.h" namespace admittance_controller @@ -25,7 +24,7 @@ IncrementalKinematics::IncrementalKinematics(const std::shared_ptr { // TODO(andyz): Parameterize robot description and joint group std::unique_ptr model_loader_ptr = - std::unique_ptr(new robot_model_loader::RobotModelLoader(node_, "/robot_description", false /* do not load kinematics plugins */)); + std::unique_ptr(new robot_model_loader::RobotModelLoader(node_, "robot_description", false /* do not load kinematics plugins */)); const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe? joint_model_group_ = kinematic_model->getJointModelGroup(group_name); @@ -34,7 +33,7 @@ IncrementalKinematics::IncrementalKinematics(const std::shared_ptr // By default, the MoveIt Jacobian frame is the last link } -bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec) +bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); @@ -83,7 +82,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vecto return true; } -bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec) +bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_theta = Eigen::Map(&delta_theta_vec[0], delta_theta_vec.size()); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 97633f47b6..6360bd08e1 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -50,6 +50,10 @@ INSTANTIATE_TEST_CASE_P( 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("IK.base"), rclcpp::ParameterValue("") diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index eda21df1e1..d3a4da8b89 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -151,6 +151,7 @@ class AdmittanceControllerTest : public ::testing::Test 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({"use_joint_commands_as_input", use_joint_commands_as_input_}); controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); @@ -237,27 +238,40 @@ class AdmittanceControllerTest : public ::testing::Test 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 = control_frame_; - transform_stamped.transform.translation.x = 0; - transform_stamped.transform.translation.y = 0; - transform_stamped.transform.translation.z = 1; + 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 = sensor_frame_; + transform_stamped.child_frame_id = ik_base_frame_; br.sendTransform(transform_stamped); - transform_stamped.child_frame_id = endeffector_frame_; + transform_stamped.child_frame_id = ik_tip_frame_; br.sendTransform(transform_stamped); - transform_stamped.child_frame_id = ik_base_frame_; + 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; - transform_stamped.child_frame_id = fixed_world_frame_; + 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); } @@ -268,7 +282,7 @@ class AdmittanceControllerTest : public ::testing::Test { }; auto subscription = - test_subscription_node_.create_subscription( + test_subscription_node_->create_subscription( "/test_admittance_controller/state", 10, subs_callback); // call update to publish the test value @@ -332,11 +346,13 @@ class AdmittanceControllerTest : public ::testing::Test const std::vector command_interface_types_ = {"position"}; const std::vector state_interface_types_ = {"position"}; const std::string ft_sensor_name_ = "ft_sensor_name"; + // TODO: We also need tests with false - there are many combinations here. It will be fun! + const bool use_joint_commands_as_input_ = true; - const std::string ik_base_frame_ = "IK_base"; - const std::string ik_tip_frame_ = "IK.tip"; - const std::string ik_group_name_ = "IK.group_name"; - const std::string robot_description_ = ros2_control_test_assets::6d_robot_urdf; + const std::string ik_base_frame_ = "base_link"; + const std::string ik_tip_frame_ = "tool0"; + const std::string ik_group_name_ = "kuka_kr6"; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string control_frame_ = "control_frame"; const std::string endeffector_frame_ = "endeffector_frame"; From 05a518d149ff1ca064fdd5b9a6b3cf5a6b318597 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 8 May 2021 01:21:01 +0200 Subject: [PATCH 14/99] Corrected subscriptions types and debugging --- .../src/admittance_controller.cpp | 33 +++++++------- admittance_controller/src/admittance_rule.cpp | 45 +++++++++++++++---- 2 files changed, 52 insertions(+), 26 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 4462d05f29..bd0fa2a688 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -265,23 +265,21 @@ CallbackReturn AdmittanceController::on_configure( "~/force_commands", rclcpp::SystemDefaultsQoS(), callback_input_force); } - if (use_joint_commands_as_input_) { - auto callback_input_joint = [&](const std::shared_ptr msg) - -> void - { - input_joint_command_.writeFromNonRT(msg); - }; - input_joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_commands", rclcpp::SystemDefaultsQoS(), callback_input_joint); - } else { - auto callback_input_pose = [&](const std::shared_ptr msg) - -> void - { - input_pose_command_.writeFromNonRT(msg); - }; - input_pose_command_subscriber_ = get_node()->create_subscription( - "~/pose_commands", rclcpp::SystemDefaultsQoS(), callback_input_pose); - } + auto callback_input_joint = [&](const std::shared_ptr msg) + -> void + { + input_joint_command_.writeFromNonRT(msg); + }; + input_joint_command_subscriber_ = get_node()->create_subscription( + "~/joint_commands", rclcpp::SystemDefaultsQoS(), callback_input_joint); + + auto callback_input_pose = [&](const std::shared_ptr msg) + -> void + { + input_pose_command_.writeFromNonRT(msg); + }; + input_pose_command_subscriber_ = get_node()->create_subscription( + "~/pose_commands", rclcpp::SystemDefaultsQoS(), callback_input_pose); // TODO(destogl): Add subscriber for velocity scaling @@ -482,6 +480,7 @@ controller_interface::return_type AdmittanceController::update() state_publisher_->lock(); state_publisher_->msg_.input_force_command = **input_force_cmd; state_publisher_->msg_.input_pose_command = **input_pose_cmd; + state_publisher_->msg_.input_joint_command = **input_joint_cmd; state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); state_publisher_->msg_.desired_joint_states.positions.assign(desired_joint_states.begin(), desired_joint_states.end()); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index f2182fbdd5..a6dbfee306 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -176,11 +176,16 @@ controller_interface::return_type AdmittanceRule::reset() // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; - auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, endeffector_frame_, tf2::TimePointZero); - - tf2::fromMsg(transform, tf2_transform); - ik_tip_to_endeffector_frame_tf_ = tf2_transform; - endeffector_frame_to_ik_tip_tf_ = tf2_transform.inverse(); + try { + auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::fromMsg(transform, tf2_transform); + ik_tip_to_endeffector_frame_tf_ = tf2_transform; + endeffector_frame_to_ik_tip_tf_ = tf2_transform.inverse(); + } catch (tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_tip_frame_ + "' and '" + endeffector_frame_ + "'."); + return controller_interface::return_type::ERROR; + } return controller_interface::return_type::OK; } @@ -247,7 +252,15 @@ controller_interface::return_type AdmittanceRule::update( // Use Jacobian-based IK // Calculate desired Cartesian displacement of the robot // TODO: replace this with FK in the long term - auto transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); + return controller_interface::return_type::ERROR; + } + std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ @@ -280,7 +293,14 @@ controller_interface::return_type AdmittanceRule::update( std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); // TODO: replace this with FK in the long term - auto transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + geometry_msgs::msg::TransformStamped transform_ik_base_tip; + try { + transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + } catch (tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); + return controller_interface::return_type::ERROR; + } if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { } else { @@ -350,8 +370,15 @@ controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffecto origin.header.frame_id = endeffector_frame_; origin.pose.orientation.w = 1; - auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); - tf2::doTransform(origin, pose, transform); + try { + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::doTransform(origin, pose, transform); + } catch (tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + endeffector_frame_ + "'."); + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; } } // namespace admittance_controller From c1d90f2d7753da949063cd3f5b7be06bbe711414 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 11 May 2021 16:26:29 +0200 Subject: [PATCH 15/99] Corrected some tests and extended testing --- .../admittance_controller/admittance_rule.hpp | 2 +- .../src/admittance_controller.cpp | 1 + admittance_controller/src/admittance_rule.cpp | 11 +- .../test/test_admittance_controller.cpp | 106 ++++++++++++++++-- .../test/test_admittance_controller.hpp | 63 +++++++---- 5 files changed, 144 insertions(+), 39 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 9a8cd65853..65fae658bc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -143,7 +143,7 @@ class AdmittanceRule geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( control_frame_, message_in.header.frame_id, tf2::TimePointZero); tf2::doTransform(message_in, message_out, transform); - } catch (tf2::TransformException e) { + } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + message_in.header.frame_id + "'."); return controller_interface::return_type::ERROR; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index bd0fa2a688..16a9e82c27 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -379,6 +379,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & std::shared_ptr msg_force = std::make_shared(); msg_force->header.frame_id = admittance_->control_frame_; input_force_command_.writeFromNonRT(msg_force); + std::shared_ptr msg_joint = std::make_shared(); msg_joint->joint_names = joint_names_; msg_joint->points.reserve(1); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index a6dbfee306..7427443184 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -181,7 +181,7 @@ controller_interface::return_type AdmittanceRule::reset() tf2::fromMsg(transform, tf2_transform); ik_tip_to_endeffector_frame_tf_ = tf2_transform; endeffector_frame_to_ik_tip_tf_ = tf2_transform.inverse(); - } catch (tf2::TransformException & e) { + } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_tip_frame_ + "' and '" + endeffector_frame_ + "'."); return controller_interface::return_type::ERROR; @@ -255,7 +255,7 @@ controller_interface::return_type AdmittanceRule::update( geometry_msgs::msg::TransformStamped transform; try { transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); - } catch (tf2::TransformException & e) { + } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); return controller_interface::return_type::ERROR; @@ -296,7 +296,7 @@ controller_interface::return_type AdmittanceRule::update( geometry_msgs::msg::TransformStamped transform_ik_base_tip; try { transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); - } catch (tf2::TransformException & e) { + } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); return controller_interface::return_type::ERROR; @@ -356,9 +356,6 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_force_endeffector_frame = measured_force_control_frame_; state_message.desired_pose = desired_pose_; state_message.relative_desired_pose = relative_desired_pose_; -// state_message.desired_joint_states = desired_pose_; -// state_message.actual_joint_states.positi = desired_pose_; - // state_message.error_joint_state = desired_pose_; return controller_interface::return_type::OK; } @@ -373,7 +370,7 @@ controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffecto try { auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); tf2::doTransform(origin, pose, transform); - } catch (tf2::TransformException & e) { + } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + endeffector_frame_ + "'."); return controller_interface::return_type::ERROR; diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 6360bd08e1..b620cdfbff 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -1,5 +1,4 @@ -// Copyright (c) 2021, salfkjsadf -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// 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. @@ -12,6 +11,8 @@ // 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" @@ -323,7 +324,8 @@ TEST_F(AdmittanceControllerTest, reactivate_success) TEST_F(AdmittanceControllerTest, publish_status_success) { - SetUpController(); + // 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); @@ -334,14 +336,101 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - // Check default parameters + // Check that force command are all zero since not used ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_force_command.wrench.force.x, 0.0); + ASSERT_EQ(msg.input_force_command.wrench.force.y, 0.0); + ASSERT_EQ(msg.input_force_command.wrench.force.z, 0.0); + ASSERT_EQ(msg.input_force_command.wrench.torque.x, 0.0); + ASSERT_EQ(msg.input_force_command.wrench.torque.y, 0.0); + ASSERT_EQ(msg.input_force_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_force.header.frame_id, sensor_frame_); + ASSERT_EQ(msg.measured_force.wrench.force.x, fts_state_values_[0]); + ASSERT_EQ(msg.measured_force.wrench.force.y, fts_state_values_[1]); + ASSERT_EQ(msg.measured_force.wrench.force.z, fts_state_values_[2]); + ASSERT_EQ(msg.measured_force.wrench.torque.x, fts_state_values_[3]); + ASSERT_EQ(msg.measured_force.wrench.torque.y, fts_state_values_[4]); + ASSERT_EQ(msg.measured_force.wrench.torque.z, fts_state_values_[5]); + + ASSERT_EQ(msg.measured_force_control_frame.header.frame_id, control_frame_); + ASSERT_EQ(msg.measured_force_control_frame.wrench.force.x, 0.0); + ASSERT_EQ(msg.measured_force_control_frame.wrench.force.y, 0.0); + ASSERT_EQ(msg.measured_force_control_frame.wrench.force.z, 0.0); + ASSERT_EQ(msg.measured_force_control_frame.wrench.torque.x, 0.0); + ASSERT_EQ(msg.measured_force_control_frame.wrench.torque.y, 0.0); + ASSERT_EQ(msg.measured_force_control_frame.wrench.torque.z, 0.0); + + ASSERT_EQ(msg.measured_force_endeffector_frame.header.frame_id, endeffector_frame_); + ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.force.x, 0.0); + ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.force.y, 0.0); + ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.force.z, 0.0); + ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.torque.x, 0.0); + ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.torque.y, 0.0); + ASSERT_EQ(msg.measured_force_endeffector_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_states.positions.begin(), msg.actual_joint_states.positions.end(), + joint_state_values_.begin(), joint_state_values_.end())); + + ASSERT_TRUE(std::equal( + msg.actual_joint_states.positions.begin(), msg.actual_joint_states.positions.end(), + joint_state_values_.begin(), joint_state_values_.end())); + + ASSERT_TRUE(std::equal( + msg.desired_joint_states.positions.begin(), msg.desired_joint_states.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(); + SetUpController(true, true); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -350,6 +439,9 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) broadcast_tfs(); ASSERT_EQ(controller_->update(), 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_force_command.header.frame_id, control_frame_); @@ -361,10 +453,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) broadcast_tfs(); ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[0], 0.0); + EXPECT_NEAR(joint_command_values_[0], 0.0, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - ASSERT_EQ(msg.input_force_command.header.frame_id, sensor_frame_); + ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); ASSERT_EQ(msg.input_pose_command.header.frame_id, endeffector_frame_); } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index d3a4da8b89..1e95d3c89b 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -1,5 +1,4 @@ -// Copyright (c) 2021, salfkjsadf -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// 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. @@ -12,6 +11,8 @@ // 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_ @@ -42,18 +43,20 @@ using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; namespace { - constexpr auto NODE_SUCCESS = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - constexpr auto NODE_ERROR = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 0.001; - 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(); - } +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 @@ -66,6 +69,7 @@ class TestableAdmittanceController 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 @@ -74,7 +78,9 @@ class TestableAdmittanceController admittance_controller::AdmittanceController::on_configure(previous_state); // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - input_force_command_subscriber_wait_set_.add_subscription(input_force_command_subscriber_); + if (admittance_->unified_mode_) { + input_force_command_subscriber_wait_set_.add_subscription(input_force_command_subscriber_); + } input_pose_command_subscriber_wait_set_.add_subscription(input_pose_command_subscriber_); } return ret; @@ -91,9 +97,11 @@ class TestableAdmittanceController rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds {500}) { - bool success = - input_force_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready && - 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 (admittance_->unified_mode_) { + success = success && input_force_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + } if (success) { executor.spin_some(); } @@ -139,7 +147,7 @@ class AdmittanceControllerTest : public ::testing::Test } protected: - void SetUpController(bool set_parameters = true) + 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); @@ -147,11 +155,12 @@ class AdmittanceControllerTest : public ::testing::Test 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({"use_joint_commands_as_input", use_joint_commands_as_input_}); controller_->get_node()->set_parameter({"IK.base", ik_base_frame_}); controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_}); @@ -159,6 +168,7 @@ class AdmittanceControllerTest : public ::testing::Test // 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_}); @@ -312,7 +322,10 @@ class AdmittanceControllerTest : public ::testing::Test } }; - wait_for_topic(force_command_publisher_->get_topic_name()); + // 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()); ControllerCommandForceMsg force_msg; @@ -334,7 +347,10 @@ class AdmittanceControllerTest : public ::testing::Test pose_msg.pose.orientation.z = 0; pose_msg.pose.orientation.w = 1; - force_command_publisher_->publish(force_msg); + // 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); } @@ -346,13 +362,12 @@ class AdmittanceControllerTest : public ::testing::Test const std::vector command_interface_types_ = {"position"}; const std::vector state_interface_types_ = {"position"}; const std::string ft_sensor_name_ = "ft_sensor_name"; - // TODO: We also need tests with false - there are many combinations here. It will be fun! - const bool use_joint_commands_as_input_ = true; const std::string ik_base_frame_ = "base_link"; const std::string ik_tip_frame_ = "tool0"; - const std::string ik_group_name_ = "kuka_kr6"; + 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"; From a45e395a567bb77bc92a13d85d73bb9dace17053 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 11 May 2021 21:41:35 +0200 Subject: [PATCH 16/99] Debugging admittance controller --- .../admittance_controller/admittance_rule.hpp | 1 + .../src/admittance_controller.cpp | 18 +++++++++++++++- admittance_controller/src/admittance_rule.cpp | 3 ++- .../test/test_admittance_controller.hpp | 21 +++++++++++++++++++ 4 files changed, 41 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 65fae658bc..5359026da3 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -106,6 +106,7 @@ class AdmittanceRule tf2::Transform endeffector_frame_to_ik_tip_tf_; geometry_msgs::msg::WrenchStamped measured_force_; + geometry_msgs::msg::WrenchStamped measured_force_filtered_; geometry_msgs::msg::WrenchStamped measured_force_control_frame_; geometry_msgs::msg::PoseStamped origin_ik_tip_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 16a9e82c27..fae50530f0 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -14,6 +14,7 @@ // /// \author: Denis Stogl +#include #include #include #include @@ -299,6 +300,21 @@ CallbackReturn AdmittanceController::on_configure( // Configure AdmittanceRule admittance_->configure(get_node()); + // wait for TF to become available. Important to do there because it is blocking, i.e. non real-time safe code + std::shared_ptr msg_pose = std::make_shared(); + // TODO(destogl): This will break tests because there is no TF inside them + auto iterations = 0u; + const auto max_iterations = 20u; + while (admittance_->get_current_pose_of_endeffector_frame(*msg_pose) != controller_interface::return_type::OK) + { + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 5000, "Waiting for base to endeffector transform becomes available."); + rclcpp::sleep_for(std::chrono::seconds(1)); + if (++iterations > max_iterations) { + RCLCPP_ERROR(get_node()->get_logger(), "After waiting for TF for %zu seconds, still no transformation available. Admittance Controller can not be configured.", max_iterations); + return CallbackReturn::ERROR; + } + } + RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -483,8 +499,8 @@ controller_interface::return_type AdmittanceController::update() state_publisher_->msg_.input_pose_command = **input_pose_cmd; state_publisher_->msg_.input_joint_command = **input_joint_cmd; - state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); state_publisher_->msg_.desired_joint_states.positions.assign(desired_joint_states.begin(), desired_joint_states.end()); + state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); for (auto index = 0u; index < num_joints; ++index) { state_publisher_->msg_.error_joint_state.positions[index] = angles::shortest_angular_distance( current_joint_states[index], desired_joint_states[index]); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 7427443184..07ff404398 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -205,7 +205,7 @@ controller_interface::return_type AdmittanceRule::update( // get current states, and transform those into controller frame measured_force_.wrench = measured_force; // TODO(destogl): Add gravity compensation of measured forces - measured_force_control_frame_ = measured_force_; + measured_force_filtered_ = measured_force_; transform_message_to_control_frame(measured_force_, measured_force_control_frame_); get_current_pose_of_endeffector_frame(current_pose_); @@ -352,6 +352,7 @@ controller_interface::return_type AdmittanceRule::get_controller_state( // state_message.input_force_control_frame = target_force_control_frame_; state_message.input_pose_control_frame = target_pose_control_frame_; state_message.measured_force = measured_force_; + state_message.measured_force = measured_force_filtered_; state_message.measured_force_control_frame = measured_force_control_frame_; state_message.measured_force_endeffector_frame = measured_force_control_frame_; state_message.desired_pose = desired_pose_; diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 1e95d3c89b..3d04e7607a 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -34,11 +34,13 @@ #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 ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory; using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; namespace @@ -131,6 +133,8 @@ class AdmittanceControllerTest : public ::testing::Test "/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"); @@ -326,6 +330,7 @@ class AdmittanceControllerTest : public ::testing::Test // if (controller_->admittance_->unified_mode_) { // wait_for_topic(force_command_publisher_->get_topic_name()); // } + wait_for_topic(pose_command_publisher_->get_topic_name()); ControllerCommandForceMsg force_msg; @@ -352,6 +357,21 @@ class AdmittanceControllerTest : public ::testing::Test // 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: @@ -392,6 +412,7 @@ class AdmittanceControllerTest : public ::testing::Test 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 968841476c6efe416990a1c0cf920a1241f5b09e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 11 May 2021 19:00:47 -0500 Subject: [PATCH 17/99] Wait for valid wrench data --- admittance_controller/src/admittance_controller.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index fae50530f0..d201b6393e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -455,6 +456,17 @@ controller_interface::return_type AdmittanceController::update() } auto ft_values = force_torque_sensor_->get_values_as_message(); + // Wait for valid data + if (isnan(ft_values.force.x) || isnan(ft_values.torque.x)) + { + ft_values.force.x = 0; + ft_values.force.y = 0; + ft_values.force.z = 0; + ft_values.torque.x = 0; + ft_values.torque.y = 0; + ft_values.torque.z = 0; + } + auto duration_since_last_call = get_node()->now() - previous_time_; // TODO(destogl): Enable this when unified mode is used From 255e5ca5b0522fcd0443b2c03e700ba5915d9027 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 11 May 2021 19:22:03 -0500 Subject: [PATCH 18/99] [WIP] Debug current joint states --- admittance_controller/src/admittance_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index d201b6393e..4901fc2d40 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -453,6 +453,8 @@ controller_interface::return_type AdmittanceController::update() for (auto index = 0u; index < num_joints; ++index) { current_joint_states[index] = joint_state_interface_[0][index].get().get_value(); + // TODO(andyz): the values here are not correct + std::cout << current_joint_states[index] << std::endl; } auto ft_values = force_torque_sensor_->get_values_as_message(); From b6ef47b5bd1e37496153598a4a790c413546a863 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 12 May 2021 21:50:18 +0200 Subject: [PATCH 19/99] Now output is working --- .../admittance_controller/admittance_rule.hpp | 7 +++-- .../src/admittance_controller.cpp | 30 ++++++------------- admittance_controller/src/admittance_rule.cpp | 26 ++++++++++++---- 3 files changed, 34 insertions(+), 29 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 5359026da3..9d58d5d8e2 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -43,14 +43,14 @@ class AdmittanceRule const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, - std::array desired_joint_states); + std::array & desired_joint_states); controller_interface::return_type update( const std::array & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const std::array & target_joint_deltas, const rclcpp::Duration & period, - std::array desired_joint_states); + std::array & desired_joint_states); controller_interface::return_type update( const std::array & current_joint_state, @@ -58,7 +58,7 @@ class AdmittanceRule const geometry_msgs::msg::PoseStamped & target_pose, const geometry_msgs::msg::WrenchStamped & target_force, const rclcpp::Duration & period, - std::array desired_joint_states); + std::array & desired_joint_states); // controller_interface::return_type update( // const geometry_msgs::msg::WrenchStamped & measured_force, @@ -108,6 +108,7 @@ class AdmittanceRule geometry_msgs::msg::WrenchStamped measured_force_; geometry_msgs::msg::WrenchStamped measured_force_filtered_; geometry_msgs::msg::WrenchStamped measured_force_control_frame_; + geometry_msgs::msg::WrenchStamped measured_force_endeffector_frame_; geometry_msgs::msg::PoseStamped origin_ik_tip_; geometry_msgs::msg::PoseStamped origin_endeffector_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 4901fc2d40..39b4f34fa4 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -453,21 +453,9 @@ controller_interface::return_type AdmittanceController::update() for (auto index = 0u; index < num_joints; ++index) { current_joint_states[index] = joint_state_interface_[0][index].get().get_value(); - // TODO(andyz): the values here are not correct - std::cout << current_joint_states[index] << std::endl; } auto ft_values = force_torque_sensor_->get_values_as_message(); - // Wait for valid data - if (isnan(ft_values.force.x) || isnan(ft_values.torque.x)) - { - ft_values.force.x = 0; - ft_values.force.y = 0; - ft_values.force.z = 0; - ft_values.torque.x = 0; - ft_values.torque.y = 0; - ft_values.torque.z = 0; - } auto duration_since_last_call = get_node()->now() - previous_time_; @@ -497,15 +485,15 @@ controller_interface::return_type AdmittanceController::update() previous_time_ = get_node()->now(); // Write new joint angles to the robot - for (auto index = 0u; index < num_joints; ++index) { - if (has_position_command_interface_) { - joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); - } - if (has_velocity_command_interface_) { - joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( - current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); - } - } +// for (auto index = 0u; index < num_joints; ++index) { +// if (has_position_command_interface_) { +// joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); +// } +// if (has_velocity_command_interface_) { +// joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( +// current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); +// } +// } // Publish controller state state_publisher_->lock(); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 07ff404398..deaa8182b6 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -196,7 +196,7 @@ controller_interface::return_type AdmittanceRule::update( const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, - std::array desired_joint_states + std::array & desired_joint_states ) { // Convert inputs to control frame @@ -220,10 +220,15 @@ controller_interface::return_type AdmittanceRule::update( get_rpy_difference_between_two_quaternions(target_pose_control_frame_.pose.orientation, current_pose_control_frame_.pose.orientation, angles_error_); + // If at least one measured force is nan set all to 0 + if (std::find_if(measured_force_control_frame_arr_.begin(), measured_force_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_force_control_frame_arr_.end()) { + measured_force_control_frame_arr_.fill(0.0); + } + // Compute admittance control law: F = M*a + D*v + K*(x_d - x) for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { - double pose_error = target_pose_control_frame_arr_[i] - measured_force_control_frame_arr_[i]; + double pose_error = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; if (i >= 3) { pose_error = angles_error_[i-3]; } @@ -266,6 +271,7 @@ controller_interface::return_type AdmittanceRule::update( relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_states.size(); ++i) { desired_joint_states[i] = current_joint_states[i] + relative_desired_joint_state_vec_[i]; + RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_states[i], relative_desired_joint_state_vec_[i], desired_joint_states[i]); } } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); @@ -288,7 +294,7 @@ controller_interface::return_type AdmittanceRule::update( const geometry_msgs::msg::Wrench & measured_force, const std::array & target_joint_deltas, const rclcpp::Duration & period, - std::array desired_joint_states) + std::array & desired_joint_states) { std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); @@ -336,7 +342,7 @@ controller_interface::return_type AdmittanceRule::update( const geometry_msgs::msg::PoseStamped & /*target_pose*/, const geometry_msgs::msg::WrenchStamped & /*target_force*/, const rclcpp::Duration & /*period*/, - std::array /*desired_joint_states*/ + std::array & /*desired_joint_states*/ ) { // TODO(destogl): Implement this update @@ -354,7 +360,17 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_force = measured_force_; state_message.measured_force = measured_force_filtered_; state_message.measured_force_control_frame = measured_force_control_frame_; - state_message.measured_force_endeffector_frame = measured_force_control_frame_; + + try { + auto transform = tf_buffer_->lookupTransform(control_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::doTransform(measured_force_control_frame_, measured_force_endeffector_frame_, transform); + } catch (const tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + endeffector_frame_ + "'."); + } + + state_message.measured_force_endeffector_frame = measured_force_endeffector_frame_; + state_message.desired_pose = desired_pose_; state_message.relative_desired_pose = relative_desired_pose_; From ad13200b1c140b0cd4cc51123617c974cfd5fd16 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 12 May 2021 22:39:44 -0500 Subject: [PATCH 20/99] Pseudoinverse method was buggy. Use a different one. --- admittance_controller/src/incremental_kinematics.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 3014e83178..9404434f51 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -62,6 +62,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorget_logger(), delta_x[0] << " " << delta_x[1] << " " << delta_x[2] << " " << delta_x[3]); } catch (tf2::TransformException & ex) { @@ -71,11 +72,15 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorgetJacobian(joint_model_group_); - svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); - matrix_s_ = svd_.singularValues().asDiagonal(); - pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); + // TODO(andyz): the SVD method here is buggy. It should be more stable near singularities. + // svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); + // matrix_s_ = svd_.singularValues().asDiagonal(); + // pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); + pseudo_inverse_ = jacobian_.transpose() * (jacobian_ * jacobian_.transpose()).inverse(); Eigen::VectorXd delta_theta = pseudo_inverse_ * delta_x; + RCLCPP_ERROR_STREAM(node_->get_logger(), pseudo_inverse_.matrix()); + std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); delta_theta_vec = delta_theta_v; From 749a57a2ebcce87c41de1ecef717c882dd676f27 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Thu, 13 May 2021 22:06:27 +0200 Subject: [PATCH 21/99] Admittance controller is working --- .../admittance_controller/admittance_rule.hpp | 4 +- .../src/admittance_controller.cpp | 18 ++-- admittance_controller/src/admittance_rule.cpp | 101 +++++++++--------- .../src/incremental_kinematics.cpp | 8 +- 4 files changed, 67 insertions(+), 64 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 9d58d5d8e2..2285746616 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -168,13 +168,13 @@ class AdmittanceRule } void - ik_tip_to_endeffector_frame(const geometry_msgs::msg::Pose & base_to_ik_tip, geometry_msgs::msg::Pose & base_to_toollink) + transform_ik_tip_to_endeffector_frame(const geometry_msgs::msg::Pose & base_to_ik_tip, geometry_msgs::msg::Pose & base_to_toollink) { direct_transform(base_to_ik_tip, ik_tip_to_endeffector_frame_tf_, base_to_toollink); } void - endeffector_to_ik_tip_frame(const geometry_msgs::msg::Pose & base_to_toollink, geometry_msgs::msg::Pose & base_to_ik_tip) + transform_endeffector_to_ik_tip_frame(const geometry_msgs::msg::Pose & base_to_toollink, geometry_msgs::msg::Pose & base_to_ik_tip) { direct_transform(base_to_toollink, endeffector_frame_to_ik_tip_tf_, base_to_ik_tip); } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 39b4f34fa4..3864b766cd 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -485,15 +485,15 @@ controller_interface::return_type AdmittanceController::update() previous_time_ = get_node()->now(); // Write new joint angles to the robot -// for (auto index = 0u; index < num_joints; ++index) { -// if (has_position_command_interface_) { -// joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); -// } -// if (has_velocity_command_interface_) { -// joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( -// current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); -// } -// } + for (auto index = 0u; index < num_joints; ++index) { + if (has_position_command_interface_) { + joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); + } + if (has_velocity_command_interface_) { + joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( + current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); + } + } // Publish controller state state_publisher_->lock(); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index deaa8182b6..02d9acb6b1 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -78,41 +78,40 @@ void convert_message_to_array( convert_message_to_array(msg.wrench, vector_out); } -void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Pose & msg_out) -{ - msg_out.position.x = vector[0]; - msg_out.position.y = vector[1]; - msg_out.position.z = vector[2]; - - tf2::Quaternion q; - q.setRPY(vector[3], vector[4], vector[5]); - - msg_out.orientation.x = q.x(); - msg_out.orientation.y = q.y(); - msg_out.orientation.z = q.z(); - msg_out.orientation.w = q.w(); -} - -void convert_array_to_message(const std::array & vector, geometry_msgs::msg::PoseStamped & msg_out) -{ - convert_array_to_message(vector, msg_out.pose); -} - -void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Wrench & msg_out) -{ - msg_out.force.x = vector[0]; - msg_out.force.y = vector[1]; - msg_out.force.z = vector[2]; - msg_out.torque.x = vector[3]; - msg_out.torque.y = vector[4]; - msg_out.torque.z = vector[5]; - -} - -void convert_array_to_message(const std::array & vector, geometry_msgs::msg::WrenchStamped & msg_out) -{ - convert_array_to_message(vector, msg_out.wrench); -} +// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Pose & msg_out) +// { +// msg_out.position.x = vector[0]; +// msg_out.position.y = vector[1]; +// msg_out.position.z = vector[2]; +// +// tf2::Quaternion q; +// q.setRPY(vector[3], vector[4], vector[5]); +// +// msg_out.orientation.x = q.x(); +// msg_out.orientation.y = q.y(); +// msg_out.orientation.z = q.z(); +// msg_out.orientation.w = q.w(); +// } + +// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::PoseStamped & msg_out) +// { +// convert_array_to_message(vector, msg_out.pose); +// } + +// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Wrench & msg_out) +// { +// msg_out.force.x = vector[0]; +// msg_out.force.y = vector[1]; +// msg_out.force.z = vector[2]; +// msg_out.torque.x = vector[3]; +// msg_out.torque.y = vector[4]; +// msg_out.torque.z = vector[5]; +// } + +// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::WrenchStamped & msg_out) +// { +// convert_array_to_message(vector, msg_out.wrench); +// } template void convert_array_to_message(const Type & vector, geometry_msgs::msg::Transform & msg_out) @@ -206,7 +205,7 @@ controller_interface::return_type AdmittanceRule::update( measured_force_.wrench = measured_force; // TODO(destogl): Add gravity compensation of measured forces measured_force_filtered_ = measured_force_; - transform_message_to_control_frame(measured_force_, measured_force_control_frame_); + transform_message_to_control_frame(measured_force_filtered_, measured_force_control_frame_); get_current_pose_of_endeffector_frame(current_pose_); transform_message_to_control_frame(current_pose_, current_pose_control_frame_); @@ -217,8 +216,8 @@ controller_interface::return_type AdmittanceRule::update( convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); // Use angle difference calculated with quaternions to avoid confusion with angles - get_rpy_difference_between_two_quaternions(target_pose_control_frame_.pose.orientation, - current_pose_control_frame_.pose.orientation, angles_error_); + // TODO(destogl): Is this really needed? This introduces tracking error, so not working as expected + get_rpy_difference_between_two_quaternions( current_pose_control_frame_.pose.orientation, target_pose_control_frame_.pose.orientation, angles_error_); // If at least one measured force is nan set all to 0 if (std::find_if(measured_force_control_frame_arr_.begin(), measured_force_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_force_control_frame_arr_.end()) { @@ -228,14 +227,16 @@ controller_interface::return_type AdmittanceRule::update( // Compute admittance control law: F = M*a + D*v + K*(x_d - x) for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { - double pose_error = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; + double pose_error = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; if (i >= 3) { - pose_error = angles_error_[i-3]; + pose_error = angles::normalize_angle(current_pose_control_frame_arr_[i]) - + angles::normalize_angle(target_pose_control_frame_arr_[i]); +// pose_error = angles_error_[i-3]; } // TODO(destogl): check if velocity is measured from hardware const double acceleration = 1 / mass_[i] * - (measured_force_control_frame_arr_[i] - damping_[i] * desired_velocity_arr_[i] - + (measured_force_control_frame_arr_[i] - damping_[i] * desired_velocity_arr_[i] - stiffness_[i] * pose_error); desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); @@ -244,15 +245,15 @@ controller_interface::return_type AdmittanceRule::update( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; + +// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); } } - // TODO: Add here desired vector at the IK tip and not tool!!!! - // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); - ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); + transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); // Use Jacobian-based IK // Calculate desired Cartesian displacement of the robot @@ -271,7 +272,7 @@ controller_interface::return_type AdmittanceRule::update( relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_states.size(); ++i) { desired_joint_states[i] = current_joint_states[i] + relative_desired_joint_state_vec_[i]; - RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_states[i], relative_desired_joint_state_vec_[i], desired_joint_states[i]); +// RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_states[i], relative_desired_joint_state_vec_[i], desired_joint_states[i]); } } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); @@ -282,7 +283,7 @@ controller_interface::return_type AdmittanceRule::update( // TODO(anyone: enable this when enabling use of IK directly // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); // tf2::doTransform(desired_pose_, ik_input_pose_, transform); - // ik_input_pose_.pose = endeffector_to_ik_tip(ik_input_pose_); + // ik_input_pose_.pose = transform_endeffector_to_ik_tip_frame(ik_input_pose_); // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... return controller_interface::return_type::OK; @@ -320,18 +321,20 @@ controller_interface::return_type AdmittanceRule::update( geometry_msgs::msg::TransformStamped current_to_target_ik_pose; geometry_msgs::msg::PoseStamped target_ik_tip_pose; geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = endeffector_frame_; static geometry_msgs::msg::PoseStamped origin; origin.header.frame_id = endeffector_frame_; origin.pose.orientation.w = 1; // If FK this is not needed - // TODO(anyone): Can I just use values from transformation instead calling doTransform? + // TODO(anyone): Can we just use values from transformation instead calling doTransform? tf2::doTransform(origin, current_ik_tip_pose, transform_ik_base_tip); convert_array_to_message(target_ik_tip_deltas_vec, current_to_target_ik_pose); + current_to_target_ik_pose.header.frame_id = ik_base_frame_; + current_to_target_ik_pose.child_frame_id = ik_base_frame_; tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, current_to_target_ik_pose); - ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_pose.pose); + transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_pose.pose); + target_pose.header = target_ik_tip_pose.header; return update(current_joint_state, measured_force, target_pose, period, desired_joint_states); } diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 9404434f51..6e83196163 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -62,9 +62,9 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorget_logger(), delta_x[0] << " " << delta_x[1] << " " << delta_x[2] << " " << delta_x[3]); +// RCLCPP_ERROR_STREAM(node_->get_logger(), delta_x[0] << " " << delta_x[1] << " " << delta_x[2] << " " << delta_x[3]); } - catch (tf2::TransformException & ex) + catch (const tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "Transformation of wrench failed."); return false; @@ -79,7 +79,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorget_logger(), pseudo_inverse_.matrix()); +// RCLCPP_ERROR_STREAM(node_->get_logger(), pseudo_inverse_.matrix()); std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); delta_theta_vec = delta_theta_v; @@ -122,7 +122,7 @@ bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(std::vectorget_logger(), "Transformation of wrench failed."); return false; From e04959373c8d88ba025d51d2a47a7c6a891713c3 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Fri, 14 May 2021 20:34:15 +0200 Subject: [PATCH 22/99] Remove quaternion calculation because it does not work --- admittance_controller/src/admittance_rule.cpp | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 02d9acb6b1..7b828c3eee 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -30,17 +30,6 @@ namespace { // Utility namespace -void get_rpy_difference_between_two_quaternions(const geometry_msgs::msg::Quaternion quat1, - const geometry_msgs::msg::Quaternion quat2, std::array vector_out) -{ - tf2::Quaternion q1(quat1.x, quat1.y, quat1.z, quat1.w); - tf2::Quaternion q2(quat2.x, quat2.y, quat2.z, quat2.w); - - // q1 - q2 - const tf2::Quaternion q_diff = q1 * q2.inverse(); - tf2::Matrix3x3(q_diff).getRPY(vector_out[0], vector_out[1], vector_out[2]); -} - void convert_message_to_array(const geometry_msgs::msg::Pose & msg, std::array & vector_out) { vector_out[0] = msg.position.x; @@ -215,23 +204,18 @@ controller_interface::return_type AdmittanceRule::update( convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_arr_); convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); - // Use angle difference calculated with quaternions to avoid confusion with angles - // TODO(destogl): Is this really needed? This introduces tracking error, so not working as expected - get_rpy_difference_between_two_quaternions( current_pose_control_frame_.pose.orientation, target_pose_control_frame_.pose.orientation, angles_error_); - // If at least one measured force is nan set all to 0 if (std::find_if(measured_force_control_frame_arr_.begin(), measured_force_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_force_control_frame_arr_.end()) { measured_force_control_frame_arr_.fill(0.0); } - // Compute admittance control law: F = M*a + D*v + K*(x_d - x) + // Compute admittance control law: F = M*a + D*v + K*(x - x_d) for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { double pose_error = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; if (i >= 3) { pose_error = angles::normalize_angle(current_pose_control_frame_arr_[i]) - angles::normalize_angle(target_pose_control_frame_arr_[i]); -// pose_error = angles_error_[i-3]; } // TODO(destogl): check if velocity is measured from hardware From fae67c7e42b970248361feb79f7d9bb901f5e4f1 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Fri, 14 May 2021 22:01:45 +0200 Subject: [PATCH 23/99] Corrected admittance rule --- admittance_controller/src/admittance_rule.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 7b828c3eee..a9e9874755 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -209,7 +209,7 @@ controller_interface::return_type AdmittanceRule::update( measured_force_control_frame_arr_.fill(0.0); } - // Compute admittance control law: F = M*a + D*v + K*(x - x_d) + // Compute admittance control law: F = M*a + D*v + S*(x - x_d) for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { double pose_error = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; From d110623235744293bd0cd6d1cc0e7dba0c7a798d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 14 May 2021 13:25:17 -0500 Subject: [PATCH 24/99] Clean up unused vars and debug statements --- .../admittance_controller/incremental_kinematics.hpp | 2 -- admittance_controller/src/incremental_kinematics.cpp | 9 ++------- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index abfa63603f..64c666be25 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -62,8 +62,6 @@ class IncrementalKinematics // Pre-allocate for speed Eigen::MatrixXd jacobian_; - Eigen::JacobiSVD svd_; - Eigen::MatrixXd matrix_s_; Eigen::MatrixXd pseudo_inverse_; }; diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 6e83196163..dd5edd6683 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -62,7 +62,6 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorget_logger(), delta_x[0] << " " << delta_x[1] << " " << delta_x[2] << " " << delta_x[3]); } catch (const tf2::TransformException & ex) { @@ -72,15 +71,11 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorgetJacobian(joint_model_group_); - // TODO(andyz): the SVD method here is buggy. It should be more stable near singularities. - // svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); - // matrix_s_ = svd_.singularValues().asDiagonal(); - // pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); + // TODO(andyz): the SVD method would be more stable near singularities + // (or do what Olivier suggested: https://github.com/ros-controls/ros2_controllers/pull/173#discussion_r627936628) pseudo_inverse_ = jacobian_.transpose() * (jacobian_ * jacobian_.transpose()).inverse(); Eigen::VectorXd delta_theta = pseudo_inverse_ * delta_x; -// RCLCPP_ERROR_STREAM(node_->get_logger(), pseudo_inverse_.matrix()); - std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); delta_theta_vec = delta_theta_v; From 40fe6a4583e932f7029ad6bc5b7dd0939f461976 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Sat, 15 May 2021 20:20:28 +0200 Subject: [PATCH 25/99] Added test with hardware state has offset --- .../admittance_controller.hpp | 3 +++ .../admittance_controller/admittance_rule.hpp | 2 ++ .../src/admittance_controller.cpp | 18 +++++++++++++++++- admittance_controller/src/admittance_rule.cpp | 14 ++++++++++---- .../test/test_admittance_controller.cpp | 4 ++++ .../test/test_admittance_controller.hpp | 3 +++ 6 files changed, 39 insertions(+), 5 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 246d101ab4..08b3dd32bf 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -77,6 +77,9 @@ class AdmittanceController : public controller_interface::ControllerInterface std::string ft_sensor_name_; bool use_joint_commands_as_input_; + bool hardware_state_has_offset_; + trajectory_msgs::msg::JointTrajectoryPoint current_state_when_offset_; + // Internal variables std::unique_ptr force_torque_sensor_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 2285746616..12221dd0b1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -75,6 +75,8 @@ class AdmittanceRule controller_interface::return_type get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose); public: + bool hardware_state_has_offset_ = false; + // IK related parameters std::string ik_base_frame_; std::string ik_tip_frame_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 3864b766cd..789b429307 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -48,6 +48,7 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter>("state_interfaces", {}); get_node()->declare_parameter("ft_sensor_name", ""); get_node()->declare_parameter("use_joint_commands_as_input", false); + get_node()->declare_parameter("hardware_state_has_offset", false); get_node()->declare_parameter("IK.base", ""); get_node()->declare_parameter("IK.tip", ""); @@ -150,6 +151,7 @@ CallbackReturn AdmittanceController::on_configure( get_string_array_param_and_error_if_empty(state_interface_types_, "state_interfaces") || get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || get_bool_param_and_error_if_empty(use_joint_commands_as_input_, "use_joint_commands_as_input") || + get_bool_param_and_error_if_empty(admittance_->hardware_state_has_offset_, "hardware_state_has_offset") || get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || @@ -316,6 +318,9 @@ CallbackReturn AdmittanceController::on_configure( } } + current_state_when_offset_.positions.reserve(joint_names_.size()); + current_state_when_offset_.velocities.reserve(joint_names_.size()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -392,6 +397,10 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & admittance_->reset(); previous_time_ = get_node()->now(); + for (auto index = 0u; index < joint_names_.size(); ++index) { + current_state_when_offset_.positions[index] = joint_state_interface_[0][index].get().get_value(); + } + // Set initial command values - initialize all to simplify update std::shared_ptr msg_force = std::make_shared(); msg_force->header.frame_id = admittance_->control_frame_; @@ -451,8 +460,13 @@ controller_interface::return_type AdmittanceController::update() std::array current_joint_states; std::array desired_joint_states; + for (auto index = 0u; index < num_joints; ++index) { - current_joint_states[index] = joint_state_interface_[0][index].get().get_value(); + if (!admittance_->hardware_state_has_offset_) { + current_joint_states[index] = joint_state_interface_[0][index].get().get_value(); + } else { + current_joint_states[index] = current_state_when_offset_.positions[index]; + } } auto ft_values = force_torque_sensor_->get_values_as_message(); @@ -488,10 +502,12 @@ controller_interface::return_type AdmittanceController::update() for (auto index = 0u; index < num_joints; ++index) { if (has_position_command_interface_) { joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); + current_state_when_offset_.positions[index] = desired_joint_states[index]; } if (has_velocity_command_interface_) { joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); + current_state_when_offset_.velocities[index] = joint_command_interface_[1][index].get().get_value(); } } diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index a9e9874755..0ca226be4f 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -137,8 +137,6 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize variables used in the update loop measured_force_.header.frame_id = sensor_frame_; - current_pose_.header.frame_id = endeffector_frame_; - relative_desired_pose_.header.frame_id = control_frame_; relative_desired_joint_state_vec_.reserve(6); @@ -162,6 +160,8 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); + get_current_pose_of_endeffector_frame(current_pose_); + // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; try { @@ -196,8 +196,14 @@ controller_interface::return_type AdmittanceRule::update( measured_force_filtered_ = measured_force_; transform_message_to_control_frame(measured_force_filtered_, measured_force_control_frame_); - get_current_pose_of_endeffector_frame(current_pose_); - transform_message_to_control_frame(current_pose_, current_pose_control_frame_); + if (!hardware_state_has_offset_) { + get_current_pose_of_endeffector_frame(current_pose_); + transform_message_to_control_frame(current_pose_, current_pose_control_frame_); + } + // TODO(destogl): This can actually work properly if we consider the offset... +// else { +// current_pose_control_frame_ = desired_pose_; +// } // Convert all data to arrays for simpler calculation convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_arr_); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index b620cdfbff..f37fabf042 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -54,6 +54,10 @@ INSTANTIATE_TEST_CASE_P( // 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"), diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 3d04e7607a..8a4add8fb0 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -165,6 +165,7 @@ class AdmittanceControllerTest : public ::testing::Test 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_}); @@ -383,6 +384,8 @@ class AdmittanceControllerTest : public ::testing::Test 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"; From 450a24dfb664a217c1eb0d4fdba7ed4ea3731590 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Sat, 15 May 2021 20:37:06 +0200 Subject: [PATCH 26/99] Move calculation of admittance rule into separate method --- .../admittance_controller/admittance_rule.hpp | 10 +++ admittance_controller/src/admittance_rule.cpp | 61 ++++++++++++------- 2 files changed, 49 insertions(+), 22 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 12221dd0b1..42352a1ed9 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -72,6 +72,16 @@ class AdmittanceRule control_msgs::msg::AdmittanceControllerState & state_message ); + /** + * All values are in he controller frame + */ + controller_interface::return_type calculate_admittance_rule( + const std::array & measured_force, + const std::array & pose_error, + const rclcpp::Duration & period, + std::array & desired_relative_pose + ); + controller_interface::return_type get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose); public: diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 0ca226be4f..6b4fe57f10 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -200,7 +200,7 @@ controller_interface::return_type AdmittanceRule::update( get_current_pose_of_endeffector_frame(current_pose_); transform_message_to_control_frame(current_pose_, current_pose_control_frame_); } - // TODO(destogl): This can actually work properly if we consider the offset... + // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { // current_pose_control_frame_ = desired_pose_; // } @@ -215,31 +215,19 @@ controller_interface::return_type AdmittanceRule::update( measured_force_control_frame_arr_.fill(0.0); } - // Compute admittance control law: F = M*a + D*v + S*(x - x_d) - for (auto i = 0u; i < 6; ++i) { - if (selected_axes_[i]) { - double pose_error = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; - if (i >= 3) { - pose_error = angles::normalize_angle(current_pose_control_frame_arr_[i]) - - angles::normalize_angle(target_pose_control_frame_arr_[i]); - } - - // TODO(destogl): check if velocity is measured from hardware - const double acceleration = 1 / mass_[i] * - (measured_force_control_frame_arr_[i] - damping_[i] * desired_velocity_arr_[i] - - stiffness_[i] * pose_error); - - desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); + std::array pose_error_vec; - relative_desired_pose_arr_[i] = (desired_velocity_previous_arr_[i] + desired_velocity_arr_[i]) * 0.5 * period.seconds(); - - desired_acceleration_previous_arr_[i] = acceleration; - desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; - -// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); + for (auto i = 0u; i < 6; ++i) { + pose_error_vec[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; + if (i >= 3) { + pose_error_vec[i] = angles::normalize_angle(current_pose_control_frame_arr_[i]) - + angles::normalize_angle(target_pose_control_frame_arr_[i]); } } + calculate_admittance_rule(measured_force_control_frame_arr_, pose_error_vec, period, + relative_desired_pose_arr_); + // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); @@ -370,6 +358,35 @@ controller_interface::return_type AdmittanceRule::get_controller_state( return controller_interface::return_type::OK; } +controller_interface::return_type AdmittanceRule::calculate_admittance_rule( + const std::array & measured_force, + const std::array & pose_error, + const rclcpp::Duration & period, + std::array & desired_relative_pose +) +{ + // Compute admittance control law: F = M*a + D*v + S*(x - x_d) + for (auto i = 0u; i < 6; ++i) { + if (selected_axes_[i]) { + // TODO(destogl): check if velocity is measured from hardware + const double acceleration = 1 / mass_[i] * + (measured_force[i] - damping_[i] * desired_velocity_arr_[i] - + stiffness_[i] * pose_error[i]); + + desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); + + desired_relative_pose[i] = (desired_velocity_previous_arr_[i] + desired_velocity_arr_[i]) * 0.5 * period.seconds(); + + desired_acceleration_previous_arr_[i] = acceleration; + desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; + + // RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); + } + } + + return controller_interface::return_type::OK; +} + controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose) { // Get tool frame position - in the future use: IKSolver->getPositionFK(...) From 86acf227a1c6547071bd1aca6ff226460a2931e4 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Sat, 15 May 2021 21:39:33 +0200 Subject: [PATCH 27/99] Extract force processing and start using desired_joint_deltas --- .../admittance_controller/admittance_rule.hpp | 24 +++--- .../src/admittance_controller.cpp | 20 ++--- admittance_controller/src/admittance_rule.cpp | 74 +++++++++++-------- .../src/incremental_kinematics.cpp | 6 +- 4 files changed, 70 insertions(+), 54 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 42352a1ed9..1c45e4169e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -72,16 +72,6 @@ class AdmittanceRule control_msgs::msg::AdmittanceControllerState & state_message ); - /** - * All values are in he controller frame - */ - controller_interface::return_type calculate_admittance_rule( - const std::array & measured_force, - const std::array & pose_error, - const rclcpp::Duration & period, - std::array & desired_relative_pose - ); - controller_interface::return_type get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose); public: @@ -107,6 +97,20 @@ class AdmittanceRule std::array stiffness_; protected: + void process_force_measurements( + const geometry_msgs::msg::Wrench & measured_forces + ); + + /** + * All values are in he controller frame + */ + void calculate_admittance_rule( + const std::array & measured_force, + const std::array & pose_error, + const rclcpp::Duration & period, + std::array & desired_relative_pose + ); + // IK variables std::shared_ptr ik_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 789b429307..5aeb9b9afe 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -458,7 +458,7 @@ controller_interface::return_type AdmittanceController::update() // Position has to always there auto num_joints = joint_state_interface_[0].size(); std::array current_joint_states; - std::array desired_joint_states; + std::array desired_joint_deltas; for (auto index = 0u; index < num_joints; ++index) { @@ -475,8 +475,10 @@ controller_interface::return_type AdmittanceController::update() // TODO(destogl): Enable this when unified mode is used // if (admittance_->unified_mode_) { - // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_force_cmd, duration_since_last_call, desired_joint_states); + // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_force_cmd, duration_since_last_call, desired_joint_deltas); // } else { + + // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) if (use_joint_commands_as_input_) { std::array joint_deltas; // If there are no positions, expect velocities @@ -491,9 +493,9 @@ controller_interface::return_type AdmittanceController::update() joint_deltas[index] = angles::shortest_angular_distance(current_joint_states[index], (*input_joint_cmd)->points[0].positions[index]); } } - admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); + admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_deltas); } else { - admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_states); + admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_deltas); } // } previous_time_ = get_node()->now(); @@ -501,12 +503,12 @@ controller_interface::return_type AdmittanceController::update() // Write new joint angles to the robot for (auto index = 0u; index < num_joints; ++index) { if (has_position_command_interface_) { - joint_command_interface_[0][index].get().set_value(desired_joint_states[index]); - current_state_when_offset_.positions[index] = desired_joint_states[index]; + joint_command_interface_[0][index].get().set_value(desired_joint_deltas[index]); + current_state_when_offset_.positions[index] = desired_joint_deltas[index]; } if (has_velocity_command_interface_) { joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( - current_joint_states[index], desired_joint_states[index]) / duration_since_last_call.seconds()); + current_joint_states[index], desired_joint_deltas[index]) / duration_since_last_call.seconds()); current_state_when_offset_.velocities[index] = joint_command_interface_[1][index].get().get_value(); } } @@ -517,11 +519,11 @@ controller_interface::return_type AdmittanceController::update() state_publisher_->msg_.input_pose_command = **input_pose_cmd; state_publisher_->msg_.input_joint_command = **input_joint_cmd; - state_publisher_->msg_.desired_joint_states.positions.assign(desired_joint_states.begin(), desired_joint_states.end()); + state_publisher_->msg_.desired_joint_states.positions.assign(desired_joint_deltas.begin(), desired_joint_deltas.end()); state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); for (auto index = 0u; index < num_joints; ++index) { state_publisher_->msg_.error_joint_state.positions[index] = angles::shortest_angular_distance( - current_joint_states[index], desired_joint_states[index]); + current_joint_states[index], desired_joint_deltas[index]); } admittance_->get_controller_state(state_publisher_->msg_); state_publisher_->unlockAndPublish(); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 6b4fe57f10..e2fad8948a 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -187,15 +187,11 @@ controller_interface::return_type AdmittanceRule::update( std::array & desired_joint_states ) { + process_force_measurements(measured_force); + // Convert inputs to control frame transform_message_to_control_frame(target_pose, target_pose_control_frame_); - // get current states, and transform those into controller frame - measured_force_.wrench = measured_force; - // TODO(destogl): Add gravity compensation of measured forces - measured_force_filtered_ = measured_force_; - transform_message_to_control_frame(measured_force_filtered_, measured_force_control_frame_); - if (!hardware_state_has_offset_) { get_current_pose_of_endeffector_frame(current_pose_); transform_message_to_control_frame(current_pose_, current_pose_control_frame_); @@ -206,15 +202,9 @@ controller_interface::return_type AdmittanceRule::update( // } // Convert all data to arrays for simpler calculation - convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_arr_); convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_arr_); convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); - // If at least one measured force is nan set all to 0 - if (std::find_if(measured_force_control_frame_arr_.begin(), measured_force_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_force_control_frame_arr_.end()) { - measured_force_control_frame_arr_.fill(0.0); - } - std::array pose_error_vec; for (auto i = 0u; i < 6; ++i) { @@ -233,7 +223,6 @@ controller_interface::return_type AdmittanceRule::update( tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); - // Use Jacobian-based IK // Calculate desired Cartesian displacement of the robot // TODO: replace this with FK in the long term geometry_msgs::msg::TransformStamped transform; @@ -245,6 +234,7 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } + // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ @@ -358,7 +348,43 @@ controller_interface::return_type AdmittanceRule::get_controller_state( return controller_interface::return_type::OK; } -controller_interface::return_type AdmittanceRule::calculate_admittance_rule( +controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose) +{ + // Get tool frame position - in the future use: IKSolver->getPositionFK(...) + static geometry_msgs::msg::PoseStamped origin; + origin.header.frame_id = endeffector_frame_; + origin.pose.orientation.w = 1; + + try { + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); + tf2::doTransform(origin, pose, transform); + } catch (const tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + endeffector_frame_ + "'."); + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; +} + +void AdmittanceRule::process_force_measurements( + const geometry_msgs::msg::Wrench & measured_forces +) +{ + // get current states, and transform those into controller frame + measured_force_.wrench = measured_forces; + // TODO(destogl): Add gravity compensation of measured forces + measured_force_filtered_ = measured_force_; + transform_message_to_control_frame(measured_force_filtered_, measured_force_control_frame_); + + convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_arr_); + + // If at least one measured force is nan set all to 0 + if (std::find_if(measured_force_control_frame_arr_.begin(), measured_force_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_force_control_frame_arr_.end()) { + measured_force_control_frame_arr_.fill(0.0); + } +} + +void AdmittanceRule::calculate_admittance_rule( const std::array & measured_force, const std::array & pose_error, const rclcpp::Duration & period, @@ -383,26 +409,10 @@ controller_interface::return_type AdmittanceRule::calculate_admittance_rule( // RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); } } - - return controller_interface::return_type::OK; } -controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose) -{ - // Get tool frame position - in the future use: IKSolver->getPositionFK(...) - static geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = endeffector_frame_; - origin.pose.orientation.w = 1; - try { - auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); - tf2::doTransform(origin, pose, transform); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + endeffector_frame_ + "'."); - return controller_interface::return_type::ERROR; - } - return controller_interface::return_type::OK; -} + + } // namespace admittance_controller diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index dd5edd6683..267f8cd358 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -26,7 +26,7 @@ IncrementalKinematics::IncrementalKinematics(const std::shared_ptr std::unique_ptr model_loader_ptr = std::unique_ptr(new robot_model_loader::RobotModelLoader(node_, "robot_description", false /* do not load kinematics plugins */)); const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe? + // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe? (Denis: there should not be multi-threading here) joint_model_group_ = kinematic_model->getJointModelGroup(group_name); kinematic_state_ = std::make_shared(kinematic_model); @@ -65,7 +65,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorget_logger(), "Transformation of wrench failed."); + RCLCPP_ERROR(node_->get_logger(), "Transformation of twist failed."); return false; } @@ -119,7 +119,7 @@ bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(std::vectorget_logger(), "Transformation of wrench failed."); + RCLCPP_ERROR(node_->get_logger(), "Transformation of twist failed."); return false; } From 068c6e8482fdf3aaa4e15ca796d0ec711834a123 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Sat, 15 May 2021 22:55:30 +0200 Subject: [PATCH 28/99] Use JointTrajectoryPoint as internal data structure --- .../admittance_controller/admittance_rule.hpp | 6 ++--- .../src/admittance_controller.cpp | 22 ++++++++-------- admittance_controller/src/admittance_rule.cpp | 25 +++++++++++-------- 3 files changed, 28 insertions(+), 25 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 1c45e4169e..d0bf1a806a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -43,14 +43,14 @@ class AdmittanceRule const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, - std::array & desired_joint_states); + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); controller_interface::return_type update( const std::array & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const std::array & target_joint_deltas, const rclcpp::Duration & period, - std::array & desired_joint_states); + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); controller_interface::return_type update( const std::array & current_joint_state, @@ -58,7 +58,7 @@ class AdmittanceRule const geometry_msgs::msg::PoseStamped & target_pose, const geometry_msgs::msg::WrenchStamped & target_force, const rclcpp::Duration & period, - std::array & desired_joint_states); + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); // controller_interface::return_type update( // const geometry_msgs::msg::WrenchStamped & measured_force, diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5aeb9b9afe..cf46b6b7b8 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -458,7 +458,9 @@ controller_interface::return_type AdmittanceController::update() // Position has to always there auto num_joints = joint_state_interface_[0].size(); std::array current_joint_states; - std::array desired_joint_deltas; + trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states; + desired_joint_states.positions.resize(num_joints); + desired_joint_states.velocities.resize(num_joints); for (auto index = 0u; index < num_joints; ++index) { @@ -475,7 +477,7 @@ controller_interface::return_type AdmittanceController::update() // TODO(destogl): Enable this when unified mode is used // if (admittance_->unified_mode_) { - // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_force_cmd, duration_since_last_call, desired_joint_deltas); + // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_force_cmd, duration_since_last_call, desired_joint_states); // } else { // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) @@ -493,9 +495,9 @@ controller_interface::return_type AdmittanceController::update() joint_deltas[index] = angles::shortest_angular_distance(current_joint_states[index], (*input_joint_cmd)->points[0].positions[index]); } } - admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_deltas); + admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); } else { - admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_deltas); + admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_states); } // } previous_time_ = get_node()->now(); @@ -503,15 +505,13 @@ controller_interface::return_type AdmittanceController::update() // Write new joint angles to the robot for (auto index = 0u; index < num_joints; ++index) { if (has_position_command_interface_) { - joint_command_interface_[0][index].get().set_value(desired_joint_deltas[index]); - current_state_when_offset_.positions[index] = desired_joint_deltas[index]; + joint_command_interface_[0][index].get().set_value(desired_joint_states.positions[index]); } if (has_velocity_command_interface_) { - joint_command_interface_[1][index].get().set_value(angles::shortest_angular_distance( - current_joint_states[index], desired_joint_deltas[index]) / duration_since_last_call.seconds()); - current_state_when_offset_.velocities[index] = joint_command_interface_[1][index].get().get_value(); + joint_command_interface_[1][index].get().set_value(desired_joint_states.velocities[index]); } } + current_state_when_offset_ = desired_joint_states; // Publish controller state state_publisher_->lock(); @@ -519,11 +519,11 @@ controller_interface::return_type AdmittanceController::update() state_publisher_->msg_.input_pose_command = **input_pose_cmd; state_publisher_->msg_.input_joint_command = **input_joint_cmd; - state_publisher_->msg_.desired_joint_states.positions.assign(desired_joint_deltas.begin(), desired_joint_deltas.end()); + state_publisher_->msg_.desired_joint_states = desired_joint_states; state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); for (auto index = 0u; index < num_joints; ++index) { state_publisher_->msg_.error_joint_state.positions[index] = angles::shortest_angular_distance( - current_joint_states[index], desired_joint_deltas[index]); + current_joint_states[index], desired_joint_states.positions[index]); } admittance_->get_controller_state(state_publisher_->msg_); state_publisher_->unlockAndPublish(); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index e2fad8948a..5e833efddb 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -180,11 +180,11 @@ controller_interface::return_type AdmittanceRule::reset() // Update with target Cartesian pose - the main update method! controller_interface::return_type AdmittanceRule::update( - const std::array & current_joint_states, + const std::array & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, - std::array & desired_joint_states + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { process_force_measurements(measured_force); @@ -238,13 +238,15 @@ controller_interface::return_type AdmittanceRule::update( std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ - for (auto i = 0u; i < desired_joint_states.size(); ++i) { - desired_joint_states[i] = current_joint_states[i] + relative_desired_joint_state_vec_[i]; -// RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_states[i], relative_desired_joint_state_vec_[i], desired_joint_states[i]); + for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { + desired_joint_state.positions[i] = current_joint_state[i] + relative_desired_joint_state_vec_[i]; + desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); +// RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); } } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); - desired_joint_states = current_joint_states; + desired_joint_state.positions.assign(current_joint_state.begin(), current_joint_state.end()); + std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); return controller_interface::return_type::ERROR; } @@ -263,7 +265,7 @@ controller_interface::return_type AdmittanceRule::update( const geometry_msgs::msg::Wrench & measured_force, const std::array & target_joint_deltas, const rclcpp::Duration & period, - std::array & desired_joint_states) + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); @@ -280,7 +282,8 @@ controller_interface::return_type AdmittanceRule::update( if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); - desired_joint_states = current_joint_state; + desired_joint_state.positions.assign(current_joint_state.begin(), current_joint_state.end()); + std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); return controller_interface::return_type::ERROR; } @@ -304,16 +307,16 @@ controller_interface::return_type AdmittanceRule::update( transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_pose.pose); target_pose.header = target_ik_tip_pose.header; - return update(current_joint_state, measured_force, target_pose, period, desired_joint_states); + return update(current_joint_state, measured_force, target_pose, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( - const std::array & /*current_joint_states*/, + const std::array & /*current_joint_state*/, const geometry_msgs::msg::Wrench & /*measured_force*/, const geometry_msgs::msg::PoseStamped & /*target_pose*/, const geometry_msgs::msg::WrenchStamped & /*target_force*/, const rclcpp::Duration & /*period*/, - std::array & /*desired_joint_states*/ + trajectory_msgs::msg::JointTrajectoryPoint & /*desired_joint_state*/ ) { // TODO(destogl): Implement this update From 2b593e5a56074c4eb5361cccc7f0eda541392b02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 17 May 2021 21:56:37 +0200 Subject: [PATCH 29/99] Use JointTrajectoryPoint internally --- .../admittance_controller/admittance_rule.hpp | 46 +++-- .../src/admittance_controller.cpp | 13 +- admittance_controller/src/admittance_rule.cpp | 188 +++++++++++------- 3 files changed, 143 insertions(+), 104 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index d0bf1a806a..e0c67df431 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -39,34 +39,29 @@ class AdmittanceRule controller_interface::return_type reset(); controller_interface::return_type update( - const std::array & /*current_joint_state*/, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states + ); controller_interface::return_type update( - const std::array & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const std::array & target_joint_deltas, const rclcpp::Duration & period, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states + ); controller_interface::return_type update( - const std::array & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const geometry_msgs::msg::WrenchStamped & target_force, const rclcpp::Duration & period, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); - -// controller_interface::return_type update( -// const geometry_msgs::msg::WrenchStamped & measured_force, -// const geometry_msgs::msg::PoseStamped & target_pose, -// const geometry_msgs::msg::PoseStamped & current_pose, -// const rclcpp::Duration & period, -// geometry_msgs::msg::TransformStamped & relative_desired_pose_vec -// ); + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states + ); controller_interface::return_type get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message @@ -111,6 +106,12 @@ class AdmittanceRule std::array & desired_relative_pose ); + controller_interface::return_type calculate_desired_joint_state( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state + ); + // IK variables std::shared_ptr ik_; @@ -172,25 +173,28 @@ class AdmittanceRule return controller_interface::return_type::OK; } + template void - direct_transform(const geometry_msgs::msg::Pose & input_pose, const tf2::Transform & transform, geometry_msgs::msg::Pose & output_pose) + direct_transform(const Type & input, const tf2::Transform & transform, Type & output) { // use TF2 data types for easy math - tf2::Transform input_pose_tf, output_pose_tf; + tf2::Transform input_tf, output_tf; - tf2::fromMsg(input_pose, input_pose_tf); - output_pose_tf = input_pose_tf * transform; - tf2::toMsg(output_pose_tf, output_pose); + tf2::fromMsg(input, input_tf); + output_tf = input_tf * transform; + tf2::toMsg(output_tf, output); } + template void - transform_ik_tip_to_endeffector_frame(const geometry_msgs::msg::Pose & base_to_ik_tip, geometry_msgs::msg::Pose & base_to_toollink) + transform_ik_tip_to_endeffector_frame(const Type & base_to_ik_tip, Type & base_to_toollink) { direct_transform(base_to_ik_tip, ik_tip_to_endeffector_frame_tf_, base_to_toollink); } + template void - transform_endeffector_to_ik_tip_frame(const geometry_msgs::msg::Pose & base_to_toollink, geometry_msgs::msg::Pose & base_to_ik_tip) + transform_endeffector_to_ik_tip_frame(const Type & base_to_toollink, Type & base_to_ik_tip) { direct_transform(base_to_toollink, endeffector_frame_to_ik_tip_tf_, base_to_ik_tip); } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index cf46b6b7b8..d3f986a46d 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -457,7 +457,8 @@ controller_interface::return_type AdmittanceController::update() // Position has to always there auto num_joints = joint_state_interface_[0].size(); - std::array current_joint_states; + trajectory_msgs::msg::JointTrajectoryPoint current_joint_states; + current_joint_states.positions.resize(num_joints, 0.0); trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states; desired_joint_states.positions.resize(num_joints); desired_joint_states.velocities.resize(num_joints); @@ -465,9 +466,9 @@ controller_interface::return_type AdmittanceController::update() for (auto index = 0u; index < num_joints; ++index) { if (!admittance_->hardware_state_has_offset_) { - current_joint_states[index] = joint_state_interface_[0][index].get().get_value(); + current_joint_states.positions[index] = joint_state_interface_[0][index].get().get_value(); } else { - current_joint_states[index] = current_state_when_offset_.positions[index]; + current_joint_states.positions[index] = current_state_when_offset_.positions[index]; } } @@ -492,7 +493,7 @@ controller_interface::return_type AdmittanceController::update() } else { for (auto index = 0u; index < num_joints; ++index) { // TODO(anyone): Is here OK to use shortest_angular_distance? - joint_deltas[index] = angles::shortest_angular_distance(current_joint_states[index], (*input_joint_cmd)->points[0].positions[index]); + joint_deltas[index] = angles::shortest_angular_distance(current_joint_states.positions[index], (*input_joint_cmd)->points[0].positions[index]); } } admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); @@ -520,10 +521,10 @@ controller_interface::return_type AdmittanceController::update() state_publisher_->msg_.input_joint_command = **input_joint_cmd; state_publisher_->msg_.desired_joint_states = desired_joint_states; - state_publisher_->msg_.actual_joint_states.positions.assign(current_joint_states.begin(), current_joint_states.end()); + state_publisher_->msg_.actual_joint_states = current_joint_states; for (auto index = 0u; index < num_joints; ++index) { state_publisher_->msg_.error_joint_state.positions[index] = angles::shortest_angular_distance( - current_joint_states[index], desired_joint_states.positions[index]); + current_joint_states.positions[index], desired_joint_states.positions[index]); } admittance_->get_controller_state(state_publisher_->msg_); state_publisher_->unlockAndPublish(); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 5e833efddb..36ee82c508 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -30,7 +30,8 @@ namespace { // Utility namespace -void convert_message_to_array(const geometry_msgs::msg::Pose & msg, std::array & vector_out) +template +void convert_message_to_array(const geometry_msgs::msg::Pose & msg, Type & vector_out) { vector_out[0] = msg.position.x; vector_out[1] = msg.position.y; @@ -44,14 +45,37 @@ void convert_message_to_array(const geometry_msgs::msg::Pose & msg, std::array void convert_message_to_array( - const geometry_msgs::msg::PoseStamped & msg, std::array & vector_out) + const geometry_msgs::msg::PoseStamped & msg, Type & vector_out) { convert_message_to_array(msg.pose, vector_out); } +template +void convert_message_to_array(const geometry_msgs::msg::Transform & msg, Type & vector_out) +{ + vector_out[0] = msg.translation.x; + vector_out[1] = msg.translation.y; + vector_out[2] = msg.translation.z; + tf2::Quaternion q; + tf2::fromMsg(msg.rotation, q); + q.normalize(); + tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); + for (auto i = 3u; i < 6; ++i) { + vector_out[i] = angles::normalize_angle(vector_out[i]); + } +} + +template +void convert_message_to_array(const geometry_msgs::msg::TransformStamped & msg, Type & vector_out) +{ + convert_message_to_array(msg.transform, vector_out); +} + +template void convert_message_to_array( - const geometry_msgs::msg::Wrench & msg, std::array & vector_out) + const geometry_msgs::msg::Wrench & msg, Type & vector_out) { vector_out[0] = msg.force.x; vector_out[1] = msg.force.y; @@ -61,33 +85,37 @@ void convert_message_to_array( vector_out[5] = msg.torque.z; } +template void convert_message_to_array( - const geometry_msgs::msg::WrenchStamped & msg, std::array & vector_out) + const geometry_msgs::msg::WrenchStamped & msg, Type & vector_out) { convert_message_to_array(msg.wrench, vector_out); } -// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Pose & msg_out) -// { -// msg_out.position.x = vector[0]; -// msg_out.position.y = vector[1]; -// msg_out.position.z = vector[2]; -// -// tf2::Quaternion q; -// q.setRPY(vector[3], vector[4], vector[5]); -// -// msg_out.orientation.x = q.x(); -// msg_out.orientation.y = q.y(); -// msg_out.orientation.z = q.z(); -// msg_out.orientation.w = q.w(); -// } +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::Pose & msg_out) +{ + msg_out.position.x = vector[0]; + msg_out.position.y = vector[1]; + msg_out.position.z = vector[2]; -// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::PoseStamped & msg_out) -// { -// convert_array_to_message(vector, msg_out.pose); -// } + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.orientation.x = q.x(); + msg_out.orientation.y = q.y(); + msg_out.orientation.z = q.z(); + msg_out.orientation.w = q.w(); +} + +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::PoseStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.pose); +} -// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::Wrench & msg_out) +// template +// void convert_array_to_message(const Type & vector, geometry_msgs::msg::Wrench & msg_out) // { // msg_out.force.x = vector[0]; // msg_out.force.y = vector[1]; @@ -97,7 +125,8 @@ void convert_message_to_array( // msg_out.torque.z = vector[5]; // } -// void convert_array_to_message(const std::array & vector, geometry_msgs::msg::WrenchStamped & msg_out) +// template +// void convert_array_to_message(const Type & vector, geometry_msgs::msg::WrenchStamped & msg_out) // { // convert_array_to_message(vector, msg_out.wrench); // } @@ -180,15 +209,13 @@ controller_interface::return_type AdmittanceRule::reset() // Update with target Cartesian pose - the main update method! controller_interface::return_type AdmittanceRule::update( - const std::array & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - process_force_measurements(measured_force); - // Convert inputs to control frame transform_message_to_control_frame(target_pose, target_pose_control_frame_); @@ -215,53 +242,17 @@ controller_interface::return_type AdmittanceRule::update( } } + process_force_measurements(measured_force); + calculate_admittance_rule(measured_force_control_frame_arr_, pose_error_vec, period, relative_desired_pose_arr_); - // Do clean conversion to the goal pose using transform and not messing with Euler angles - convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); - transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); - - // Calculate desired Cartesian displacement of the robot - // TODO: replace this with FK in the long term - geometry_msgs::msg::TransformStamped transform; - try { - transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); - return controller_interface::return_type::ERROR; - } - - // Use Jacobian-based IK - std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); - if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ - for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { - desired_joint_state.positions[i] = current_joint_state[i] + relative_desired_joint_state_vec_[i]; - desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); -// RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); - } - } else { - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); - desired_joint_state.positions.assign(current_joint_state.begin(), current_joint_state.end()); - std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); - return controller_interface::return_type::ERROR; - } - - // TODO(anyone: enable this when enabling use of IK directly - // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); - // tf2::doTransform(desired_pose_, ik_input_pose_, transform); - // ik_input_pose_.pose = transform_endeffector_to_ik_tip_frame(ik_input_pose_); - // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... - - return controller_interface::return_type::OK; + return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } // Update from target joint deltas controller_interface::return_type AdmittanceRule::update( - const std::array & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_force, const std::array & target_joint_deltas, const rclcpp::Duration & period, @@ -282,7 +273,7 @@ controller_interface::return_type AdmittanceRule::update( if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); - desired_joint_state.positions.assign(current_joint_state.begin(), current_joint_state.end()); + desired_joint_state = current_joint_state; std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); return controller_interface::return_type::ERROR; } @@ -290,28 +281,28 @@ controller_interface::return_type AdmittanceRule::update( // TODO(destogl): Use as class variables to avoid memory allocation geometry_msgs::msg::PoseStamped current_ik_tip_pose; geometry_msgs::msg::TransformStamped current_to_target_ik_pose; + current_to_target_ik_pose.header.frame_id = ik_base_frame_; + current_to_target_ik_pose.child_frame_id = ik_base_frame_; geometry_msgs::msg::PoseStamped target_ik_tip_pose; - geometry_msgs::msg::PoseStamped target_pose; + geometry_msgs::msg::PoseStamped target_eff_pose; static geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = endeffector_frame_; + origin.header.frame_id = ik_tip_frame_; origin.pose.orientation.w = 1; // If FK this is not needed // TODO(anyone): Can we just use values from transformation instead calling doTransform? tf2::doTransform(origin, current_ik_tip_pose, transform_ik_base_tip); convert_array_to_message(target_ik_tip_deltas_vec, current_to_target_ik_pose); - current_to_target_ik_pose.header.frame_id = ik_base_frame_; - current_to_target_ik_pose.child_frame_id = ik_base_frame_; tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, current_to_target_ik_pose); - transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_pose.pose); - target_pose.header = target_ik_tip_pose.header; + transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_eff_pose.pose); + target_eff_pose.header = target_ik_tip_pose.header; - return update(current_joint_state, measured_force, target_pose, period, desired_joint_state); + return update(current_joint_state, measured_force, target_eff_pose, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( - const std::array & /*current_joint_state*/, + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_state*/, const geometry_msgs::msg::Wrench & /*measured_force*/, const geometry_msgs::msg::PoseStamped & /*target_pose*/, const geometry_msgs::msg::WrenchStamped & /*target_force*/, @@ -409,13 +400,56 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; - // RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); +// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); } } } +controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state +) +{ + // Do clean conversion to the goal pose using transform and not messing with Euler angles + convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); + tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); + transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); + + // Calculate desired Cartesian displacement of the robot + // TODO: replace this with FK in the long term + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + } catch (const tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); + return controller_interface::return_type::ERROR; + } + // Use Jacobian-based IK + std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); + if (ik_->convertCartesianDeltasToJointDeltas( + relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ + for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { + desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; + desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); + // RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state.positions[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); + desired_joint_state = current_joint_state; + std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); + return controller_interface::return_type::ERROR; + } + // TODO(anyone: enable this when enabling use of IK directly + // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); + // tf2::doTransform(desired_pose_, ik_input_pose_, transform); + // ik_input_pose_.pose = transform_endeffector_to_ik_tip_frame(ik_input_pose_); + // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... + return controller_interface::return_type::OK; +} } // namespace admittance_controller From 28d20512ffa29bdbf9cda4d1c708cf94e417f503 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 May 2021 01:17:28 +0200 Subject: [PATCH 30/99] Added gravity compensation --- .../admittance_controller/admittance_rule.hpp | 21 ++++++++++++ .../src/admittance_controller.cpp | 34 +++++++++++++++++-- admittance_controller/src/admittance_rule.cpp | 29 ++++++++++++++-- 3 files changed, 79 insertions(+), 5 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index e0c67df431..d3dc913bbd 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -24,11 +24,22 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" +// TODO(destogl): Enable use of filters +// #include "iirob_filters/gravity_compensation.h" #include "tf2_ros/transform_listener.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" namespace admittance_controller { +struct GravityCompensationParameters +{ +public: + std::string world_frame_; + std::string sensor_frame_; + geometry_msgs::msg::Vector3Stamped cog_; + double force_; +}; + class AdmittanceRule { public: @@ -91,6 +102,9 @@ class AdmittanceRule std::array damping_; std::array stiffness_; + // Filters + std::vector gravity_compensation_params_; + protected: void process_force_measurements( const geometry_msgs::msg::Wrench & measured_forces @@ -115,6 +129,13 @@ class AdmittanceRule // IK variables std::shared_ptr ik_; + // Filters +// using GravityCompensatorType = +// iirob_filters::GravityCompensator; +// +// std::unique_ptr wrist_gravity_compensator_; +// std::unique_ptr tool_gravity_compensator_; + // Transformation variables std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index d3f986a46d..5b2dc2fbdf 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -61,9 +61,6 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("fixed_world_frame", ""); get_node()->declare_parameter("sensor_frame", ""); - get_node()->declare_parameter>("gravity_compensation.masses", {}); - get_node()->declare_parameter>("gravity_compensation.center_of_masses", {}); - // TODO(destogl): enable when force/position control is implemented // get_node()->declare_parameter("admitance.unified_mode", false); get_node()->declare_parameter("admittance.selected_axes.x", false); @@ -94,6 +91,20 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("admittance.stiffness.ry", std::numeric_limits::quiet_NaN()); get_node()->declare_parameter("admittance.stiffness.rz", std::numeric_limits::quiet_NaN()); + // TODO(destogl): use filters here to make it more flexible + // declare and read Gravity compensation parameters + for (const auto & name : std::vector{"wrist", "tool"}) { + get_node()->declare_parameter("gravity_compensation." + name + ".CoG_x", + std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("gravity_compensation." + name + ".CoG_y", + std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("gravity_compensation." + name + ".CoG_z", + std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("gravity_compensation." + name + ".force", + std::numeric_limits::quiet_NaN()); + get_node()->declare_parameter("gravity_compensation." + name + ".frame_id", ""); + } + // get_node()->declare_parameter>("") } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); @@ -193,6 +204,23 @@ CallbackReturn AdmittanceController::on_configure( return CallbackReturn::ERROR; } + admittance_->gravity_compensation_params_.reserve(2); + for (const auto & name : std::vector{"wrist", "tool"}) { + GravityCompensationParameters params; + if ( + get_double_param_and_error_if_empty(params.cog_.vector.x, ("gravity_compensation." + name + ".CoG_x").c_str()) || + get_double_param_and_error_if_empty(params.cog_.vector.y, ("gravity_compensation." + name + ".CoG_y").c_str()) || + get_double_param_and_error_if_empty(params.cog_.vector.z, ("gravity_compensation." + name + ".CoG_z").c_str()) || + get_double_param_and_error_if_empty(params.force_, ("gravity_compensation." + name + ".force").c_str()) || + get_string_param_and_error_if_empty(params.cog_.header.frame_id, ("gravity_compensation." + name + ".frame_id").c_str()) + ) + { + return CallbackReturn::ERROR; + } + + admittance_->gravity_compensation_params_.emplace_back(params); + } + // 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()); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 36ee82c508..c31c831f40 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -366,8 +366,33 @@ void AdmittanceRule::process_force_measurements( { // get current states, and transform those into controller frame measured_force_.wrench = measured_forces; - // TODO(destogl): Add gravity compensation of measured forces - measured_force_filtered_ = measured_force_; + try { + auto transform = tf_buffer_->lookupTransform(fixed_world_frame_, measured_force_.header.frame_id, tf2::TimePointZero); + auto transform_back = tf_buffer_->lookupTransform(measured_force_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); + + geometry_msgs::msg::WrenchStamped measured_force_transformed; + tf2::doTransform(measured_force_, measured_force_transformed, transform); + + geometry_msgs::msg::Vector3Stamped cog_transformed; + + for (const auto & params : gravity_compensation_params_) { + auto transform_cog = tf_buffer_->lookupTransform(fixed_world_frame_, params.cog_.header.frame_id, tf2::TimePointZero); + tf2::doTransform(params.cog_, cog_transformed, transform_cog); + + measured_force_transformed.wrench.force.z += params.force_; + measured_force_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y); + measured_force_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); + } + + tf2::doTransform(measured_force_transformed, measured_force_filtered_, transform_back); + + } catch (const tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_force_.header.frame_id + "' or ''."); + // If transform error just use measured force + measured_force_filtered_ = measured_force_; + } + transform_message_to_control_frame(measured_force_filtered_, measured_force_control_frame_); convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_arr_); From 20fff9be4eb4b49825d28f06cbf5e9971cbcb57a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 May 2021 01:37:28 +0200 Subject: [PATCH 31/99] Add output of filtered forces --- admittance_controller/src/admittance_rule.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index c31c831f40..67a8f51b2b 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -323,11 +323,11 @@ controller_interface::return_type AdmittanceRule::get_controller_state( // state_message.input_force_control_frame = target_force_control_frame_; state_message.input_pose_control_frame = target_pose_control_frame_; state_message.measured_force = measured_force_; - state_message.measured_force = measured_force_filtered_; + state_message.measured_force_filtered = measured_force_filtered_; state_message.measured_force_control_frame = measured_force_control_frame_; try { - auto transform = tf_buffer_->lookupTransform(control_frame_, endeffector_frame_, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); tf2::doTransform(measured_force_control_frame_, measured_force_endeffector_frame_, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE From eff3834789f6af16cb76f25d310f7e2453786482 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 17 May 2021 17:00:54 -0500 Subject: [PATCH 32/99] Try calculating target_pose differently --- admittance_controller/src/admittance_rule.cpp | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 67a8f51b2b..4ecdbb5cd9 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -278,27 +278,26 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // TODO(destogl): Use as class variables to avoid memory allocation - geometry_msgs::msg::PoseStamped current_ik_tip_pose; - geometry_msgs::msg::TransformStamped current_to_target_ik_pose; - current_to_target_ik_pose.header.frame_id = ik_base_frame_; - current_to_target_ik_pose.child_frame_id = ik_base_frame_; - geometry_msgs::msg::PoseStamped target_ik_tip_pose; - geometry_msgs::msg::PoseStamped target_eff_pose; - static geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = ik_tip_frame_; - origin.pose.orientation.w = 1; - - // If FK this is not needed - // TODO(anyone): Can we just use values from transformation instead calling doTransform? - tf2::doTransform(origin, current_ik_tip_pose, transform_ik_base_tip); - convert_array_to_message(target_ik_tip_deltas_vec, current_to_target_ik_pose); - tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, current_to_target_ik_pose); - - transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_eff_pose.pose); - target_eff_pose.header = target_ik_tip_pose.header; - - return update(current_joint_state, measured_force, target_eff_pose, period, desired_joint_state); + // Get the target pose. This assumes Servo provides deltas in the "tip" frame + // TODO(andyz): check that Servo does provide deltas in the tip frame + // target_pose = current_pose + target_ik_tip_deltas_vec + geometry_msgs::msg::PoseStamped target_pose; + target_pose.pose.position.x = transform_ik_base_tip.transform.translation.x; + target_pose.pose.position.y = transform_ik_base_tip.transform.translation.y; + target_pose.pose.position.z = transform_ik_base_tip.transform.translation.z; + target_pose.pose.orientation = transform_ik_base_tip.transform.rotation; + target_pose.pose.position.x += target_ik_tip_deltas_vec.at(0); + target_pose.pose.position.y += target_ik_tip_deltas_vec.at(1); + target_pose.pose.position.z += target_ik_tip_deltas_vec.at(2); + + target_pose.pose.orientation.w = 1.0; + target_pose.pose.orientation.x = 0; + target_pose.pose.orientation.y = 0; + target_pose.pose.orientation.z = 0; + + target_pose.header.frame_id = ik_tip_frame_; + + return update(current_joint_state, measured_force, target_pose, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( From 50992e68588c161aaf696b1242fa4b63b3d210cd Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 17 May 2021 17:11:19 -0500 Subject: [PATCH 33/99] Small comment cleanup --- admittance_controller/src/admittance_rule.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 4ecdbb5cd9..dc4f0f7060 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -260,7 +260,7 @@ controller_interface::return_type AdmittanceRule::update( { std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); - // TODO: replace this with FK in the long term + geometry_msgs::msg::TransformStamped transform_ik_base_tip; try { transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); @@ -270,6 +270,7 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } + // Get cartesian deltas in the IK tip frame if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); @@ -282,9 +283,9 @@ controller_interface::return_type AdmittanceRule::update( // TODO(andyz): check that Servo does provide deltas in the tip frame // target_pose = current_pose + target_ik_tip_deltas_vec geometry_msgs::msg::PoseStamped target_pose; - target_pose.pose.position.x = transform_ik_base_tip.transform.translation.x; - target_pose.pose.position.y = transform_ik_base_tip.transform.translation.y; - target_pose.pose.position.z = transform_ik_base_tip.transform.translation.z; + target_pose.pose.position.x = 0; //transform_ik_base_tip.transform.translation.x; + target_pose.pose.position.y = 0; //transform_ik_base_tip.transform.translation.y; + target_pose.pose.position.z = 0; //transform_ik_base_tip.transform.translation.z; target_pose.pose.orientation = transform_ik_base_tip.transform.rotation; target_pose.pose.position.x += target_ik_tip_deltas_vec.at(0); target_pose.pose.position.y += target_ik_tip_deltas_vec.at(1); From f8e1b43732d3daabfbd78ae22cef4ce4b8d8f50c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 17 May 2021 17:26:16 -0500 Subject: [PATCH 34/99] Small bug fix to eliminate spurious rotations --- admittance_controller/src/admittance_rule.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index dc4f0f7060..2afaea3f35 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -217,7 +217,8 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to control frame - transform_message_to_control_frame(target_pose, target_pose_control_frame_); +// transform_message_to_control_frame(target_pose, target_pose_control_frame_); + target_pose_control_frame_ = target_pose; if (!hardware_state_has_offset_) { get_current_pose_of_endeffector_frame(current_pose_); From ad8816a004c75bb00f8691bf6de6d584c02b72f3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 17 May 2021 17:36:13 -0500 Subject: [PATCH 35/99] Reverse lookupTransform(). The fixes coordinate frame and rotation issues --- admittance_controller/src/admittance_rule.cpp | 53 +++++++++++++++---- 1 file changed, 44 insertions(+), 9 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 2afaea3f35..1ec3ad0670 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -217,8 +217,7 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to control frame -// transform_message_to_control_frame(target_pose, target_pose_control_frame_); - target_pose_control_frame_ = target_pose; + transform_message_to_control_frame(target_pose, target_pose_control_frame_); if (!hardware_state_has_offset_) { get_current_pose_of_endeffector_frame(current_pose_); @@ -248,7 +247,45 @@ controller_interface::return_type AdmittanceRule::update( calculate_admittance_rule(measured_force_control_frame_arr_, pose_error_vec, period, relative_desired_pose_arr_); - return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); + // Do clean conversion to the goal pose using transform and not messing with Euler angles + convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); + tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); + transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); + + // Calculate desired Cartesian displacement of the robot + // TODO: replace this with FK in the long term + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_->lookupTransform(ik_tip_frame_, ik_base_frame_, tf2::TimePointZero); + } catch (const tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); + return controller_interface::return_type::ERROR; + } + + // Use Jacobian-based IK + std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); + if (ik_->convertCartesianDeltasToJointDeltas( + relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ + for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { + desired_joint_state.positions[i] = current_joint_state[i] + relative_desired_joint_state_vec_[i]; + desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); +// RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); + desired_joint_state.positions.assign(current_joint_state.begin(), current_joint_state.end()); + std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); + return controller_interface::return_type::ERROR; + } + + // TODO(anyone: enable this when enabling use of IK directly + // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); + // tf2::doTransform(desired_pose_, ik_input_pose_, transform); + // ik_input_pose_.pose = transform_endeffector_to_ik_tip_frame(ik_input_pose_); + // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... + + return controller_interface::return_type::OK; } // Update from target joint deltas @@ -280,13 +317,11 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // Get the target pose. This assumes Servo provides deltas in the "tip" frame - // TODO(andyz): check that Servo does provide deltas in the tip frame - // target_pose = current_pose + target_ik_tip_deltas_vec + // Get the target pose geometry_msgs::msg::PoseStamped target_pose; - target_pose.pose.position.x = 0; //transform_ik_base_tip.transform.translation.x; - target_pose.pose.position.y = 0; //transform_ik_base_tip.transform.translation.y; - target_pose.pose.position.z = 0; //transform_ik_base_tip.transform.translation.z; + target_pose.pose.position.x = 0; + target_pose.pose.position.y = 0; + target_pose.pose.position.z = 0; target_pose.pose.orientation = transform_ik_base_tip.transform.rotation; target_pose.pose.position.x += target_ik_tip_deltas_vec.at(0); target_pose.pose.position.y += target_ik_tip_deltas_vec.at(1); From 1c704546bd3e2d77b6d29b3c1abb835f086bcf18 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 17 May 2021 18:43:44 -0500 Subject: [PATCH 36/99] Small fixup --- admittance_controller/src/admittance_rule.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 1ec3ad0670..84ddd81890 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -217,7 +217,9 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to control frame - transform_message_to_control_frame(target_pose, target_pose_control_frame_); + // TODO(andyz): this causes unexpected rotation + //transform_message_to_control_frame(target_pose, target_pose_control_frame_); + target_pose_control_frame_ = target_pose; if (!hardware_state_has_offset_) { get_current_pose_of_endeffector_frame(current_pose_); @@ -268,13 +270,13 @@ controller_interface::return_type AdmittanceRule::update( if (ik_->convertCartesianDeltasToJointDeltas( relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { - desired_joint_state.positions[i] = current_joint_state[i] + relative_desired_joint_state_vec_[i]; + desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); // RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); } } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); - desired_joint_state.positions.assign(current_joint_state.begin(), current_joint_state.end()); + desired_joint_state = current_joint_state; std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); return controller_interface::return_type::ERROR; } @@ -327,6 +329,7 @@ controller_interface::return_type AdmittanceRule::update( target_pose.pose.position.y += target_ik_tip_deltas_vec.at(1); target_pose.pose.position.z += target_ik_tip_deltas_vec.at(2); + // TODO(andyz): add orientation target_pose.pose.orientation.w = 1.0; target_pose.pose.orientation.x = 0; target_pose.pose.orientation.y = 0; From 0cbc4282aa9c7c62cac9671452e1b172a3445a3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 May 2021 02:12:08 +0200 Subject: [PATCH 37/99] Remove unnecessary dependencies --- admittance_controller/CMakeLists.txt | 3 --- .../include/admittance_controller/admittance_controller.hpp | 2 -- admittance_controller/package.xml | 1 - 3 files changed, 6 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index b2b7061501..3c31726593 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(controller_interface REQUIRED) find_package(Eigen3 REQUIRED) find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) -find_package(iirob_filters REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -49,7 +48,6 @@ ament_target_dependencies( ${Eigen_LIBRARIES} geometry_msgs hardware_interface - iirob_filters moveit_ros_move_group moveit_ros_planning_interface pluginlib @@ -115,7 +113,6 @@ ament_export_dependencies( controller_interface geometry_msgs hardware_interface - iirob_filters pluginlib rclcpp rclcpp_lifecycle diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 08b3dd32bf..a279225a47 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -29,8 +29,6 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "iirob_filters/low_pass_filter.h" -#include "iirob_filters/gravity_compensation.h" #include "semantic_components/force_torque_sensor.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 456296e972..0d6964496f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -13,7 +13,6 @@ controller_interface geometry_msgs hardware_interface - iirob_filters moveit_ros_move_group moveit_ros_planning_interface pluginlib From 055e7cf37784ab8e9db43a736b87f9d13e4a50c7 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 18 May 2021 09:48:04 -0500 Subject: [PATCH 38/99] Add orientation to admittance control. Small simplifications. --- admittance_controller/src/admittance_rule.cpp | 22 ++++++++----------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 84ddd81890..d5bcadf77e 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -321,20 +321,16 @@ controller_interface::return_type AdmittanceRule::update( // Get the target pose geometry_msgs::msg::PoseStamped target_pose; - target_pose.pose.position.x = 0; - target_pose.pose.position.y = 0; - target_pose.pose.position.z = 0; - target_pose.pose.orientation = transform_ik_base_tip.transform.rotation; - target_pose.pose.position.x += target_ik_tip_deltas_vec.at(0); - target_pose.pose.position.y += target_ik_tip_deltas_vec.at(1); - target_pose.pose.position.z += target_ik_tip_deltas_vec.at(2); - - // TODO(andyz): add orientation - target_pose.pose.orientation.w = 1.0; - target_pose.pose.orientation.x = 0; - target_pose.pose.orientation.y = 0; - target_pose.pose.orientation.z = 0; + target_pose.pose.position.x = target_ik_tip_deltas_vec.at(0); + target_pose.pose.position.y = target_ik_tip_deltas_vec.at(1); + target_pose.pose.position.z = target_ik_tip_deltas_vec.at(2); + tf2::Quaternion q; + q.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); + target_pose.pose.orientation.w = q.w(); + target_pose.pose.orientation.x = q.x(); + target_pose.pose.orientation.y = q.y(); + target_pose.pose.orientation.z = q.z(); target_pose.header.frame_id = ik_tip_frame_; return update(current_joint_state, measured_force, target_pose, period, desired_joint_state); From dbb76a75f4c6023d94f7e6697835c33ff6948007 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 18 May 2021 23:23:45 +0200 Subject: [PATCH 39/99] Remove small number noise from substraction --- admittance_controller/src/admittance_rule.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index d5bcadf77e..fa6ee2df5e 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -242,6 +242,11 @@ controller_interface::return_type AdmittanceRule::update( pose_error_vec[i] = angles::normalize_angle(current_pose_control_frame_arr_[i]) - angles::normalize_angle(target_pose_control_frame_arr_[i]); } + if (pose_error_vec[i] < 1e-10) { + pose_error_vec[i] = 0; + } +// RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), +// "Pose error [%zu]: %e", pose_error_vec[i]); } process_force_measurements(measured_force); @@ -321,6 +326,7 @@ controller_interface::return_type AdmittanceRule::update( // Get the target pose geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = ik_tip_frame_; target_pose.pose.position.x = target_ik_tip_deltas_vec.at(0); target_pose.pose.position.y = target_ik_tip_deltas_vec.at(1); target_pose.pose.position.z = target_ik_tip_deltas_vec.at(2); @@ -331,7 +337,13 @@ controller_interface::return_type AdmittanceRule::update( target_pose.pose.orientation.x = q.x(); target_pose.pose.orientation.y = q.y(); target_pose.pose.orientation.z = q.z(); - target_pose.header.frame_id = ik_tip_frame_; + +// RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), +// "IK-tip deltas: x: %e, y: %e, z: %e, rx: %e, ry: %e, rz: %e", +// target_ik_tip_deltas_vec.at(0), target_ik_tip_deltas_vec.at(1), +// target_ik_tip_deltas_vec.at(2), target_ik_tip_deltas_vec.at(3), +// target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5) +// ); return update(current_joint_state, measured_force, target_pose, period, desired_joint_state); } @@ -460,7 +472,7 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; -// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_force_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); + RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%e - D*%e - S*%e = %e)", i, measured_force[i], desired_velocity_arr_[i], pose_error , acceleration); } } } From 39f5a1377e8a1122907637ffb2aeadcf95a81fcc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 19 May 2021 17:30:02 +0200 Subject: [PATCH 40/99] Remove small number noise from substraction --- admittance_controller/src/admittance_rule.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index fa6ee2df5e..d19818f377 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -234,24 +234,31 @@ controller_interface::return_type AdmittanceRule::update( convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_arr_); convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); - std::array pose_error_vec; + std::array pose_error; for (auto i = 0u; i < 6; ++i) { - pose_error_vec[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; + pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; if (i >= 3) { - pose_error_vec[i] = angles::normalize_angle(current_pose_control_frame_arr_[i]) - + pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i]) - angles::normalize_angle(target_pose_control_frame_arr_[i]); } +<<<<<<< HEAD if (pose_error_vec[i] < 1e-10) { pose_error_vec[i] = 0; } // RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), // "Pose error [%zu]: %e", pose_error_vec[i]); +======= + // remove small noise due transformations + if (pose_error[i] < 1e-10) { + pose_error[i] = 0; + } +>>>>>>> 50e5553... Remove small number noise from substraction } process_force_measurements(measured_force); - calculate_admittance_rule(measured_force_control_frame_arr_, pose_error_vec, period, + calculate_admittance_rule(measured_force_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); // Do clean conversion to the goal pose using transform and not messing with Euler angles From 704063edb2a936f5ece24c281cebabd543cc80a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 19 May 2021 23:38:54 +0200 Subject: [PATCH 41/99] REnaming of variables --- .../src/admittance_controller.cpp | 5 +++ admittance_controller/src/admittance_rule.cpp | 34 ++++++++++--------- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5b2dc2fbdf..b5ae2a2ca0 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -349,6 +349,11 @@ CallbackReturn AdmittanceController::on_configure( current_state_when_offset_.positions.reserve(joint_names_.size()); current_state_when_offset_.velocities.reserve(joint_names_.size()); + if (use_joint_commands_as_input_) { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Using Joint input mode."); + } else { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Using Cartesian input mode."); + } RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index d19818f377..317594f92f 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -221,10 +221,10 @@ controller_interface::return_type AdmittanceRule::update( //transform_message_to_control_frame(target_pose, target_pose_control_frame_); target_pose_control_frame_ = target_pose; - if (!hardware_state_has_offset_) { +// if (!hardware_state_has_offset_) { get_current_pose_of_endeffector_frame(current_pose_); transform_message_to_control_frame(current_pose_, current_pose_control_frame_); - } +// } // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { // current_pose_control_frame_ = desired_pose_; @@ -242,18 +242,10 @@ controller_interface::return_type AdmittanceRule::update( pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i]) - angles::normalize_angle(target_pose_control_frame_arr_[i]); } -<<<<<<< HEAD - if (pose_error_vec[i] < 1e-10) { - pose_error_vec[i] = 0; - } -// RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), -// "Pose error [%zu]: %e", pose_error_vec[i]); -======= // remove small noise due transformations if (pose_error[i] < 1e-10) { pose_error[i] = 0; } ->>>>>>> 50e5553... Remove small number noise from substraction } process_force_measurements(measured_force); @@ -331,12 +323,22 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // Get the target pose - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = ik_tip_frame_; - target_pose.pose.position.x = target_ik_tip_deltas_vec.at(0); - target_pose.pose.position.y = target_ik_tip_deltas_vec.at(1); - target_pose.pose.position.z = target_ik_tip_deltas_vec.at(2); + // TODO(destogl): Use as class variables to avoid memory allocation + geometry_msgs::msg::PoseStamped current_ik_tip_pose; + geometry_msgs::msg::TransformStamped target_ik_tip_deltas_pose; + target_ik_tip_deltas_pose.header.frame_id = ik_base_frame_; + target_ik_tip_deltas_pose.child_frame_id = ik_base_frame_; + geometry_msgs::msg::PoseStamped target_ik_tip_pose; + geometry_msgs::msg::PoseStamped target_eff_pose; + static geometry_msgs::msg::PoseStamped origin; + origin.header.frame_id = ik_tip_frame_; + origin.pose.orientation.w = 1; + + // If FK this is not needed + // TODO(anyone): Can we just use values from transformation instead calling doTransform? + tf2::doTransform(origin, current_ik_tip_pose, transform_ik_base_tip); + convert_array_to_message(target_ik_tip_deltas_vec, target_ik_tip_deltas_pose); + tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, target_ik_tip_deltas_pose); tf2::Quaternion q; q.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); From f53c0b514782b0faf042aa894903a8ba7b72b958 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 19 May 2021 15:12:54 -0500 Subject: [PATCH 42/99] Rename force->wrench to be technically correct --- .../admittance_controller/admittance_rule.hpp | 20 ++--- .../src/admittance_controller.cpp | 6 +- admittance_controller/src/admittance_rule.cpp | 83 +++++++++---------- .../test/test_admittance_controller.cpp | 66 +++++++-------- 4 files changed, 83 insertions(+), 92 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index d3dc913bbd..b04de827d2 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -51,7 +51,7 @@ class AdmittanceRule controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::Wrench & measured_wrench, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states @@ -59,7 +59,7 @@ class AdmittanceRule controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::Wrench & measured_wrench, const std::array & target_joint_deltas, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states @@ -67,7 +67,7 @@ class AdmittanceRule controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::Wrench & measured_wrench, const geometry_msgs::msg::PoseStamped & target_pose, const geometry_msgs::msg::WrenchStamped & target_force, const rclcpp::Duration & period, @@ -107,14 +107,14 @@ class AdmittanceRule protected: void process_force_measurements( - const geometry_msgs::msg::Wrench & measured_forces + const geometry_msgs::msg::Wrench & measured_wrench ); /** * All values are in he controller frame */ void calculate_admittance_rule( - const std::array & measured_force, + const std::array & measured_wrench, const std::array & pose_error, const rclcpp::Duration & period, std::array & desired_relative_pose @@ -143,10 +143,10 @@ class AdmittanceRule tf2::Transform ik_tip_to_endeffector_frame_tf_; tf2::Transform endeffector_frame_to_ik_tip_tf_; - geometry_msgs::msg::WrenchStamped measured_force_; - geometry_msgs::msg::WrenchStamped measured_force_filtered_; - geometry_msgs::msg::WrenchStamped measured_force_control_frame_; - geometry_msgs::msg::WrenchStamped measured_force_endeffector_frame_; + geometry_msgs::msg::WrenchStamped measured_wrench_; + geometry_msgs::msg::WrenchStamped measured_wrench_filtered_; + geometry_msgs::msg::WrenchStamped measured_wrench_control_frame_; + geometry_msgs::msg::WrenchStamped measured_wrench_endeffector_frame_; geometry_msgs::msg::PoseStamped origin_ik_tip_; geometry_msgs::msg::PoseStamped origin_endeffector_; @@ -160,7 +160,7 @@ class AdmittanceRule geometry_msgs::msg::TransformStamped relative_desired_pose_; // Pre-reserved update-loop variables - std::array measured_force_control_frame_arr_; + std::array measured_wrench_control_frame_arr_; std::array target_pose_control_frame_arr_; std::array current_pose_control_frame_arr_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b5ae2a2ca0..a0c8538b8c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -484,7 +484,7 @@ CallbackReturn AdmittanceController::on_deactivate( controller_interface::return_type AdmittanceController::update() { // get input commands - auto input_force_cmd = input_force_command_.readFromRT(); + auto input_wrench_cmd = input_force_command_.readFromRT(); auto input_joint_cmd = input_joint_command_.readFromRT(); auto input_pose_cmd = input_pose_command_.readFromRT(); @@ -511,7 +511,7 @@ controller_interface::return_type AdmittanceController::update() // TODO(destogl): Enable this when unified mode is used // if (admittance_->unified_mode_) { - // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_force_cmd, duration_since_last_call, desired_joint_states); + // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_wrench_cmd, duration_since_last_call, desired_joint_states); // } else { // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) @@ -549,7 +549,7 @@ controller_interface::return_type AdmittanceController::update() // Publish controller state state_publisher_->lock(); - state_publisher_->msg_.input_force_command = **input_force_cmd; + state_publisher_->msg_.input_wrench_command = **input_wrench_cmd; state_publisher_->msg_.input_pose_command = **input_pose_cmd; state_publisher_->msg_.input_joint_command = **input_joint_cmd; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 317594f92f..db75ab2d18 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -164,7 +164,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared tf_listener_ = std::make_shared(*tf_buffer_); // Initialize variables used in the update loop - measured_force_.header.frame_id = sensor_frame_; + measured_wrench_.header.frame_id = sensor_frame_; relative_desired_pose_.header.frame_id = control_frame_; @@ -178,7 +178,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared controller_interface::return_type AdmittanceRule::reset() { - measured_force_control_frame_arr_.fill(0.0); + measured_wrench_control_frame_arr_.fill(0.0); target_pose_control_frame_arr_.fill(0.0); current_pose_control_frame_arr_.fill(0.0); @@ -210,7 +210,7 @@ controller_interface::return_type AdmittanceRule::reset() // Update with target Cartesian pose - the main update method! controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::Wrench & measured_wrench, const geometry_msgs::msg::PoseStamped & target_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state @@ -246,11 +246,13 @@ controller_interface::return_type AdmittanceRule::update( if (pose_error[i] < 1e-10) { pose_error[i] = 0; } + RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", + i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); } - process_force_measurements(measured_force); + process_force_measurements(measured_wrench); - calculate_admittance_rule(measured_force_control_frame_arr_, pose_error, period, + calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); // Do clean conversion to the goal pose using transform and not messing with Euler angles @@ -297,7 +299,7 @@ controller_interface::return_type AdmittanceRule::update( // Update from target joint deltas controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const geometry_msgs::msg::Wrench & measured_force, + const geometry_msgs::msg::Wrench & measured_wrench, const std::array & target_joint_deltas, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) @@ -340,26 +342,15 @@ controller_interface::return_type AdmittanceRule::update( convert_array_to_message(target_ik_tip_deltas_vec, target_ik_tip_deltas_pose); tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, target_ik_tip_deltas_pose); - tf2::Quaternion q; - q.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); - target_pose.pose.orientation.w = q.w(); - target_pose.pose.orientation.x = q.x(); - target_pose.pose.orientation.y = q.y(); - target_pose.pose.orientation.z = q.z(); - -// RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), -// "IK-tip deltas: x: %e, y: %e, z: %e, rx: %e, ry: %e, rz: %e", -// target_ik_tip_deltas_vec.at(0), target_ik_tip_deltas_vec.at(1), -// target_ik_tip_deltas_vec.at(2), target_ik_tip_deltas_vec.at(3), -// target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5) -// ); - - return update(current_joint_state, measured_force, target_pose, period, desired_joint_state); + transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_eff_pose.pose); + target_eff_pose.header = target_ik_tip_pose.header; + + return update(current_joint_state, measured_wrench, target_eff_pose, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_state*/, - const geometry_msgs::msg::Wrench & /*measured_force*/, + const geometry_msgs::msg::Wrench & /*measured_wrench*/, const geometry_msgs::msg::PoseStamped & /*target_pose*/, const geometry_msgs::msg::WrenchStamped & /*target_force*/, const rclcpp::Duration & /*period*/, @@ -378,19 +369,19 @@ controller_interface::return_type AdmittanceRule::get_controller_state( { // state_message.input_force_control_frame = target_force_control_frame_; state_message.input_pose_control_frame = target_pose_control_frame_; - state_message.measured_force = measured_force_; - state_message.measured_force_filtered = measured_force_filtered_; - state_message.measured_force_control_frame = measured_force_control_frame_; + state_message.measured_wrench = measured_wrench_; + state_message.measured_wrench_filtered = measured_wrench_filtered_; + state_message.measured_wrench_control_frame = measured_wrench_control_frame_; try { auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); - tf2::doTransform(measured_force_control_frame_, measured_force_endeffector_frame_, transform); + tf2::doTransform(measured_wrench_control_frame_, measured_wrench_endeffector_frame_, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + endeffector_frame_ + "'."); } - state_message.measured_force_endeffector_frame = measured_force_endeffector_frame_; + state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; state_message.desired_pose = desired_pose_; state_message.relative_desired_pose = relative_desired_pose_; @@ -417,17 +408,17 @@ controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffecto } void AdmittanceRule::process_force_measurements( - const geometry_msgs::msg::Wrench & measured_forces + const geometry_msgs::msg::Wrench & measured_wrench ) { // get current states, and transform those into controller frame - measured_force_.wrench = measured_forces; + measured_wrench_.wrench = measured_wrench; try { - auto transform = tf_buffer_->lookupTransform(fixed_world_frame_, measured_force_.header.frame_id, tf2::TimePointZero); - auto transform_back = tf_buffer_->lookupTransform(measured_force_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(fixed_world_frame_, measured_wrench_.header.frame_id, tf2::TimePointZero); + auto transform_back = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); - geometry_msgs::msg::WrenchStamped measured_force_transformed; - tf2::doTransform(measured_force_, measured_force_transformed, transform); + geometry_msgs::msg::WrenchStamped measured_wrench_transformed; + tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform); geometry_msgs::msg::Vector3Stamped cog_transformed; @@ -435,32 +426,32 @@ void AdmittanceRule::process_force_measurements( auto transform_cog = tf_buffer_->lookupTransform(fixed_world_frame_, params.cog_.header.frame_id, tf2::TimePointZero); tf2::doTransform(params.cog_, cog_transformed, transform_cog); - measured_force_transformed.wrench.force.z += params.force_; - measured_force_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y); - measured_force_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); + measured_wrench_transformed.wrench.force.z += params.force_; + measured_wrench_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y); + measured_wrench_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); } - tf2::doTransform(measured_force_transformed, measured_force_filtered_, transform_back); + tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_back); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_force_.header.frame_id + "' or ''."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_wrench_.header.frame_id + "' or ''."); // If transform error just use measured force - measured_force_filtered_ = measured_force_; + measured_wrench_filtered_ = measured_wrench_; } - transform_message_to_control_frame(measured_force_filtered_, measured_force_control_frame_); + transform_message_to_control_frame(measured_wrench_filtered_, measured_wrench_control_frame_); - convert_message_to_array(measured_force_control_frame_, measured_force_control_frame_arr_); + convert_message_to_array(measured_wrench_control_frame_, measured_wrench_control_frame_arr_); // If at least one measured force is nan set all to 0 - if (std::find_if(measured_force_control_frame_arr_.begin(), measured_force_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_force_control_frame_arr_.end()) { - measured_force_control_frame_arr_.fill(0.0); + if (std::find_if(measured_wrench_control_frame_arr_.begin(), measured_wrench_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_wrench_control_frame_arr_.end()) { + measured_wrench_control_frame_arr_.fill(0.0); } } void AdmittanceRule::calculate_admittance_rule( - const std::array & measured_force, + const std::array & measured_wrench, const std::array & pose_error, const rclcpp::Duration & period, std::array & desired_relative_pose @@ -471,7 +462,7 @@ void AdmittanceRule::calculate_admittance_rule( if (selected_axes_[i]) { // TODO(destogl): check if velocity is measured from hardware const double acceleration = 1 / mass_[i] * - (measured_force[i] - damping_[i] * desired_velocity_arr_[i] - + (measured_wrench[i] - damping_[i] * desired_velocity_arr_[i] - stiffness_[i] * pose_error[i]); desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); @@ -481,7 +472,7 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; - RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%e - D*%e - S*%e = %e)", i, measured_force[i], desired_velocity_arr_[i], pose_error , acceleration); +// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_wrench_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); } } } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index f37fabf042..f5de010017 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -340,14 +340,14 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - // Check that force command are all zero since not used - ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); - ASSERT_EQ(msg.input_force_command.wrench.force.x, 0.0); - ASSERT_EQ(msg.input_force_command.wrench.force.y, 0.0); - ASSERT_EQ(msg.input_force_command.wrench.force.z, 0.0); - ASSERT_EQ(msg.input_force_command.wrench.torque.x, 0.0); - ASSERT_EQ(msg.input_force_command.wrench.torque.y, 0.0); - ASSERT_EQ(msg.input_force_command.wrench.torque.z, 0.0); + // 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_); @@ -370,29 +370,29 @@ TEST_F(AdmittanceControllerTest, publish_status_success) [](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_force.header.frame_id, sensor_frame_); - ASSERT_EQ(msg.measured_force.wrench.force.x, fts_state_values_[0]); - ASSERT_EQ(msg.measured_force.wrench.force.y, fts_state_values_[1]); - ASSERT_EQ(msg.measured_force.wrench.force.z, fts_state_values_[2]); - ASSERT_EQ(msg.measured_force.wrench.torque.x, fts_state_values_[3]); - ASSERT_EQ(msg.measured_force.wrench.torque.y, fts_state_values_[4]); - ASSERT_EQ(msg.measured_force.wrench.torque.z, fts_state_values_[5]); - - ASSERT_EQ(msg.measured_force_control_frame.header.frame_id, control_frame_); - ASSERT_EQ(msg.measured_force_control_frame.wrench.force.x, 0.0); - ASSERT_EQ(msg.measured_force_control_frame.wrench.force.y, 0.0); - ASSERT_EQ(msg.measured_force_control_frame.wrench.force.z, 0.0); - ASSERT_EQ(msg.measured_force_control_frame.wrench.torque.x, 0.0); - ASSERT_EQ(msg.measured_force_control_frame.wrench.torque.y, 0.0); - ASSERT_EQ(msg.measured_force_control_frame.wrench.torque.z, 0.0); - - ASSERT_EQ(msg.measured_force_endeffector_frame.header.frame_id, endeffector_frame_); - ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.force.x, 0.0); - ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.force.y, 0.0); - ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.force.z, 0.0); - ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.torque.x, 0.0); - ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.torque.y, 0.0); - ASSERT_EQ(msg.measured_force_endeffector_frame.wrench.torque.z, 0.0); + 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.measured_wrench_endeffector_frame.header.frame_id, endeffector_frame_); + ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.force.x, 0.0); + ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.force.y, 0.0); + ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.force.z, 0.0); + ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.torque.x, 0.0); + ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.torque.y, 0.0); + ASSERT_EQ(msg.measured_wrench_endeffector_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)); @@ -448,7 +448,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); ASSERT_EQ(msg.input_pose_command.header.frame_id, control_frame_); publish_commands(); @@ -460,7 +460,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) EXPECT_NEAR(joint_command_values_[0], 0.0, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - ASSERT_EQ(msg.input_force_command.header.frame_id, control_frame_); + ASSERT_EQ(msg.input_wrench_command.header.frame_id, control_frame_); ASSERT_EQ(msg.input_pose_command.header.frame_id, endeffector_frame_); } From 5ab76406444cf910e3359bdf2542953d52c4b44d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 20 May 2021 08:50:32 +0200 Subject: [PATCH 43/99] Try to debug force input --- .../admittance_controller/admittance_rule.hpp | 2 +- admittance_controller/src/admittance_rule.cpp | 24 +++++++++++-------- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index b04de827d2..b01daecb1c 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -181,7 +181,7 @@ class AdmittanceRule if (control_frame_ != message_in.header.frame_id) { try { geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - control_frame_, message_in.header.frame_id, tf2::TimePointZero); + message_in.header.frame_id, control_frame_, tf2::TimePointZero); tf2::doTransform(message_in, message_out, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index db75ab2d18..84da3a4624 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -217,13 +217,13 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to control frame - // TODO(andyz): this causes unexpected rotation - //transform_message_to_control_frame(target_pose, target_pose_control_frame_); +// transform_message_to_control_frame(target_pose, target_pose_control_frame_); target_pose_control_frame_ = target_pose; // if (!hardware_state_has_offset_) { get_current_pose_of_endeffector_frame(current_pose_); - transform_message_to_control_frame(current_pose_, current_pose_control_frame_); +// transform_message_to_control_frame(current_pose_, current_pose_control_frame_); + current_pose_control_frame_ = current_pose_; // } // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { @@ -239,15 +239,17 @@ controller_interface::return_type AdmittanceRule::update( for (auto i = 0u; i < 6; ++i) { pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; if (i >= 3) { - pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i]) - - angles::normalize_angle(target_pose_control_frame_arr_[i]); - } - // remove small noise due transformations - if (pose_error[i] < 1e-10) { - pose_error[i] = 0; + pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i] - + target_pose_control_frame_arr_[i]); } RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); + // remove small noise due transformations +// if (pose_error[i] < 0.00000000001) { +// pose_error[i] = 0; +// } +// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", +// i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); } process_force_measurements(measured_wrench); @@ -472,7 +474,9 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; -// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error, acceleration, desired velocity, relative desired pose [%zu]: (%f - D*%f - S*%f = %f), %f, %f", i, measured_wrench_control_frame_arr_[i], desired_velocity_arr_[i], pose_error , acceleration, desired_velocity_arr_[i], relative_desired_pose_arr_[i]); + RCLCPP_INFO(rclcpp::get_logger("AR"), "Rule [%zu]: (%e = %e - D(%.1f)*%e - S(%.1f)*%e)", i, + acceleration, measured_wrench[i], damping_[i], desired_velocity_arr_[i], + stiffness_[i], pose_error); } } } From 5ec8851a7869d2466b59ad3b680971dacb42a8ab Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 10:06:08 -0500 Subject: [PATCH 44/99] Add update_robot_state() function --- .../admittance_controller/incremental_kinematics.hpp | 12 ++++++++++++ admittance_controller/src/admittance_rule.cpp | 2 ++ 2 files changed, 14 insertions(+) diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index 64c666be25..02c16b812b 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -54,6 +54,18 @@ class IncrementalKinematics bool convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec); + bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) + { + if (current_joint_state.positions.size() != kinematic_state_->getVariableNames().size()) + { + RCLCPP_ERROR(node_->get_logger(), "Vector size mismatch in update_robot_state()"); + return false; + } + + kinematic_state_->setVariablePositions(current_joint_state.positions); + return true; + } + private: // MoveIt setup, required to retrieve the Jacobian const moveit::core::JointModelGroup* joint_model_group_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 84da3a4624..70f21a4e6c 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -327,6 +327,8 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } + ik_->update_robot_state(current_joint_state); + // TODO(destogl): Use as class variables to avoid memory allocation geometry_msgs::msg::PoseStamped current_ik_tip_pose; geometry_msgs::msg::TransformStamped target_ik_tip_deltas_pose; From 137a4cab9fef1a15e4494d9771a4211d51c0e3f1 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 10:32:14 -0500 Subject: [PATCH 45/99] Rename get_current_pose_of_endeffector_frame->get_pose_of_endeffector_in_base_frame --- .../admittance_controller/admittance_rule.hpp | 2 +- admittance_controller/src/admittance_rule.cpp | 24 ++++++++++--------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index b01daecb1c..ece0bb2a2d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -78,7 +78,7 @@ class AdmittanceRule control_msgs::msg::AdmittanceControllerState & state_message ); - controller_interface::return_type get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose); + controller_interface::return_type get_pose_of_endeffector_in_base_frame(geometry_msgs::msg::PoseStamped & pose); public: bool hardware_state_has_offset_ = false; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 70f21a4e6c..3308d9939b 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -189,7 +189,7 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); - get_current_pose_of_endeffector_frame(current_pose_); + get_pose_of_endeffector_in_base_frame(current_pose_); // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; @@ -221,9 +221,8 @@ controller_interface::return_type AdmittanceRule::update( target_pose_control_frame_ = target_pose; // if (!hardware_state_has_offset_) { - get_current_pose_of_endeffector_frame(current_pose_); -// transform_message_to_control_frame(current_pose_, current_pose_control_frame_); - current_pose_control_frame_ = current_pose_; + get_pose_of_endeffector_in_base_frame(current_pose_); + transform_message_to_control_frame(current_pose_, current_pose_control_frame_); // } // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { @@ -236,14 +235,17 @@ controller_interface::return_type AdmittanceRule::update( std::array pose_error; + // TODO(andy): these errors should be (target - current) for (auto i = 0u; i < 6; ++i) { pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; if (i >= 3) { pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i] - - target_pose_control_frame_arr_[i]); + target_pose_control_frame_arr_[i]); + RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", + i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); } - RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", - i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); + // RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", + // i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); // remove small noise due transformations // if (pose_error[i] < 0.00000000001) { // pose_error[i] = 0; @@ -393,7 +395,7 @@ controller_interface::return_type AdmittanceRule::get_controller_state( return controller_interface::return_type::OK; } -controller_interface::return_type AdmittanceRule::get_current_pose_of_endeffector_frame(geometry_msgs::msg::PoseStamped & pose) +controller_interface::return_type AdmittanceRule::get_pose_of_endeffector_in_base_frame(geometry_msgs::msg::PoseStamped & pose) { // Get tool frame position - in the future use: IKSolver->getPositionFK(...) static geometry_msgs::msg::PoseStamped origin; @@ -476,9 +478,9 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; - RCLCPP_INFO(rclcpp::get_logger("AR"), "Rule [%zu]: (%e = %e - D(%.1f)*%e - S(%.1f)*%e)", i, - acceleration, measured_wrench[i], damping_[i], desired_velocity_arr_[i], - stiffness_[i], pose_error); + // RCLCPP_INFO(rclcpp::get_logger("AR"), "Rule [%zu]: (%e = %e - D(%.1f)*%e - S(%.1f)*%e)", i, + // acceleration, measured_wrench[i], damping_[i], desired_velocity_arr_[i], + // stiffness_[i], pose_error); } } } From f73eb5f064c8e7f15a1861722af339d8b2b33803 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 10:55:53 -0500 Subject: [PATCH 46/99] Small variable renaming. Refactor get_pose_of_endeffector_in_base_frame() --- .../admittance_controller/admittance_rule.hpp | 2 +- .../src/admittance_controller.cpp | 4 ++-- admittance_controller/src/admittance_rule.cpp | 18 +++++++++--------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index ece0bb2a2d..fd1103ada8 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -150,7 +150,7 @@ class AdmittanceRule geometry_msgs::msg::PoseStamped origin_ik_tip_; geometry_msgs::msg::PoseStamped origin_endeffector_; - geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::PoseStamped current_pose_in_base_frame_; geometry_msgs::msg::PoseStamped current_pose_control_frame_; geometry_msgs::msg::WrenchStamped target_force_control_frame_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a0c8538b8c..453fcf6d61 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -336,7 +336,7 @@ CallbackReturn AdmittanceController::on_configure( // TODO(destogl): This will break tests because there is no TF inside them auto iterations = 0u; const auto max_iterations = 20u; - while (admittance_->get_current_pose_of_endeffector_frame(*msg_pose) != controller_interface::return_type::OK) + while (admittance_->get_pose_of_endeffector_in_base_frame(*msg_pose) != controller_interface::return_type::OK) { RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 5000, "Waiting for base to endeffector transform becomes available."); rclcpp::sleep_for(std::chrono::seconds(1)); @@ -455,7 +455,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & std::shared_ptr msg_pose = std::make_shared(); msg_pose->header.frame_id = admittance_->control_frame_; - admittance_->get_current_pose_of_endeffector_frame(*msg_pose); + admittance_->get_pose_of_endeffector_in_base_frame(*msg_pose); input_pose_command_.writeFromNonRT(msg_pose); return CallbackReturn::SUCCESS; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 3308d9939b..d3c6869f00 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -189,7 +189,7 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); - get_pose_of_endeffector_in_base_frame(current_pose_); + get_pose_of_endeffector_in_base_frame(current_pose_in_base_frame_); // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; @@ -221,8 +221,8 @@ controller_interface::return_type AdmittanceRule::update( target_pose_control_frame_ = target_pose; // if (!hardware_state_has_offset_) { - get_pose_of_endeffector_in_base_frame(current_pose_); - transform_message_to_control_frame(current_pose_, current_pose_control_frame_); + get_pose_of_endeffector_in_base_frame(current_pose_in_base_frame_); + transform_message_to_control_frame(current_pose_in_base_frame_, current_pose_control_frame_); // } // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { @@ -397,14 +397,14 @@ controller_interface::return_type AdmittanceRule::get_controller_state( controller_interface::return_type AdmittanceRule::get_pose_of_endeffector_in_base_frame(geometry_msgs::msg::PoseStamped & pose) { - // Get tool frame position - in the future use: IKSolver->getPositionFK(...) - static geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = endeffector_frame_; - origin.pose.orientation.w = 1; - try { auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); - tf2::doTransform(origin, pose, transform); + + pose.header = transform.header; + pose.pose.position.x = transform.transform.translation.x; + pose.pose.position.y = transform.transform.translation.y; + pose.pose.position.z = transform.transform.translation.z; + pose.pose.orientation= transform.transform.rotation; } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + endeffector_frame_ + "'."); From 7c9ef25c3a15c4c369cba4e3582235b3087e1f59 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 13:09:22 -0500 Subject: [PATCH 47/99] Hard-code to always use joint commands --- admittance_controller/src/admittance_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 453fcf6d61..b9b8158f06 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -515,7 +515,8 @@ controller_interface::return_type AdmittanceController::update() // } else { // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) - if (use_joint_commands_as_input_) { + // TODO(andyz): this should be optional + if (true) { std::array joint_deltas; // If there are no positions, expect velocities // TODO(destogl): add error handling From 9a5e42e018a5eac2527a3fc88708ad84526c108a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 13:14:50 -0500 Subject: [PATCH 48/99] This works but does not "spring back" --- admittance_controller/src/admittance_rule.cpp | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index d3c6869f00..44be2b2c24 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -331,27 +331,26 @@ controller_interface::return_type AdmittanceRule::update( ik_->update_robot_state(current_joint_state); - // TODO(destogl): Use as class variables to avoid memory allocation - geometry_msgs::msg::PoseStamped current_ik_tip_pose; - geometry_msgs::msg::TransformStamped target_ik_tip_deltas_pose; - target_ik_tip_deltas_pose.header.frame_id = ik_base_frame_; - target_ik_tip_deltas_pose.child_frame_id = ik_base_frame_; - geometry_msgs::msg::PoseStamped target_ik_tip_pose; - geometry_msgs::msg::PoseStamped target_eff_pose; - static geometry_msgs::msg::PoseStamped origin; - origin.header.frame_id = ik_tip_frame_; - origin.pose.orientation.w = 1; - - // If FK this is not needed - // TODO(anyone): Can we just use values from transformation instead calling doTransform? - tf2::doTransform(origin, current_ik_tip_pose, transform_ik_base_tip); - convert_array_to_message(target_ik_tip_deltas_vec, target_ik_tip_deltas_pose); - tf2::doTransform(current_ik_tip_pose, target_ik_tip_pose, target_ik_tip_deltas_pose); - - transform_ik_tip_to_endeffector_frame(target_ik_tip_pose.pose, target_eff_pose.pose); - target_eff_pose.header = target_ik_tip_pose.header; - - return update(current_joint_state, measured_wrench, target_eff_pose, period, desired_joint_state); + // Get the target pose in ik_base frame + geometry_msgs::msg::PoseStamped target_pose_base_frame; + target_pose_base_frame.header.frame_id = ik_base_frame_; + target_pose_base_frame.pose.position.x = transform_ik_base_tip.transform.translation.x; + target_pose_base_frame.pose.position.y = transform_ik_base_tip.transform.translation.y; + target_pose_base_frame.pose.position.z = transform_ik_base_tip.transform.translation.z; + target_pose_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); + target_pose_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); + target_pose_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); tf2::Quaternion q(transform_ik_base_tip.transform.rotation.x, transform_ik_base_tip.transform.rotation.y, transform_ik_base_tip.transform.rotation.z, transform_ik_base_tip.transform.rotation.w); + tf2::Quaternion q_rot; + RCLCPP_INFO_STREAM(rclcpp::get_logger("AdmittanceRule"), "Rotation delta: " << target_ik_tip_deltas_vec.at(3) << " " << target_ik_tip_deltas_vec.at(4) << " " << target_ik_tip_deltas_vec.at(5)); + q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); + q = q_rot * q; + q.normalize(); + target_pose_base_frame.pose.orientation.w = q.w(); + target_pose_base_frame.pose.orientation.x = q.x(); + target_pose_base_frame.pose.orientation.y = q.y(); + target_pose_base_frame.pose.orientation.z = q.z(); + + return update(current_joint_state, measured_wrench, target_pose_base_frame, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( From 3e7bc371680e1d9ec95181fa5ba9a4323b64bee5 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 13:22:45 -0500 Subject: [PATCH 49/99] Feed different transform into IK --- admittance_controller/src/admittance_rule.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 44be2b2c24..b944d24726 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -320,8 +320,10 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // Get cartesian deltas in the IK tip frame - if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { + geometry_msgs::msg::TransformStamped identity_tf; + identity_tf.header.frame_id = ik_base_frame_; + identity_tf.transform.rotation.w = 1; + if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, identity_tf, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); desired_joint_state = current_joint_state; From 48b962d18b27e865d020e16730cf2fac853f7543 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 13:36:12 -0500 Subject: [PATCH 50/99] Rename tf arg to IK. Delete debug statements. --- .../admittance_controller/incremental_kinematics.hpp | 4 ++-- admittance_controller/src/admittance_rule.cpp | 11 ----------- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index 02c16b812b..06e4157017 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -47,12 +47,12 @@ class IncrementalKinematics /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. * \param[in] delta_theta_vec vector with joint states - * \param[in] ik_base_to_tip_tf transformation between ik base to ik tip + * \param[in] tf_ik_base_to_desired_cartesian_frame transformation to the desired Cartesian frame. Use identity matrix to stay in the ik_base frame. * \param[out] delta_x_vec Cartesian deltas (x, y, z, rx, ry, rz) * \return true if successful */ bool - convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec); + convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec); bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index b944d24726..a849305f3b 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -241,17 +241,7 @@ controller_interface::return_type AdmittanceRule::update( if (i >= 3) { pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]); - RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", - i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); } - // RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", - // i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); - // remove small noise due transformations -// if (pose_error[i] < 0.00000000001) { -// pose_error[i] = 0; -// } -// RCLCPP_INFO(rclcpp::get_logger("AR"), "Pose error [%zu]: (%e = %e - %e)", -// i, pose_error[i], current_pose_control_frame_arr_[i], target_pose_control_frame_arr_[i]); } process_force_measurements(measured_wrench); @@ -343,7 +333,6 @@ controller_interface::return_type AdmittanceRule::update( target_pose_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); target_pose_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); tf2::Quaternion q(transform_ik_base_tip.transform.rotation.x, transform_ik_base_tip.transform.rotation.y, transform_ik_base_tip.transform.rotation.z, transform_ik_base_tip.transform.rotation.w); tf2::Quaternion q_rot; - RCLCPP_INFO_STREAM(rclcpp::get_logger("AdmittanceRule"), "Rotation delta: " << target_ik_tip_deltas_vec.at(3) << " " << target_ik_tip_deltas_vec.at(4) << " " << target_ik_tip_deltas_vec.at(5)); q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); q = q_rot * q; q.normalize(); From f16fc35b35db0115955c026ff09526948767da12 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 21 May 2021 14:20:49 -0500 Subject: [PATCH 51/99] General cleanup, variable documentation and renaming --- .../admittance_controller/admittance_rule.hpp | 6 +++-- admittance_controller/src/admittance_rule.cpp | 24 +++++++------------ 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index fd1103ada8..dee42d272a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -143,20 +143,22 @@ class AdmittanceRule tf2::Transform ik_tip_to_endeffector_frame_tf_; tf2::Transform endeffector_frame_to_ik_tip_tf_; + // measured_wrench_ could arrive in any frame. It will be transformed geometry_msgs::msg::WrenchStamped measured_wrench_; geometry_msgs::msg::WrenchStamped measured_wrench_filtered_; + geometry_msgs::msg::WrenchStamped measured_wrench_control_frame_; geometry_msgs::msg::WrenchStamped measured_wrench_endeffector_frame_; geometry_msgs::msg::PoseStamped origin_ik_tip_; geometry_msgs::msg::PoseStamped origin_endeffector_; - geometry_msgs::msg::PoseStamped current_pose_in_base_frame_; + geometry_msgs::msg::PoseStamped current_pose_base_frame_; geometry_msgs::msg::PoseStamped current_pose_control_frame_; geometry_msgs::msg::WrenchStamped target_force_control_frame_; geometry_msgs::msg::PoseStamped target_pose_control_frame_; - geometry_msgs::msg::PoseStamped desired_pose_; + geometry_msgs::msg::PoseStamped desired_pose_control_frame_; geometry_msgs::msg::TransformStamped relative_desired_pose_; // Pre-reserved update-loop variables diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index a849305f3b..75cfcb63da 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -189,7 +189,7 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); - get_pose_of_endeffector_in_base_frame(current_pose_in_base_frame_); + get_pose_of_endeffector_in_base_frame(current_pose_base_frame_); // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; @@ -220,13 +220,13 @@ controller_interface::return_type AdmittanceRule::update( // transform_message_to_control_frame(target_pose, target_pose_control_frame_); target_pose_control_frame_ = target_pose; -// if (!hardware_state_has_offset_) { - get_pose_of_endeffector_in_base_frame(current_pose_in_base_frame_); - transform_message_to_control_frame(current_pose_in_base_frame_, current_pose_control_frame_); -// } + if (!hardware_state_has_offset_) { + get_pose_of_endeffector_in_base_frame(current_pose_base_frame_); + transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); + } // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { -// current_pose_control_frame_ = desired_pose_; +// current_pose_control_frame_ = desired_pose_control_frame_; // } // Convert all data to arrays for simpler calculation @@ -379,7 +379,7 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; - state_message.desired_pose = desired_pose_; + state_message.desired_pose = desired_pose_control_frame_; state_message.relative_desired_pose = relative_desired_pose_; return controller_interface::return_type::OK; @@ -483,8 +483,8 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( { // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); - transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); + tf2::doTransform(current_pose_control_frame_, desired_pose_control_frame_, relative_desired_pose_); + transform_ik_tip_to_endeffector_frame(desired_pose_control_frame_.pose, desired_pose_control_frame_.pose); // Calculate desired Cartesian displacement of the robot // TODO: replace this with FK in the long term @@ -513,12 +513,6 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( return controller_interface::return_type::ERROR; } - // TODO(anyone: enable this when enabling use of IK directly - // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); - // tf2::doTransform(desired_pose_, ik_input_pose_, transform); - // ik_input_pose_.pose = transform_endeffector_to_ik_tip_frame(ik_input_pose_); - // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... - return controller_interface::return_type::OK; } From 469a9f4d59cc6794c1a9453154e4d722d50374e8 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 25 May 2021 11:21:33 -0500 Subject: [PATCH 52/99] More renaming of force->wrench --- .../admittance_controller.hpp | 10 +++++----- .../src/admittance_controller.cpp | 14 +++++++------- .../test/test_admittance_controller.hpp | 14 +++++++------- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index a279225a47..46b8bd1d77 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -86,18 +86,18 @@ class AdmittanceController : public controller_interface::ControllerInterface rclcpp::Time previous_time_; // Command subscribers and Controller State publisher - using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; + using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory; - rclcpp::Subscription::SharedPtr - input_force_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + input_wrench_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr input_pose_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr input_joint_command_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> - input_force_command_; + realtime_tools::RealtimeBuffer> + input_wrench_command_; realtime_tools::RealtimeBuffer> input_pose_command_; realtime_tools::RealtimeBuffer> diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b9b8158f06..6ab281949b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -288,12 +288,12 @@ CallbackReturn AdmittanceController::on_configure( // Subscribers and callbacks if (admittance_->unified_mode_) { - auto callback_input_force = [&](const std::shared_ptr msg) + auto callback_input_force = [&](const std::shared_ptr msg) -> void { - input_force_command_.writeFromNonRT(msg); + input_wrench_command_.writeFromNonRT(msg); }; - input_force_command_subscriber_ = get_node()->create_subscription( + input_wrench_command_subscriber_ = get_node()->create_subscription( "~/force_commands", rclcpp::SystemDefaultsQoS(), callback_input_force); } @@ -435,9 +435,9 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & } // Set initial command values - initialize all to simplify update - std::shared_ptr msg_force = std::make_shared(); - msg_force->header.frame_id = admittance_->control_frame_; - input_force_command_.writeFromNonRT(msg_force); + std::shared_ptr msg_wrench = std::make_shared(); + msg_wrench->header.frame_id = admittance_->control_frame_; + input_wrench_command_.writeFromNonRT(msg_wrench); std::shared_ptr msg_joint = std::make_shared(); msg_joint->joint_names = joint_names_; @@ -484,7 +484,7 @@ CallbackReturn AdmittanceController::on_deactivate( controller_interface::return_type AdmittanceController::update() { // get input commands - auto input_wrench_cmd = input_force_command_.readFromRT(); + auto input_wrench_cmd = input_wrench_command_.readFromRT(); auto input_joint_cmd = input_joint_command_.readFromRT(); auto input_pose_cmd = input_pose_command_.readFromRT(); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 8a4add8fb0..dd84624e3d 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -38,7 +38,7 @@ #include "tf2_ros/transform_broadcaster.h" // TODO(anyone): replace the state and command message types -using ControllerCommandForceMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory; using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; @@ -81,7 +81,7 @@ class TestableAdmittanceController // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { if (admittance_->unified_mode_) { - input_force_command_subscriber_wait_set_.add_subscription(input_force_command_subscriber_); + input_wrench_command_subscriber_wait_set_.add_subscription(input_wrench_command_subscriber_); } input_pose_command_subscriber_wait_set_.add_subscription(input_pose_command_subscriber_); } @@ -102,7 +102,7 @@ class TestableAdmittanceController bool success = input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; if (admittance_->unified_mode_) { - success = success && input_force_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + success = success && input_wrench_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; } if (success) { executor.spin_some(); @@ -111,7 +111,7 @@ class TestableAdmittanceController } private: - rclcpp::WaitSet input_force_command_subscriber_wait_set_; + rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; rclcpp::WaitSet input_pose_command_subscriber_wait_set_; }; @@ -129,7 +129,7 @@ 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( + 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()); @@ -334,7 +334,7 @@ class AdmittanceControllerTest : public ::testing::Test wait_for_topic(pose_command_publisher_->get_topic_name()); - ControllerCommandForceMsg force_msg; + ControllerCommandWrenchMsg force_msg; force_msg.header.frame_id = sensor_frame_; force_msg.wrench.force.x = 0.1; force_msg.wrench.force.y = 0.2; @@ -413,7 +413,7 @@ class AdmittanceControllerTest : public ::testing::Test // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Publisher::SharedPtr force_command_publisher_; + rclcpp::Publisher::SharedPtr force_command_publisher_; rclcpp::Publisher::SharedPtr pose_command_publisher_; rclcpp::Publisher::SharedPtr joint_command_publisher_; rclcpp::Node::SharedPtr test_subscription_node_; From b897a9cd29ee705d310ddc78a604ab7659dd94c1 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 25 May 2021 14:24:51 -0500 Subject: [PATCH 53/99] Remove endeffector_frame (redundant with control_frame) --- .../admittance_controller/admittance_rule.hpp | 15 +++-- .../src/admittance_controller.cpp | 8 +-- admittance_controller/src/admittance_rule.cpp | 59 ++++++++----------- .../test/test_admittance_controller.cpp | 14 ----- 4 files changed, 35 insertions(+), 61 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index dee42d272a..2d23561344 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -78,7 +78,7 @@ class AdmittanceRule control_msgs::msg::AdmittanceControllerState & state_message ); - controller_interface::return_type get_pose_of_endeffector_in_base_frame(geometry_msgs::msg::PoseStamped & pose); + controller_interface::return_type get_pose_of_control_frame_in_base_frame(geometry_msgs::msg::PoseStamped & pose); public: bool hardware_state_has_offset_ = false; @@ -90,7 +90,6 @@ class AdmittanceRule // Controller frames std::string control_frame_; - std::string endeffector_frame_; std::string fixed_world_frame_; std::string sensor_frame_; @@ -140,8 +139,8 @@ class AdmittanceRule std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - tf2::Transform ik_tip_to_endeffector_frame_tf_; - tf2::Transform endeffector_frame_to_ik_tip_tf_; + tf2::Transform ik_tip_to_control_frame_tf_; + tf2::Transform control_frame_to_ik_tip_tf_; // measured_wrench_ could arrive in any frame. It will be transformed geometry_msgs::msg::WrenchStamped measured_wrench_; @@ -210,16 +209,16 @@ class AdmittanceRule template void - transform_ik_tip_to_endeffector_frame(const Type & base_to_ik_tip, Type & base_to_toollink) + transform_ik_tip_to_control_frame(const Type & base_to_ik_tip, Type & base_to_toollink) { - direct_transform(base_to_ik_tip, ik_tip_to_endeffector_frame_tf_, base_to_toollink); + direct_transform(base_to_ik_tip, ik_tip_to_control_frame_tf_, base_to_toollink); } template void - transform_endeffector_to_ik_tip_frame(const Type & base_to_toollink, Type & base_to_ik_tip) + transform_control_to_ik_tip_frame(const Type & base_to_toollink, Type & base_to_ik_tip) { - direct_transform(base_to_toollink, endeffector_frame_to_ik_tip_tf_, base_to_ik_tip); + direct_transform(base_to_toollink, control_frame_to_ik_tip_tf_, base_to_ik_tip); } }; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6ab281949b..e242b2ce07 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -57,7 +57,6 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("IK.group_name", ""); get_node()->declare_parameter("control_frame", ""); - get_node()->declare_parameter("endeffector_frame", ""); get_node()->declare_parameter("fixed_world_frame", ""); get_node()->declare_parameter("sensor_frame", ""); @@ -167,7 +166,6 @@ CallbackReturn AdmittanceController::on_configure( get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") || - get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") || get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") || get_string_param_and_error_if_empty(admittance_->sensor_frame_, "sensor_frame") || // TODO(destogl): add unified mode considering target force @@ -336,9 +334,9 @@ CallbackReturn AdmittanceController::on_configure( // TODO(destogl): This will break tests because there is no TF inside them auto iterations = 0u; const auto max_iterations = 20u; - while (admittance_->get_pose_of_endeffector_in_base_frame(*msg_pose) != controller_interface::return_type::OK) + while (admittance_->get_pose_of_control_frame_in_base_frame(*msg_pose) != controller_interface::return_type::OK) { - RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 5000, "Waiting for base to endeffector transform becomes available."); + RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 5000, "Waiting for base to control frame transform to become available."); rclcpp::sleep_for(std::chrono::seconds(1)); if (++iterations > max_iterations) { RCLCPP_ERROR(get_node()->get_logger(), "After waiting for TF for %zu seconds, still no transformation available. Admittance Controller can not be configured.", max_iterations); @@ -455,7 +453,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & std::shared_ptr msg_pose = std::make_shared(); msg_pose->header.frame_id = admittance_->control_frame_; - admittance_->get_pose_of_endeffector_in_base_frame(*msg_pose); + admittance_->get_pose_of_control_frame_in_base_frame(*msg_pose); input_pose_command_.writeFromNonRT(msg_pose); return CallbackReturn::SUCCESS; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 75cfcb63da..16622babfe 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -189,18 +189,18 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); - get_pose_of_endeffector_in_base_frame(current_pose_base_frame_); + get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; try { - auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, endeffector_frame_, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, control_frame_, tf2::TimePointZero); tf2::fromMsg(transform, tf2_transform); - ik_tip_to_endeffector_frame_tf_ = tf2_transform; - endeffector_frame_to_ik_tip_tf_ = tf2_transform.inverse(); + ik_tip_to_control_frame_tf_ = tf2_transform; + control_frame_to_ik_tip_tf_ = tf2_transform.inverse(); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_tip_frame_ + "' and '" + endeffector_frame_ + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_tip_frame_ + "' and '" + control_frame_ + "'."); return controller_interface::return_type::ERROR; } @@ -221,7 +221,7 @@ controller_interface::return_type AdmittanceRule::update( target_pose_control_frame_ = target_pose; if (!hardware_state_has_offset_) { - get_pose_of_endeffector_in_base_frame(current_pose_base_frame_); + get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); } // TODO(destogl): Can this work properly, when considering offset between states and commands? @@ -301,19 +301,19 @@ controller_interface::return_type AdmittanceRule::update( std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); - geometry_msgs::msg::TransformStamped transform_ik_base_tip; - try { - transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); - return controller_interface::return_type::ERROR; - } + // geometry_msgs::msg::TransformStamped transform_ik_base_tip; + // try { + // transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + // } catch (const tf2::TransformException & e) { + // // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + // RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); + // return controller_interface::return_type::ERROR; + // } - geometry_msgs::msg::TransformStamped identity_tf; - identity_tf.header.frame_id = ik_base_frame_; - identity_tf.transform.rotation.w = 1; - if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, identity_tf, target_ik_tip_deltas_vec)) { + geometry_msgs::msg::TransformStamped transform_ik_base_tip; + transform_ik_base_tip.header.frame_id = ik_base_frame_; + transform_ik_base_tip.transform.rotation.w = 1; + if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); desired_joint_state = current_joint_state; @@ -331,7 +331,9 @@ controller_interface::return_type AdmittanceRule::update( target_pose_base_frame.pose.position.z = transform_ik_base_tip.transform.translation.z; target_pose_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); target_pose_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); - target_pose_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); tf2::Quaternion q(transform_ik_base_tip.transform.rotation.x, transform_ik_base_tip.transform.rotation.y, transform_ik_base_tip.transform.rotation.z, transform_ik_base_tip.transform.rotation.w); + target_pose_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); + + tf2::Quaternion q(transform_ik_base_tip.transform.rotation.x, transform_ik_base_tip.transform.rotation.y, transform_ik_base_tip.transform.rotation.z, transform_ik_base_tip.transform.rotation.w); tf2::Quaternion q_rot; q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); q = q_rot * q; @@ -368,27 +370,16 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_wrench = measured_wrench_; state_message.measured_wrench_filtered = measured_wrench_filtered_; state_message.measured_wrench_control_frame = measured_wrench_control_frame_; - - try { - auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); - tf2::doTransform(measured_wrench_control_frame_, measured_wrench_endeffector_frame_, transform); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + endeffector_frame_ + "'."); - } - - state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; - state_message.desired_pose = desired_pose_control_frame_; state_message.relative_desired_pose = relative_desired_pose_; return controller_interface::return_type::OK; } -controller_interface::return_type AdmittanceRule::get_pose_of_endeffector_in_base_frame(geometry_msgs::msg::PoseStamped & pose) +controller_interface::return_type AdmittanceRule::get_pose_of_control_frame_in_base_frame(geometry_msgs::msg::PoseStamped & pose) { try { - auto transform = tf_buffer_->lookupTransform(ik_base_frame_, endeffector_frame_, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(ik_base_frame_, control_frame_, tf2::TimePointZero); pose.header = transform.header; pose.pose.position.x = transform.transform.translation.x; @@ -397,7 +388,7 @@ controller_interface::return_type AdmittanceRule::get_pose_of_endeffector_in_bas pose.pose.orientation= transform.transform.rotation; } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + endeffector_frame_ + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + control_frame_ + "'."); return controller_interface::return_type::ERROR; } return controller_interface::return_type::OK; @@ -484,7 +475,7 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); tf2::doTransform(current_pose_control_frame_, desired_pose_control_frame_, relative_desired_pose_); - transform_ik_tip_to_endeffector_frame(desired_pose_control_frame_.pose, desired_pose_control_frame_.pose); + transform_ik_tip_to_control_frame(desired_pose_control_frame_.pose, desired_pose_control_frame_.pose); // Calculate desired Cartesian displacement of the robot // TODO: replace this with FK in the long term diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index f5de010017..035cb4c3b1 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -75,10 +75,6 @@ INSTANTIATE_TEST_CASE_P( std::string("control_frame"), rclcpp::ParameterValue("") ), - std::make_tuple( - std::string("endeffector_frame"), - rclcpp::ParameterValue("") - ), std::make_tuple( std::string("fixed_world_frame"), rclcpp::ParameterValue("") @@ -217,7 +213,6 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->admittance_->ik_base_frame_, ik_base_frame_); ASSERT_EQ(controller_->admittance_->ik_tip_frame_, ik_tip_frame_); // ASSERT_EQ(controller_->admittance_->ik_group_name_, ik_group_name_); - ASSERT_EQ(controller_->admittance_->endeffector_frame_, endeffector_frame_); ASSERT_EQ(controller_->admittance_->fixed_world_frame_, fixed_world_frame_); ASSERT_EQ(controller_->admittance_->sensor_frame_, sensor_frame_); @@ -386,14 +381,6 @@ TEST_F(AdmittanceControllerTest, publish_status_success) 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.measured_wrench_endeffector_frame.header.frame_id, endeffector_frame_); - ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.force.x, 0.0); - ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.force.y, 0.0); - ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.force.z, 0.0); - ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.torque.x, 0.0); - ASSERT_EQ(msg.measured_wrench_endeffector_frame.wrench.torque.y, 0.0); - ASSERT_EQ(msg.measured_wrench_endeffector_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)); @@ -461,7 +448,6 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) 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, endeffector_frame_); } From d4f98dd2e6d36ec5fc5a798b24ee40ba392ecee0 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 25 May 2021 15:46:30 -0500 Subject: [PATCH 54/99] Debug calculate_desired_joint_state --- .../admittance_controller/admittance_rule.hpp | 2 +- .../incremental_kinematics.hpp | 4 +- admittance_controller/src/admittance_rule.cpp | 65 +++++++++---------- .../src/incremental_kinematics.cpp | 6 +- 4 files changed, 39 insertions(+), 38 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 2d23561344..5aa883b168 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -105,7 +105,7 @@ class AdmittanceRule std::vector gravity_compensation_params_; protected: - void process_force_measurements( + void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench ); diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index 06e4157017..6b86c64c7e 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -37,12 +37,12 @@ class IncrementalKinematics /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. * \param delta_x_vec input Cartesian deltas (x, y, z, rx, ry, rz) - * \param ik_base_to_tip_tf transformation between ik base and ik tip + * \param control_frame_to_ik_base transform the requested delta_x to MoveIt's ik_base frame * \param delta_theta_vec output vector with joint states * \return true if successful */ bool - convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec); + convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec); /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 16622babfe..31959f0bf2 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -244,7 +244,7 @@ controller_interface::return_type AdmittanceRule::update( } } - process_force_measurements(measured_wrench); + process_wrench_measurements(measured_wrench); calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); @@ -394,41 +394,45 @@ controller_interface::return_type AdmittanceRule::get_pose_of_control_frame_in_b return controller_interface::return_type::OK; } -void AdmittanceRule::process_force_measurements( +void AdmittanceRule::process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench ) { - // get current states, and transform those into controller frame + // TODO(andyz): Implement gravity comp. For now, just pass the measured wrench through measured_wrench_.wrench = measured_wrench; - try { - auto transform = tf_buffer_->lookupTransform(fixed_world_frame_, measured_wrench_.header.frame_id, tf2::TimePointZero); - auto transform_back = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); + measured_wrench_filtered_ = measured_wrench_; - geometry_msgs::msg::WrenchStamped measured_wrench_transformed; - tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform); + // // get current states, and transform those into controller frame + // measured_wrench_.wrench = measured_wrench; + // try { + // auto transform = tf_buffer_->lookupTransform(fixed_world_frame_, measured_wrench_.header.frame_id, tf2::TimePointZero); + // auto transform_back = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); - geometry_msgs::msg::Vector3Stamped cog_transformed; + // geometry_msgs::msg::WrenchStamped measured_wrench_transformed; + // tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform); - for (const auto & params : gravity_compensation_params_) { - auto transform_cog = tf_buffer_->lookupTransform(fixed_world_frame_, params.cog_.header.frame_id, tf2::TimePointZero); - tf2::doTransform(params.cog_, cog_transformed, transform_cog); + // geometry_msgs::msg::Vector3Stamped cog_transformed; + // for (const auto & params : gravity_compensation_params_) { + // auto transform_cog = tf_buffer_->lookupTransform(fixed_world_frame_, params.cog_.header.frame_id, tf2::TimePointZero); + // tf2::doTransform(params.cog_, cog_transformed, transform_cog); - measured_wrench_transformed.wrench.force.z += params.force_; - measured_wrench_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y); - measured_wrench_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); - } + // measured_wrench_transformed.wrench.force.z += params.force_; + // measured_wrench_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y); + // measured_wrench_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); + // } - tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_back); + // tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_back); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_wrench_.header.frame_id + "' or ''."); - // If transform error just use measured force - measured_wrench_filtered_ = measured_wrench_; - } + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), transform.transform.translation.y); + // RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), transform.transform.translation.z); + // } catch (const tf2::TransformException & e) { + // // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + // RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_wrench_.header.frame_id + "' or ''."); + // // If transform error just use measured force + // measured_wrench_filtered_ = measured_wrench_; + // } transform_message_to_control_frame(measured_wrench_filtered_, measured_wrench_control_frame_); - convert_message_to_array(measured_wrench_control_frame_, measured_wrench_control_frame_arr_); // If at least one measured force is nan set all to 0 @@ -458,10 +462,6 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; - - // RCLCPP_INFO(rclcpp::get_logger("AR"), "Rule [%zu]: (%e = %e - D(%.1f)*%e - S(%.1f)*%e)", i, - // acceleration, measured_wrench[i], damping_[i], desired_velocity_arr_[i], - // stiffness_[i], pose_error); } } } @@ -478,20 +478,19 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( transform_ik_tip_to_control_frame(desired_pose_control_frame_.pose, desired_pose_control_frame_.pose); // Calculate desired Cartesian displacement of the robot - // TODO: replace this with FK in the long term - geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::TransformStamped control_frame_to_ik_base; try { - transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + control_frame_to_ik_base = tf_buffer_->lookupTransform(control_frame_, ik_base_frame_, tf2::TimePointZero); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + control_frame_ + "'."); return controller_interface::return_type::ERROR; } // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ + relative_desired_pose_vec, control_frame_to_ik_base, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 267f8cd358..bb3ad6121a 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -33,7 +33,7 @@ IncrementalKinematics::IncrementalKinematics(const std::shared_ptr // By default, the MoveIt Jacobian frame is the last link } -bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec) +bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); @@ -44,7 +44,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorgetJacobian(joint_model_group_); // TODO(andyz): the SVD method would be more stable near singularities From ba04d2be9d3bfc3001c867185c9542261fcd2985 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 25 May 2021 15:55:23 -0500 Subject: [PATCH 55/99] error = target - current --- admittance_controller/src/admittance_rule.cpp | 51 +++---------------- .../src/incremental_kinematics.cpp | 2 - 2 files changed, 7 insertions(+), 46 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 31959f0bf2..e6e6e1e2d8 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -217,8 +217,7 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to control frame -// transform_message_to_control_frame(target_pose, target_pose_control_frame_); - target_pose_control_frame_ = target_pose; + transform_message_to_control_frame(target_pose, target_pose_control_frame_); if (!hardware_state_has_offset_) { get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); @@ -235,12 +234,10 @@ controller_interface::return_type AdmittanceRule::update( std::array pose_error; - // TODO(andy): these errors should be (target - current) for (auto i = 0u; i < 6; ++i) { - pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; + pose_error[i] = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; if (i >= 3) { - pose_error[i] = angles::normalize_angle(current_pose_control_frame_arr_[i] - - target_pose_control_frame_arr_[i]); + pose_error[i] = angles::normalize_angle(target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]); } } @@ -249,45 +246,11 @@ controller_interface::return_type AdmittanceRule::update( calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); - // Do clean conversion to the goal pose using transform and not messing with Euler angles - convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_); - transform_ik_tip_to_endeffector_frame(desired_pose_.pose, desired_pose_.pose); - - // Calculate desired Cartesian displacement of the robot - // TODO: replace this with FK in the long term - geometry_msgs::msg::TransformStamped transform; - try { - transform = tf_buffer_->lookupTransform(ik_tip_frame_, ik_base_frame_, tf2::TimePointZero); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); - return controller_interface::return_type::ERROR; - } - - // Use Jacobian-based IK - std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); - if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_vec, transform, relative_desired_joint_state_vec_)){ - for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { - desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; - desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); -// RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); - } - } else { - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); - desired_joint_state = current_joint_state; - std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); - return controller_interface::return_type::ERROR; - } - - // TODO(anyone: enable this when enabling use of IK directly - // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero); - // tf2::doTransform(desired_pose_, ik_input_pose_, transform); - // ik_input_pose_.pose = transform_endeffector_to_ik_tip_frame(ik_input_pose_); - // std::vector ik_sol = ik_solver_->getPositionIK ( ); ... + RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "wrench: " << measured_wrench_control_frame_arr_[0] << " " << measured_wrench_control_frame_arr_[1]); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "pose_error: " << pose_error[0] << " " << pose_error[1]); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "rel. des. pose: " << relative_desired_pose_arr_[0] << " " << relative_desired_pose_arr_[1]); - return controller_interface::return_type::OK; + return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } // Update from target joint deltas diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index bb3ad6121a..b19de8ed0d 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -69,8 +69,6 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vectorgetJacobian(joint_model_group_); // TODO(andyz): the SVD method would be more stable near singularities From ae2001d8cc5052a18fe140af04a0bf0590399f47 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 25 May 2021 22:12:03 -0500 Subject: [PATCH 56/99] Correct the current pose --- .../include/admittance_controller/admittance_rule.hpp | 2 +- admittance_controller/src/admittance_rule.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 5aa883b168..bf2c2c9a81 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -182,7 +182,7 @@ class AdmittanceRule if (control_frame_ != message_in.header.frame_id) { try { geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - message_in.header.frame_id, control_frame_, tf2::TimePointZero); + control_frame_, message_in.header.frame_id, tf2::TimePointZero); tf2::doTransform(message_in, message_out, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index e6e6e1e2d8..0e9510634b 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -217,7 +217,7 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to control frame - transform_message_to_control_frame(target_pose, target_pose_control_frame_); + transform_message_to_control_frame(target_pose, target_pose_control_frame_); if (!hardware_state_has_offset_) { get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); @@ -236,6 +236,7 @@ controller_interface::return_type AdmittanceRule::update( for (auto i = 0u; i < 6; ++i) { pose_error[i] = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose: " << target_pose_control_frame_arr_[i] << " current_pose: " << current_pose_control_frame_arr_[i]); if (i >= 3) { pose_error[i] = angles::normalize_angle(target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]); } @@ -246,9 +247,7 @@ controller_interface::return_type AdmittanceRule::update( calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); - RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "wrench: " << measured_wrench_control_frame_arr_[0] << " " << measured_wrench_control_frame_arr_[1]); RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "pose_error: " << pose_error[0] << " " << pose_error[1]); - RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "rel. des. pose: " << relative_desired_pose_arr_[0] << " " << relative_desired_pose_arr_[1]); return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } From 4dee49ec6c08a8fda1c1e55c77704d0c33a18e52 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 25 May 2021 23:07:17 -0500 Subject: [PATCH 57/99] Translation seems fine! --- .../admittance_controller/admittance_rule.hpp | 4 +- admittance_controller/src/admittance_rule.cpp | 61 +++++++++++-------- .../src/incremental_kinematics.cpp | 6 +- 3 files changed, 41 insertions(+), 30 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index bf2c2c9a81..6c0ca1c8bd 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -88,9 +88,11 @@ class AdmittanceRule std::string ik_tip_frame_; std::string ik_group_name_; - // Controller frames + // Generally most quantities are transformed to and calculations are done in control_frame std::string control_frame_; + // Gravity points down (neg. Z) in the world frame std::string fixed_world_frame_; + // Frame where wrench measurements are taken std::string sensor_frame_; // Admittance parameters diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 0e9510634b..3f226b9b1e 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -219,6 +219,9 @@ controller_interface::return_type AdmittanceRule::update( // Convert inputs to control frame transform_message_to_control_frame(target_pose, target_pose_control_frame_); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose frame: " << target_pose.header.frame_id); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose: " << target_pose.pose.position.x << " " << target_pose.pose.position.y); + if (!hardware_state_has_offset_) { get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); @@ -238,7 +241,8 @@ controller_interface::return_type AdmittanceRule::update( pose_error[i] = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose: " << target_pose_control_frame_arr_[i] << " current_pose: " << current_pose_control_frame_arr_[i]); if (i >= 3) { - pose_error[i] = angles::normalize_angle(target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]); + //pose_error[i] = angles::normalize_angle(target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]); + pose_error[i] = 0; } } @@ -247,8 +251,6 @@ controller_interface::return_type AdmittanceRule::update( calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); - RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "pose_error: " << pose_error[0] << " " << pose_error[1]); - return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } @@ -263,19 +265,22 @@ controller_interface::return_type AdmittanceRule::update( std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); - // geometry_msgs::msg::TransformStamped transform_ik_base_tip; + // geometry_msgs::msg::TransformStamped transform_ik_base_control_frame; // try { - // transform_ik_base_tip = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero); + // transform_ik_base_control_frame = tf_buffer_->lookupTransform(control_frame_, ik_base_frame_, tf2::TimePointZero); // } catch (const tf2::TransformException & e) { // // TODO(destogl): Use RCLCPP_ERROR_THROTTLE // RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); // return controller_interface::return_type::ERROR; // } - geometry_msgs::msg::TransformStamped transform_ik_base_tip; - transform_ik_base_tip.header.frame_id = ik_base_frame_; - transform_ik_base_tip.transform.rotation.w = 1; - if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_tip, target_ik_tip_deltas_vec)) { + // Get feed-forward cartesian deltas in the ik_base frame. + // Since this is MoveIt's working frame, the transform is identity. + geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; + transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; + transform_ik_base_to_desired_frame.transform.rotation.w = 1; + ik_->update_robot_state(current_joint_state); + if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_to_desired_frame, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); desired_joint_state = current_joint_state; @@ -283,29 +288,33 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - ik_->update_robot_state(current_joint_state); + // Get current robot pose + if (!hardware_state_has_offset_) { + get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); + transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); + } + // TODO(destogl): Can this work properly, when considering offset between states and commands? +// else { +// current_pose_control_frame_ = desired_pose_control_frame_; +// } + + // Add deltas to current pose to get the desired pose + geometry_msgs::msg::PoseStamped target_pose_ik_base_frame = current_pose_base_frame_; + target_pose_ik_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); + target_pose_ik_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); + target_pose_ik_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); - // Get the target pose in ik_base frame - geometry_msgs::msg::PoseStamped target_pose_base_frame; - target_pose_base_frame.header.frame_id = ik_base_frame_; - target_pose_base_frame.pose.position.x = transform_ik_base_tip.transform.translation.x; - target_pose_base_frame.pose.position.y = transform_ik_base_tip.transform.translation.y; - target_pose_base_frame.pose.position.z = transform_ik_base_tip.transform.translation.z; - target_pose_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); - target_pose_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); - target_pose_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); - - tf2::Quaternion q(transform_ik_base_tip.transform.rotation.x, transform_ik_base_tip.transform.rotation.y, transform_ik_base_tip.transform.rotation.z, transform_ik_base_tip.transform.rotation.w); + tf2::Quaternion q(target_pose_ik_base_frame.pose.orientation.x, target_pose_ik_base_frame.pose.orientation.y, target_pose_ik_base_frame.pose.orientation.z, target_pose_ik_base_frame.pose.orientation.z); tf2::Quaternion q_rot; q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); q = q_rot * q; q.normalize(); - target_pose_base_frame.pose.orientation.w = q.w(); - target_pose_base_frame.pose.orientation.x = q.x(); - target_pose_base_frame.pose.orientation.y = q.y(); - target_pose_base_frame.pose.orientation.z = q.z(); + target_pose_ik_base_frame.pose.orientation.w = q.w(); + target_pose_ik_base_frame.pose.orientation.x = q.x(); + target_pose_ik_base_frame.pose.orientation.y = q.y(); + target_pose_ik_base_frame.pose.orientation.z = q.z(); - return update(current_joint_state, measured_wrench, target_pose_base_frame, period, desired_joint_state); + return update(current_joint_state, measured_wrench, target_pose_ik_base_frame, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index b19de8ed0d..cc547e02df 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -82,14 +82,14 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec) +bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_theta = Eigen::Map(&delta_theta_vec[0], delta_theta_vec.size()); // Multiply with the Jacobian to get delta_x jacobian_ = kinematic_state_->getJacobian(joint_model_group_); - // delta_x will be in the working frame of MoveIt + // delta_x will be in the working frame of MoveIt (ik_base frame) Eigen::VectorXd delta_x = jacobian_ * delta_theta; // Transform delta_x to the tip frame @@ -98,7 +98,7 @@ bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(std::vector Date: Tue, 25 May 2021 23:10:27 -0500 Subject: [PATCH 58/99] Delete commented code blocks --- admittance_controller/src/admittance_rule.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 3f226b9b1e..2136a0f7da 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -265,15 +265,6 @@ controller_interface::return_type AdmittanceRule::update( std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); std::vector target_ik_tip_deltas_vec(6); - // geometry_msgs::msg::TransformStamped transform_ik_base_control_frame; - // try { - // transform_ik_base_control_frame = tf_buffer_->lookupTransform(control_frame_, ik_base_frame_, tf2::TimePointZero); - // } catch (const tf2::TransformException & e) { - // // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - // RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + ik_tip_frame_ + "'."); - // return controller_interface::return_type::ERROR; - // } - // Get feed-forward cartesian deltas in the ik_base frame. // Since this is MoveIt's working frame, the transform is identity. geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; From 3e8d6a35cc373f5ef924ca5149f29a78ef152857 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 26 May 2021 10:32:38 -0500 Subject: [PATCH 59/99] Re-enable orientation control (#13) * Small optimization in orientation error calculation. * Re-enable orientation control --- admittance_controller/src/admittance_rule.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 2136a0f7da..abfd14f040 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -219,9 +219,6 @@ controller_interface::return_type AdmittanceRule::update( // Convert inputs to control frame transform_message_to_control_frame(target_pose, target_pose_control_frame_); - RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose frame: " << target_pose.header.frame_id); - RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose: " << target_pose.pose.position.x << " " << target_pose.pose.position.y); - if (!hardware_state_has_offset_) { get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); @@ -239,10 +236,8 @@ controller_interface::return_type AdmittanceRule::update( for (auto i = 0u; i < 6; ++i) { pose_error[i] = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; - RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), "target_pose: " << target_pose_control_frame_arr_[i] << " current_pose: " << current_pose_control_frame_arr_[i]); if (i >= 3) { - //pose_error[i] = angles::normalize_angle(target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]); - pose_error[i] = 0; + pose_error[i] = angles::normalize_angle(pose_error[i]); } } @@ -282,7 +277,6 @@ controller_interface::return_type AdmittanceRule::update( // Get current robot pose if (!hardware_state_has_offset_) { get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); - transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); } // TODO(destogl): Can this work properly, when considering offset between states and commands? // else { @@ -293,9 +287,9 @@ controller_interface::return_type AdmittanceRule::update( geometry_msgs::msg::PoseStamped target_pose_ik_base_frame = current_pose_base_frame_; target_pose_ik_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); target_pose_ik_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); - target_pose_ik_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); + target_pose_ik_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); - tf2::Quaternion q(target_pose_ik_base_frame.pose.orientation.x, target_pose_ik_base_frame.pose.orientation.y, target_pose_ik_base_frame.pose.orientation.z, target_pose_ik_base_frame.pose.orientation.z); + tf2::Quaternion q(target_pose_ik_base_frame.pose.orientation.x, target_pose_ik_base_frame.pose.orientation.y, target_pose_ik_base_frame.pose.orientation.z, target_pose_ik_base_frame.pose.orientation.w); tf2::Quaternion q_rot; q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); q = q_rot * q; From d08000fb9adb50ddb0c49f28aa1f55fef9629018 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 26 May 2021 22:18:13 -0500 Subject: [PATCH 60/99] Flip the "pose error" calculation --- admittance_controller/src/admittance_rule.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index abfd14f040..4ed9a44db3 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -235,7 +235,7 @@ controller_interface::return_type AdmittanceRule::update( std::array pose_error; for (auto i = 0u; i < 6; ++i) { - pose_error[i] = target_pose_control_frame_arr_[i] - current_pose_control_frame_arr_[i]; + pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } From ac73fdc73aa14f1f3847e81a5f057178620552d7 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 27 May 2021 00:00:52 -0500 Subject: [PATCH 61/99] Implement "spring back" behavior. Use inertial reference frame. (#14) * Delete 2 unused variables * First shot at saving a feedforward pose * Do calcs in an inertial frame of reference Delete unused functions * This seems to work great! * Delete 'motion_scale' hacky param --- .../admittance_controller/admittance_rule.hpp | 44 ++++------ admittance_controller/src/admittance_rule.cpp | 88 ++++++++----------- 2 files changed, 50 insertions(+), 82 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 6c0ca1c8bd..4f2a6d4cb1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -84,11 +84,12 @@ class AdmittanceRule bool hardware_state_has_offset_ = false; // IK related parameters + // ik_base_frame should be stationary so vel/accel calculations are correct std::string ik_base_frame_; std::string ik_tip_frame_; std::string ik_group_name_; - // Generally most quantities are transformed to and calculations are done in control_frame + // Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector std::string control_frame_; // Gravity points down (neg. Z) in the world frame std::string fixed_world_frame_; @@ -151,21 +152,21 @@ class AdmittanceRule geometry_msgs::msg::WrenchStamped measured_wrench_control_frame_; geometry_msgs::msg::WrenchStamped measured_wrench_endeffector_frame_; - geometry_msgs::msg::PoseStamped origin_ik_tip_; - geometry_msgs::msg::PoseStamped origin_endeffector_; - geometry_msgs::msg::PoseStamped current_pose_base_frame_; - geometry_msgs::msg::PoseStamped current_pose_control_frame_; + geometry_msgs::msg::PoseStamped current_pose_ik_base_frame_; - geometry_msgs::msg::WrenchStamped target_force_control_frame_; - geometry_msgs::msg::PoseStamped target_pose_control_frame_; + // This is the feedforward pose. Where should the end effector be with no wrench applied? + geometry_msgs::msg::PoseStamped feedforward_pose_ik_base_frame_; - geometry_msgs::msg::PoseStamped desired_pose_control_frame_; + geometry_msgs::msg::WrenchStamped target_force_ik_base_frame_; + geometry_msgs::msg::PoseStamped target_pose_ik_base_frame_; + + geometry_msgs::msg::PoseStamped desired_pose_ik_base_frame_; geometry_msgs::msg::TransformStamped relative_desired_pose_; // Pre-reserved update-loop variables std::array measured_wrench_control_frame_arr_; - std::array target_pose_control_frame_arr_; - std::array current_pose_control_frame_arr_; + std::array target_pose_ik_base_frame_arr_; + std::array current_pose_ik_base_frame_arr_; std::array angles_error_; @@ -179,16 +180,16 @@ class AdmittanceRule private: template controller_interface::return_type - transform_message_to_control_frame(const MsgType & message_in, MsgType & message_out) + transform_message_to_ik_base_frame(const MsgType & message_in, MsgType & message_out) { - if (control_frame_ != message_in.header.frame_id) { + if (ik_base_frame_ != message_in.header.frame_id) { try { geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - control_frame_, message_in.header.frame_id, tf2::TimePointZero); + ik_base_frame_, message_in.header.frame_id, tf2::TimePointZero); tf2::doTransform(message_in, message_out, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + message_in.header.frame_id + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + message_in.header.frame_id + "'."); return controller_interface::return_type::ERROR; } } else { @@ -208,21 +209,6 @@ class AdmittanceRule output_tf = input_tf * transform; tf2::toMsg(output_tf, output); } - - template - void - transform_ik_tip_to_control_frame(const Type & base_to_ik_tip, Type & base_to_toollink) - { - direct_transform(base_to_ik_tip, ik_tip_to_control_frame_tf_, base_to_toollink); - } - - template - void - transform_control_to_ik_tip_frame(const Type & base_to_toollink, Type & base_to_ik_tip) - { - direct_transform(base_to_toollink, control_frame_to_ik_tip_tf_, base_to_ik_tip); - } - }; } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 4ed9a44db3..4e085b9685 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -173,15 +173,17 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize IK ik_ = std::make_shared(node, ik_group_name_); + reset(); + return controller_interface::return_type::OK; } controller_interface::return_type AdmittanceRule::reset() { measured_wrench_control_frame_arr_.fill(0.0); - target_pose_control_frame_arr_.fill(0.0); + target_pose_ik_base_frame_arr_.fill(0.0); - current_pose_control_frame_arr_.fill(0.0); + current_pose_ik_base_frame_arr_.fill(0.0); angles_error_.fill(0.0); @@ -189,7 +191,8 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); - get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); + get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); + feedforward_pose_ik_base_frame_ = current_pose_ik_base_frame_; // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; @@ -216,26 +219,22 @@ controller_interface::return_type AdmittanceRule::update( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - // Convert inputs to control frame - transform_message_to_control_frame(target_pose, target_pose_control_frame_); + // Convert inputs to ik_base frame (assumed stationary) + transform_message_to_ik_base_frame(target_pose, target_pose_ik_base_frame_); + // TODO(andyz): what if there is a hardware offset? if (!hardware_state_has_offset_) { - get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); - transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); + get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); } - // TODO(destogl): Can this work properly, when considering offset between states and commands? -// else { -// current_pose_control_frame_ = desired_pose_control_frame_; -// } // Convert all data to arrays for simpler calculation - convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_arr_); - convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); + convert_message_to_array(target_pose_ik_base_frame_, target_pose_ik_base_frame_arr_); + convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); std::array pose_error; for (auto i = 0u; i < 6; ++i) { - pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; + pose_error[i] = current_pose_ik_base_frame_arr_[i] - target_pose_ik_base_frame_arr_[i]; if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } @@ -261,7 +260,7 @@ controller_interface::return_type AdmittanceRule::update( std::vector target_ik_tip_deltas_vec(6); // Get feed-forward cartesian deltas in the ik_base frame. - // Since this is MoveIt's working frame, the transform is identity. + // Since ik_base is MoveIt's working frame, the transform is identity. geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; transform_ik_base_to_desired_frame.transform.rotation.w = 1; @@ -274,32 +273,22 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // Get current robot pose - if (!hardware_state_has_offset_) { - get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); - } - // TODO(destogl): Can this work properly, when considering offset between states and commands? -// else { -// current_pose_control_frame_ = desired_pose_control_frame_; -// } - - // Add deltas to current pose to get the desired pose - geometry_msgs::msg::PoseStamped target_pose_ik_base_frame = current_pose_base_frame_; - target_pose_ik_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); - target_pose_ik_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); - target_pose_ik_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); - - tf2::Quaternion q(target_pose_ik_base_frame.pose.orientation.x, target_pose_ik_base_frame.pose.orientation.y, target_pose_ik_base_frame.pose.orientation.z, target_pose_ik_base_frame.pose.orientation.w); + // Add deltas to previously-desired pose to get the next desired pose + feedforward_pose_ik_base_frame_.pose.position.x += target_ik_tip_deltas_vec.at(0); + feedforward_pose_ik_base_frame_.pose.position.y += target_ik_tip_deltas_vec.at(1); + feedforward_pose_ik_base_frame_.pose.position.z += target_ik_tip_deltas_vec.at(2); + + tf2::Quaternion q(feedforward_pose_ik_base_frame_.pose.orientation.x, feedforward_pose_ik_base_frame_.pose.orientation.y, feedforward_pose_ik_base_frame_.pose.orientation.z, feedforward_pose_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); q = q_rot * q; q.normalize(); - target_pose_ik_base_frame.pose.orientation.w = q.w(); - target_pose_ik_base_frame.pose.orientation.x = q.x(); - target_pose_ik_base_frame.pose.orientation.y = q.y(); - target_pose_ik_base_frame.pose.orientation.z = q.z(); + feedforward_pose_ik_base_frame_.pose.orientation.w = q.w(); + feedforward_pose_ik_base_frame_.pose.orientation.x = q.x(); + feedforward_pose_ik_base_frame_.pose.orientation.y = q.y(); + feedforward_pose_ik_base_frame_.pose.orientation.z = q.z(); - return update(current_joint_state, measured_wrench, target_pose_ik_base_frame, period, desired_joint_state); + return update(current_joint_state, measured_wrench, feedforward_pose_ik_base_frame_, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( @@ -312,7 +301,7 @@ controller_interface::return_type AdmittanceRule::update( ) { // TODO(destogl): Implement this update - // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); + // transform_message_to_ik_base_frame(**input_force_cmd, force_cmd_ctrl_frame_); // TODO(destogl) reuse things from other update return controller_interface::return_type::OK; @@ -322,11 +311,12 @@ controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message) { // state_message.input_force_control_frame = target_force_control_frame_; - state_message.input_pose_control_frame = target_pose_control_frame_; + // TODO(andyz): update msg fields +// state_message.input_pose_control_frame = target_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_control_frame_; - state_message.desired_pose = desired_pose_control_frame_; + state_message.desired_pose = desired_pose_ik_base_frame_; state_message.relative_desired_pose = relative_desired_pose_; return controller_interface::return_type::OK; @@ -388,7 +378,7 @@ void AdmittanceRule::process_wrench_measurements( // measured_wrench_filtered_ = measured_wrench_; // } - transform_message_to_control_frame(measured_wrench_filtered_, measured_wrench_control_frame_); + transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_control_frame_); convert_message_to_array(measured_wrench_control_frame_, measured_wrench_control_frame_arr_); // If at least one measured force is nan set all to 0 @@ -428,25 +418,17 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - tf2::doTransform(current_pose_control_frame_, desired_pose_control_frame_, relative_desired_pose_); - transform_ik_tip_to_control_frame(desired_pose_control_frame_.pose, desired_pose_control_frame_.pose); - // Calculate desired Cartesian displacement of the robot - geometry_msgs::msg::TransformStamped control_frame_to_ik_base; - try { - control_frame_to_ik_base = tf_buffer_->lookupTransform(control_frame_, ik_base_frame_, tf2::TimePointZero); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + control_frame_ + "'."); - return controller_interface::return_type::ERROR; - } + // Since ik_base is MoveIt's working frame, the transform is identity. + geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; + transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; + transform_ik_base_to_desired_frame.transform.rotation.w = 1; // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_vec, control_frame_to_ik_base, relative_desired_joint_state_vec_)){ + relative_desired_pose_vec, transform_ik_base_to_desired_frame, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); From 0200ecdcea979c617d00f0c1b134d0204db7d340 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 27 May 2021 12:20:56 -0500 Subject: [PATCH 62/99] Pre-allocate an identity transform --- .../admittance_controller/admittance_rule.hpp | 3 +++ admittance_controller/src/admittance_rule.cpp | 14 ++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 4f2a6d4cb1..fd23124ed1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -96,6 +96,9 @@ class AdmittanceRule // Frame where wrench measurements are taken std::string sensor_frame_; + // An identity matrix is needed in several places + geometry_msgs::msg::TransformStamped identity_transform_; + // Admittance parameters // TODO(destogl): unified mode does not have to be here bool unified_mode_ = false; // Unified mode enables simultaneous force and position goals diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 4e085b9685..ace8e904b9 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -168,6 +168,8 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared relative_desired_pose_.header.frame_id = control_frame_; + identity_transform_.transform.rotation.w = 1; + relative_desired_joint_state_vec_.reserve(6); // Initialize IK @@ -261,11 +263,9 @@ controller_interface::return_type AdmittanceRule::update( // Get feed-forward cartesian deltas in the ik_base frame. // Since ik_base is MoveIt's working frame, the transform is identity. - geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; - transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; - transform_ik_base_to_desired_frame.transform.rotation.w = 1; + identity_transform_.header.frame_id = ik_base_frame_; ik_->update_robot_state(current_joint_state); - if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, transform_ik_base_to_desired_frame, target_ik_tip_deltas_vec)) { + if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, identity_transform_, target_ik_tip_deltas_vec)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); desired_joint_state = current_joint_state; @@ -421,14 +421,12 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); // Since ik_base is MoveIt's working frame, the transform is identity. - geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; - transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; - transform_ik_base_to_desired_frame.transform.rotation.w = 1; + identity_transform_.header.frame_id = ik_base_frame_; // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_vec, transform_ik_base_to_desired_frame, relative_desired_joint_state_vec_)){ + relative_desired_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); From d8ffba05ca77c02a435dd4a5b8864af8c6a139b8 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 28 May 2021 07:59:56 -0500 Subject: [PATCH 63/99] Add feedforward acceleration term (#15) --- .../admittance_controller/admittance_rule.hpp | 4 ++++ admittance_controller/src/admittance_rule.cpp | 18 ++++++++++++++---- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index fd23124ed1..0470ab6c65 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -121,6 +121,7 @@ class AdmittanceRule void calculate_admittance_rule( const std::array & measured_wrench, const std::array & pose_error, + const std::array & feedforward_acceleration, const rclcpp::Duration & period, std::array & desired_relative_pose ); @@ -159,6 +160,9 @@ class AdmittanceRule // This is the feedforward pose. Where should the end effector be with no wrench applied? geometry_msgs::msg::PoseStamped feedforward_pose_ik_base_frame_; + std::array feedforward_velocity_ik_base_frame_; + // Need to save the previous velocity to calculate acceleration + std::array prev_feedforward_velocity_ik_base_frame_; geometry_msgs::msg::WrenchStamped target_force_ik_base_frame_; geometry_msgs::msg::PoseStamped target_pose_ik_base_frame_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index ace8e904b9..010bd56d16 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -234,17 +234,23 @@ controller_interface::return_type AdmittanceRule::update( convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); std::array pose_error; + // Estimate feedforward acceleration from target_pose_ik_base_frame_arr_ and previous + std::array feedforward_acceleration; for (auto i = 0u; i < 6; ++i) { pose_error[i] = current_pose_ik_base_frame_arr_[i] - target_pose_ik_base_frame_arr_[i]; if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } + + // Estimate feedforward acceleration + feedforward_acceleration[i] = (feedforward_velocity_ik_base_frame_[i] - prev_feedforward_velocity_ik_base_frame_[i]) / period.seconds(); } + prev_feedforward_velocity_ik_base_frame_ = feedforward_velocity_ik_base_frame_; process_wrench_measurements(measured_wrench); - calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, period, + calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, feedforward_acceleration, period, relative_desired_pose_arr_); return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); @@ -273,6 +279,10 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } + for (auto i = 0u; i < 6; ++i) { + feedforward_velocity_ik_base_frame_[i] = target_ik_tip_deltas_vec.at(i) / period.seconds(); + } + // Add deltas to previously-desired pose to get the next desired pose feedforward_pose_ik_base_frame_.pose.position.x += target_ik_tip_deltas_vec.at(0); feedforward_pose_ik_base_frame_.pose.position.y += target_ik_tip_deltas_vec.at(1); @@ -390,6 +400,7 @@ void AdmittanceRule::process_wrench_measurements( void AdmittanceRule::calculate_admittance_rule( const std::array & measured_wrench, const std::array & pose_error, + const std::array & feedforward_acceleration, const rclcpp::Duration & period, std::array & desired_relative_pose ) @@ -398,9 +409,8 @@ void AdmittanceRule::calculate_admittance_rule( for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { // TODO(destogl): check if velocity is measured from hardware - const double acceleration = 1 / mass_[i] * - (measured_wrench[i] - damping_[i] * desired_velocity_arr_[i] - - stiffness_[i] * pose_error[i]); + const double acceleration = feedforward_acceleration[i] + (1 / mass_[i]) * + (measured_wrench[i] - damping_[i] * desired_velocity_arr_[i] - stiffness_[i] * pose_error[i]); desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); From 59518db7c7ea578e68c5a75565f08cd1f45486c3 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 28 May 2021 08:04:30 -0500 Subject: [PATCH 64/99] configure() admittance differently. Fixes "XBox first" bug --- admittance_controller/src/admittance_rule.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 010bd56d16..2ef5c38672 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -175,7 +175,16 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize IK ik_ = std::make_shared(node, ik_group_name_); - reset(); + measured_wrench_control_frame_arr_.fill(0.0); + target_pose_ik_base_frame_arr_.fill(0.0); + current_pose_ik_base_frame_arr_.fill(0.0); + angles_error_.fill(0.0); + desired_velocity_arr_.fill(0.0); + desired_velocity_previous_arr_.fill(0.0); + desired_acceleration_previous_arr_.fill(0.0); + + get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); + feedforward_pose_ik_base_frame_ = current_pose_ik_base_frame_; return controller_interface::return_type::OK; } @@ -184,11 +193,8 @@ controller_interface::return_type AdmittanceRule::reset() { measured_wrench_control_frame_arr_.fill(0.0); target_pose_ik_base_frame_arr_.fill(0.0); - current_pose_ik_base_frame_arr_.fill(0.0); - angles_error_.fill(0.0); - desired_velocity_arr_.fill(0.0); desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); From cb9facc6cad05a5bf5b4fdb3bbe16b43e5b2ec57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 30 May 2021 09:40:09 +0200 Subject: [PATCH 65/99] Revert endeffector frame to publish forces in endeffector frame --- .../admittance_controller/admittance_rule.hpp | 2 ++ .../src/admittance_controller.cpp | 2 ++ admittance_controller/src/admittance_rule.cpp | 34 +++++++++++-------- 3 files changed, 24 insertions(+), 14 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0470ab6c65..a4abe6b582 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -89,6 +89,8 @@ class AdmittanceRule std::string ik_tip_frame_; std::string ik_group_name_; + // Frame which position should be controlled + std::string endeffector_frame_; // Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector std::string control_frame_; // Gravity points down (neg. Z) in the world frame diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index e242b2ce07..9fbfd4945a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -57,6 +57,7 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("IK.group_name", ""); get_node()->declare_parameter("control_frame", ""); + get_node()->declare_parameter("endeffector_frame", ""); get_node()->declare_parameter("fixed_world_frame", ""); get_node()->declare_parameter("sensor_frame", ""); @@ -165,6 +166,7 @@ CallbackReturn AdmittanceController::on_configure( get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || + get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") || get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") || get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") || get_string_param_and_error_if_empty(admittance_->sensor_frame_, "sensor_frame") || diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 2ef5c38672..8e2e6cdc58 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -175,17 +175,6 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize IK ik_ = std::make_shared(node, ik_group_name_); - measured_wrench_control_frame_arr_.fill(0.0); - target_pose_ik_base_frame_arr_.fill(0.0); - current_pose_ik_base_frame_arr_.fill(0.0); - angles_error_.fill(0.0); - desired_velocity_arr_.fill(0.0); - desired_velocity_previous_arr_.fill(0.0); - desired_acceleration_previous_arr_.fill(0.0); - - get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); - feedforward_pose_ik_base_frame_ = current_pose_ik_base_frame_; - return controller_interface::return_type::OK; } @@ -259,6 +248,10 @@ controller_interface::return_type AdmittanceRule::update( calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, feedforward_acceleration, period, relative_desired_pose_arr_); + // Do clean conversion to the goal pose using transform and not messing with Euler angles + convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); + tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); + return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } @@ -326,12 +319,22 @@ controller_interface::return_type AdmittanceRule::update( controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message) { - // state_message.input_force_control_frame = target_force_control_frame_; - // TODO(andyz): update msg fields -// state_message.input_pose_control_frame = target_pose_ik_base_frame_; + // state_message.input_wrench_control_frame = target_wrench_control_frame_; + state_message.input_pose_control_frame = target_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_control_frame_; + + try { + auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); + tf2::doTransform(measured_wrench_control_frame_, measured_wrench_endeffector_frame_, transform); + } catch (const tf2::TransformException & e) { + // TODO(destogl): Use RCLCPP_ERROR_THROTTLE + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + endeffector_frame_ + "'."); + } + + state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; + state_message.desired_pose = desired_pose_ik_base_frame_; state_message.relative_desired_pose = relative_desired_pose_; @@ -424,6 +427,9 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; + +// RCLCPP_INFO(rclcpp::get_logger("AR"), "%e = %e - (1/M(%.1f)) (%e - D(%.1f)*%e - S(%.1f)*%e)", +// acceleration, feedforward_acceleration[i], mass_[i], measured_wrench[i], damping_[i], desired_velocity_arr_[i], stiffness_[i], pose_error[i]); } } } From 313eabe6fda58e29af972d2223432d8328fdd93d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 30 May 2021 12:13:46 +0200 Subject: [PATCH 66/99] Enable support of NaN values on command interface --- .../admittance_controller.hpp | 5 +- .../admittance_controller/admittance_rule.hpp | 2 +- .../src/admittance_controller.cpp | 144 +++++++++++++++--- admittance_controller/src/admittance_rule.cpp | 4 +- 4 files changed, 133 insertions(+), 22 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 46b8bd1d77..f034fddad5 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -76,7 +76,7 @@ class AdmittanceController : public controller_interface::ControllerInterface bool use_joint_commands_as_input_; bool hardware_state_has_offset_; - trajectory_msgs::msg::JointTrajectoryPoint current_state_when_offset_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; // Internal variables std::unique_ptr force_torque_sensor_; @@ -131,6 +131,9 @@ class AdmittanceController : public controller_interface::ControllerInterface bool has_position_command_interface_ = false; bool has_velocity_command_interface_ = false; + void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state); + + bool read_state_from_command_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 a4abe6b582..e6c7acf79e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -81,7 +81,7 @@ class AdmittanceRule controller_interface::return_type get_pose_of_control_frame_in_base_frame(geometry_msgs::msg::PoseStamped & pose); public: - bool hardware_state_has_offset_ = false; + bool open_loop_control_ = false; // IK related parameters // ik_base_frame should be stationary so vel/accel calculations are correct diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 9fbfd4945a..9182f2f950 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -43,12 +43,13 @@ controller_interface::return_type AdmittanceController::init(const std::string & } try { + // TODO: use variables as parameters get_node()->declare_parameter>("joints", {}); get_node()->declare_parameter>("command_interfaces", {}); get_node()->declare_parameter>("state_interfaces", {}); get_node()->declare_parameter("ft_sensor_name", ""); get_node()->declare_parameter("use_joint_commands_as_input", false); - get_node()->declare_parameter("hardware_state_has_offset", false); + get_node()->declare_parameter("open_loop_control", false); get_node()->declare_parameter("IK.base", ""); get_node()->declare_parameter("IK.tip", ""); @@ -162,7 +163,7 @@ CallbackReturn AdmittanceController::on_configure( get_string_array_param_and_error_if_empty(state_interface_types_, "state_interfaces") || get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || get_bool_param_and_error_if_empty(use_joint_commands_as_input_, "use_joint_commands_as_input") || - get_bool_param_and_error_if_empty(admittance_->hardware_state_has_offset_, "hardware_state_has_offset") || + get_bool_param_and_error_if_empty(admittance_->open_loop_control_, "open_loop_control") || get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || @@ -320,12 +321,14 @@ CallbackReturn AdmittanceController::on_configure( "~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(s_publisher_); + auto num_joints = joint_names_.size(); + // Initialize state message state_publisher_->lock(); state_publisher_->msg_.joint_names = joint_names_; - state_publisher_->msg_.actual_joint_states.positions.resize(6, 0.0); - state_publisher_->msg_.desired_joint_states.positions.resize(6, 0.0); - state_publisher_->msg_.error_joint_state.positions.resize(6, 0.0); + state_publisher_->msg_.actual_joint_states.positions.resize(num_joints, 0.0); + state_publisher_->msg_.desired_joint_states.positions.resize(num_joints, 0.0); + state_publisher_->msg_.error_joint_state.positions.resize(num_joints, 0.0); state_publisher_->unlock(); // Configure AdmittanceRule @@ -346,8 +349,10 @@ CallbackReturn AdmittanceController::on_configure( } } - current_state_when_offset_.positions.reserve(joint_names_.size()); - current_state_when_offset_.velocities.reserve(joint_names_.size()); + last_commanded_state_.positions.reserve(num_joints); + // TODO(destogl): Use reserve instead of resize? + last_commanded_state_.velocities.resize(num_joints, 0.0); + last_commanded_state_.accelerations.resize(num_joints, 0.0); if (use_joint_commands_as_input_) { RCLCPP_INFO_STREAM(get_node()->get_logger(), "Using Joint input mode."); @@ -395,6 +400,8 @@ const CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + const auto num_joints = joint_names_.size(); + // order all joints in the storage for (const auto & interface : command_interface_types_) { auto it = std::find( @@ -405,7 +412,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & { RCLCPP_ERROR( node_->get_logger(), "Expected %u '%s' command interfaces, got %u.", - joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + num_joints, interface.c_str(), joint_command_interface_[index].size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } @@ -418,7 +425,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & { RCLCPP_ERROR( node_->get_logger(), "Expected %u '%s' state interfaces, got %u.", - joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); + num_joints, interface.c_str(), joint_state_interface_[index].size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } @@ -430,8 +437,16 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & admittance_->reset(); previous_time_ = get_node()->now(); - for (auto index = 0u; index < joint_names_.size(); ++index) { - current_state_when_offset_.positions[index] = joint_state_interface_[0][index].get().get_value(); + read_state_from_hardware(last_commanded_state_); + // Handle restart of controller by reading last_commanded_state_ from commands if + // those are not nan + // TODO(destogl): remove memory allocation because it is not real-time safe + trajectory_msgs::msg::JointTrajectoryPoint state; + state.positions.reserve(num_joints); + state.velocities.resize(num_joints, 0.0); + state.accelerations.resize(num_joints, 0.0); + if (read_state_from_command_interfaces(state)) { + last_commanded_state_ = state; } // Set initial command values - initialize all to simplify update @@ -443,7 +458,6 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & msg_joint->joint_names = joint_names_; msg_joint->points.reserve(1); - const auto num_joints = joint_names_.size(); trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; trajectory_point.positions.reserve(num_joints); trajectory_point.velocities.resize(num_joints, 0.0); @@ -451,6 +465,8 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & trajectory_point.positions.emplace_back(joint_state_interface_[0][index].get().get_value()); } msg_joint->points.emplace_back(trajectory_point); + + msg_joint->points.emplace_back(last_commanded_state_); input_joint_command_.writeFromNonRT(msg_joint); std::shared_ptr msg_pose = std::make_shared(); @@ -496,13 +512,11 @@ controller_interface::return_type AdmittanceController::update() desired_joint_states.positions.resize(num_joints); desired_joint_states.velocities.resize(num_joints); + read_state_from_hardware(current_joint_states); - for (auto index = 0u; index < num_joints; ++index) { - if (!admittance_->hardware_state_has_offset_) { - current_joint_states.positions[index] = joint_state_interface_[0][index].get().get_value(); - } else { - current_joint_states.positions[index] = current_state_when_offset_.positions[index]; - } + if (admittance_->open_loop_control_) { + // TODO(destogl): This may not work in every case. Please add checking which states are available and which not! + current_joint_states = last_commanded_state_; } auto ft_values = force_torque_sensor_->get_values_as_message(); @@ -546,7 +560,7 @@ controller_interface::return_type AdmittanceController::update() joint_command_interface_[1][index].get().set_value(desired_joint_states.velocities[index]); } } - current_state_when_offset_ = desired_joint_states; + last_commanded_state_ = desired_joint_states; // Publish controller state state_publisher_->lock(); @@ -566,6 +580,98 @@ controller_interface::return_type AdmittanceController::update() return controller_interface::return_type::OK; } +void AdmittanceController::read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state) +{ + const auto joint_num = joint_names_.size(); + auto assign_point_from_interface = [&, joint_num]( + std::vector & trajectory_point_interface, const auto & joint_inteface) + { + for (auto index = 0ul; index < joint_num; ++index) { + trajectory_point_interface[index] = joint_inteface[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_point_from_interface(state.velocities, joint_state_interface_[1]); + // Acceleration is used only in combination with velocity + // TODO(destogl): enable acceleration and remove next line + state.accelerations.clear(); +// 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(); +// } + } else { + // Make empty so the property is ignored during interpolation + state.velocities.clear(); + state.accelerations.clear(); + } +} + +bool AdmittanceController::read_state_from_command_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint & state) +{ + bool has_values = true; + + const auto joint_num = joint_names_.size(); + auto assign_point_from_interface = [&, joint_num]( + std::vector & trajectory_point_interface, const auto & joint_inteface) + { + for (auto index = 0ul; index < joint_num; ++index) { + trajectory_point_interface[index] = joint_inteface[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(); + }; + + // 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_point_from_interface(state.velocities, joint_command_interface_[1]); + } else { + state.velocities.clear(); + has_values = false; + } + } + else { + state.velocities.clear(); + } + +// TODO(destogl): Enable this +// // 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(); +// } + + return has_values; +} + } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp" diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 8e2e6cdc58..06c21bdbea 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -220,7 +220,7 @@ controller_interface::return_type AdmittanceRule::update( transform_message_to_ik_base_frame(target_pose, target_pose_ik_base_frame_); // TODO(andyz): what if there is a hardware offset? - if (!hardware_state_has_offset_) { + if (!open_loop_control_) { get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); } @@ -283,6 +283,7 @@ controller_interface::return_type AdmittanceRule::update( } // Add deltas to previously-desired pose to get the next desired pose + // TODO: Use convert_to_array method feedforward_pose_ik_base_frame_.pose.position.x += target_ik_tip_deltas_vec.at(0); feedforward_pose_ik_base_frame_.pose.position.y += target_ik_tip_deltas_vec.at(1); feedforward_pose_ik_base_frame_.pose.position.z += target_ik_tip_deltas_vec.at(2); @@ -335,6 +336,7 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; + state_message.current_pose = current_pose_ik_base_frame_; state_message.desired_pose = desired_pose_ik_base_frame_; state_message.relative_desired_pose = relative_desired_pose_; From 2e0f7fdaac14993d2871bea963028d361e42fc80 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 30 May 2021 12:32:54 +0200 Subject: [PATCH 67/99] Add JointDeltas output to see what is happening before XBox is enabled --- admittance_controller/src/admittance_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 9182f2f950..50c096f555 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -544,6 +544,11 @@ controller_interface::return_type AdmittanceController::update() joint_deltas[index] = angles::shortest_angular_distance(current_joint_states.positions[index], (*input_joint_cmd)->points[0].positions[index]); } } + RCLCPP_INFO(get_node()->get_logger(), "JointDeltas: [%e, %e, %e, %e, %e, %e]", + joint_deltas[0], joint_deltas[1], joint_deltas[2], joint_deltas[3], + joint_deltas[4], joint_deltas[5] + ); + admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); } else { admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_states); From 2611ad509e6fa9a0d8c771bf4b552e40f831aee8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 30 May 2021 12:47:13 +0200 Subject: [PATCH 68/99] Squash "not-moving before XBox" bug --- .../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 50c096f555..4d4f02ccb4 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -461,9 +461,10 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; 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_interface_[0][index].get().get_value()); - } + // TODO(destogl): ATTENTION: This does not work properly, so using velocity mode and commenting positions out! +// for (auto index = 0u; index < num_joints; ++index) { +// trajectory_point.positions.emplace_back(joint_state_interface_[0][index].get().get_value()); +// } msg_joint->points.emplace_back(trajectory_point); msg_joint->points.emplace_back(last_commanded_state_); @@ -530,7 +531,7 @@ controller_interface::return_type AdmittanceController::update() // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) // TODO(andyz): this should be optional - if (true) { + if (use_joint_commands_as_input_) { std::array joint_deltas; // If there are no positions, expect velocities // TODO(destogl): add error handling @@ -540,14 +541,15 @@ controller_interface::return_type AdmittanceController::update() } } else { for (auto index = 0u; index < num_joints; ++index) { + // TODO(destogl): ATTENTION: This does not work properly, deltas are getting neutralized and robot is not moving on external forces // TODO(anyone): Is here OK to use shortest_angular_distance? joint_deltas[index] = angles::shortest_angular_distance(current_joint_states.positions[index], (*input_joint_cmd)->points[0].positions[index]); } } - RCLCPP_INFO(get_node()->get_logger(), "JointDeltas: [%e, %e, %e, %e, %e, %e]", - joint_deltas[0], joint_deltas[1], joint_deltas[2], joint_deltas[3], - joint_deltas[4], joint_deltas[5] - ); +// RCLCPP_INFO(get_node()->get_logger(), "JointDeltas: [%e, %e, %e, %e, %e, %e]", +// joint_deltas[0], joint_deltas[1], joint_deltas[2], joint_deltas[3], +// joint_deltas[4], joint_deltas[5] +// ); admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); } else { From 404bcc925625470d62fff04cb71a88961fc5428f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 1 Jun 2021 21:11:51 +0200 Subject: [PATCH 69/99] Enable open loop control (#16) * Added support for open-loop-control * Reduce output and add admittance rule results to the state message --- .../admittance_controller.hpp | 6 ++ .../admittance_controller/admittance_rule.hpp | 9 ++- .../src/admittance_controller.cpp | 62 +++++++++--------- admittance_controller/src/admittance_rule.cpp | 65 ++++++++++++------- .../test/test_admittance_controller.cpp | 6 +- 5 files changed, 91 insertions(+), 57 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index f034fddad5..80b4fa1bf4 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -133,6 +133,12 @@ class AdmittanceController : public controller_interface::ControllerInterface void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state); + /// Use values on command interfaces as states when robot should be controller in open-loop. + /** + * If velocities and positions are both available from the joint command interface, set + * output_state equal to them. + * If velocities or positions are unknown, output_state is unchanged and the function returns false. + */ bool read_state_from_command_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 e6c7acf79e..9e275962d3 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -186,6 +186,12 @@ class AdmittanceRule std::vector relative_desired_joint_state_vec_; + // TODO(destogl): find out better datatype for this + // Values calculated by admittance rule (Cartesian space: [x, y, z, rx, ry, rz]) - state output + // "positions" hold "pose_error" values + // "effort" hold "measured_wrench" values + trajectory_msgs::msg::JointTrajectoryPoint admittance_rule_calculated_values_; + private: template controller_interface::return_type @@ -198,7 +204,8 @@ class AdmittanceRule tf2::doTransform(message_in, message_out, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + message_in.header.frame_id + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + + message_in.header.frame_id + "' to '" + ik_base_frame_ + "'."); return controller_interface::return_type::ERROR; } } else { diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 4d4f02ccb4..15aa692009 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -326,8 +326,8 @@ CallbackReturn AdmittanceController::on_configure( // Initialize state message state_publisher_->lock(); state_publisher_->msg_.joint_names = joint_names_; - state_publisher_->msg_.actual_joint_states.positions.resize(num_joints, 0.0); - state_publisher_->msg_.desired_joint_states.positions.resize(num_joints, 0.0); + 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(); @@ -349,8 +349,8 @@ CallbackReturn AdmittanceController::on_configure( } } - last_commanded_state_.positions.reserve(num_joints); // TODO(destogl): Use reserve instead of resize? + last_commanded_state_.positions.resize(num_joints); last_commanded_state_.velocities.resize(num_joints, 0.0); last_commanded_state_.accelerations.resize(num_joints, 0.0); @@ -438,16 +438,8 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & previous_time_ = get_node()->now(); read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading last_commanded_state_ from commands if - // those are not nan - // TODO(destogl): remove memory allocation because it is not real-time safe - trajectory_msgs::msg::JointTrajectoryPoint state; - state.positions.reserve(num_joints); - state.velocities.resize(num_joints, 0.0); - state.accelerations.resize(num_joints, 0.0); - if (read_state_from_command_interfaces(state)) { - last_commanded_state_ = state; - } + // Handle restart of controller by reading last_commanded_state_ from commands if not nan + read_state_from_command_interfaces(last_commanded_state_); // Set initial command values - initialize all to simplify update std::shared_ptr msg_wrench = std::make_shared(); @@ -461,13 +453,12 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; trajectory_point.positions.reserve(num_joints); trajectory_point.velocities.resize(num_joints, 0.0); - // TODO(destogl): ATTENTION: This does not work properly, so using velocity mode and commenting positions out! + // FIXME(destogl): ATTENTION: This does not work properly, so using velocity mode and commenting positions out! // for (auto index = 0u; index < num_joints; ++index) { // trajectory_point.positions.emplace_back(joint_state_interface_[0][index].get().get_value()); // } msg_joint->points.emplace_back(trajectory_point); - msg_joint->points.emplace_back(last_commanded_state_); input_joint_command_.writeFromNonRT(msg_joint); std::shared_ptr msg_pose = std::make_shared(); @@ -516,7 +507,8 @@ controller_interface::return_type AdmittanceController::update() read_state_from_hardware(current_joint_states); if (admittance_->open_loop_control_) { - // TODO(destogl): This may not work in every case. Please add checking which states are available and which not! + // TODO(destogl): This may not work in every case. + // Please add checking which states are available and which not! current_joint_states = last_commanded_state_; } @@ -530,7 +522,6 @@ controller_interface::return_type AdmittanceController::update() // } else { // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) - // TODO(andyz): this should be optional if (use_joint_commands_as_input_) { std::array joint_deltas; // If there are no positions, expect velocities @@ -575,8 +566,8 @@ controller_interface::return_type AdmittanceController::update() state_publisher_->msg_.input_pose_command = **input_pose_cmd; state_publisher_->msg_.input_joint_command = **input_joint_cmd; - state_publisher_->msg_.desired_joint_states = desired_joint_states; - state_publisher_->msg_.actual_joint_states = current_joint_states; + state_publisher_->msg_.desired_joint_state = desired_joint_states; + state_publisher_->msg_.actual_joint_state = current_joint_states; for (auto index = 0u; index < num_joints; ++index) { state_publisher_->msg_.error_joint_state.positions[index] = angles::shortest_angular_distance( current_joint_states.positions[index], desired_joint_states.positions[index]); @@ -587,14 +578,15 @@ controller_interface::return_type AdmittanceController::update() return controller_interface::return_type::OK; } -void AdmittanceController::read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state) +void AdmittanceController::read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state) { - const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = [&, joint_num]( - std::vector & trajectory_point_interface, const auto & joint_inteface) + const auto num_joints = joint_names_.size(); + auto assign_point_from_interface = [&, num_joints]( + std::vector & trajectory_point_interface, const auto & joint_interface) { - for (auto index = 0ul; index < joint_num; ++index) { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); + for (auto index = 0ul; index < num_joints; ++index) { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; @@ -621,16 +613,17 @@ void AdmittanceController::read_state_from_hardware(trajectory_msgs::msg::JointT } bool AdmittanceController::read_state_from_command_interfaces( - trajectory_msgs::msg::JointTrajectoryPoint & state) + trajectory_msgs::msg::JointTrajectoryPoint & output_state) { bool has_values = true; + const auto num_joints = joint_names_.size(); + trajectory_msgs::msg::JointTrajectoryPoint state = output_state; - const auto joint_num = joint_names_.size(); - auto assign_point_from_interface = [&, joint_num]( - std::vector & trajectory_point_interface, const auto & joint_inteface) + auto assign_point_from_interface = [&, num_joints]( + std::vector & trajectory_point_interface, const auto & joint_interface) { - for (auto index = 0ul; index < joint_num; ++index) { - trajectory_point_interface[index] = joint_inteface[index].get().get_value(); + for (auto index = 0ul; index < num_joints; ++index) { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; @@ -654,6 +647,9 @@ bool AdmittanceController::read_state_from_command_interfaces( if (has_velocity_state_interface_) { if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1])) { assign_point_from_interface(state.velocities, joint_command_interface_[1]); + //TODO(destogl): enable this line under to be sure if positions are not existing and velocities + // are existing to still update the output_state; !commented because not tested! +// has_values = true; } else { state.velocities.clear(); has_values = false; @@ -676,6 +672,10 @@ bool AdmittanceController::read_state_from_command_interfaces( // state.accelerations.clear(); // } + if (has_values) { + output_state = state; + } + return has_values; } diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 06c21bdbea..68f601e430 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -172,6 +172,11 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared relative_desired_joint_state_vec_.reserve(6); + admittance_rule_calculated_values_.positions.resize(6, 0.0); + admittance_rule_calculated_values_.velocities.resize(6, 0.0); + admittance_rule_calculated_values_.accelerations.resize(6, 0.0); + admittance_rule_calculated_values_.effort.resize(6, 0.0); + // Initialize IK ik_ = std::make_shared(node, ik_group_name_); @@ -191,6 +196,12 @@ controller_interface::return_type AdmittanceRule::reset() get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); feedforward_pose_ik_base_frame_ = current_pose_ik_base_frame_; + // "Open-loop" controller uses old desired pose as current pose: current_pose(K) = desired_pose(K-1) + // Therefore desired pose has to be set before calling *update*-method + if (open_loop_control_) { + get_pose_of_control_frame_in_base_frame(desired_pose_ik_base_frame_); + } + // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; try { @@ -200,7 +211,8 @@ controller_interface::return_type AdmittanceRule::reset() control_frame_to_ik_tip_tf_ = tf2_transform.inverse(); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_tip_frame_ + "' and '" + control_frame_ + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + + control_frame_ + "' to '" + ik_tip_frame_ + "'."); return controller_interface::return_type::ERROR; } @@ -219,9 +231,10 @@ controller_interface::return_type AdmittanceRule::update( // Convert inputs to ik_base frame (assumed stationary) transform_message_to_ik_base_frame(target_pose, target_pose_ik_base_frame_); - // TODO(andyz): what if there is a hardware offset? if (!open_loop_control_) { get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); + } else { + current_pose_ik_base_frame_ = desired_pose_ik_base_frame_; } // Convert all data to arrays for simpler calculation @@ -248,6 +261,7 @@ controller_interface::return_type AdmittanceRule::update( calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, feedforward_acceleration, period, relative_desired_pose_arr_); + // This works in all cases because not current TF data are used // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); @@ -264,33 +278,37 @@ controller_interface::return_type AdmittanceRule::update( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); - std::vector target_ik_tip_deltas_vec(6); + std::vector target_deltas_vec_ik_base(6); // Get feed-forward cartesian deltas in the ik_base frame. // Since ik_base is MoveIt's working frame, the transform is identity. identity_transform_.header.frame_id = ik_base_frame_; ik_->update_robot_state(current_joint_state); - if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, identity_transform_, target_ik_tip_deltas_vec)) { + // FIXME: Do we need if here? Can we simply use if (!ik_->...)? + if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, identity_transform_, target_deltas_vec_ik_base)) { } else { - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint values to the robot."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), + "Conversion of joint deltas to Cartesian deltas failed. Sending current joint" + " values to the robot."); desired_joint_state = current_joint_state; std::fill(desired_joint_state.velocities.begin(), desired_joint_state.velocities.end(), 0.0); return controller_interface::return_type::ERROR; } for (auto i = 0u; i < 6; ++i) { - feedforward_velocity_ik_base_frame_[i] = target_ik_tip_deltas_vec.at(i) / period.seconds(); + feedforward_velocity_ik_base_frame_[i] = target_deltas_vec_ik_base.at(i) / period.seconds(); } // Add deltas to previously-desired pose to get the next desired pose - // TODO: Use convert_to_array method - feedforward_pose_ik_base_frame_.pose.position.x += target_ik_tip_deltas_vec.at(0); - feedforward_pose_ik_base_frame_.pose.position.y += target_ik_tip_deltas_vec.at(1); - feedforward_pose_ik_base_frame_.pose.position.z += target_ik_tip_deltas_vec.at(2); + // FIXME: Why not use convert_to_array method? + // FIXME: (?) Does this variable have a wrong name? Shouldn't it be target_pose_ik_base_frame? + feedforward_pose_ik_base_frame_.pose.position.x += target_deltas_vec_ik_base.at(0); + feedforward_pose_ik_base_frame_.pose.position.y += target_deltas_vec_ik_base.at(1); + feedforward_pose_ik_base_frame_.pose.position.z += target_deltas_vec_ik_base.at(2); tf2::Quaternion q(feedforward_pose_ik_base_frame_.pose.orientation.x, feedforward_pose_ik_base_frame_.pose.orientation.y, feedforward_pose_ik_base_frame_.pose.orientation.z, feedforward_pose_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; - q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); + q_rot.setRPY(target_deltas_vec_ik_base.at(3), target_deltas_vec_ik_base.at(4), target_deltas_vec_ik_base.at(5)); q = q_rot * q; q.normalize(); feedforward_pose_ik_base_frame_.pose.orientation.w = q.w(); @@ -331,11 +349,13 @@ controller_interface::return_type AdmittanceRule::get_controller_state( tf2::doTransform(measured_wrench_control_frame_, measured_wrench_endeffector_frame_, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + endeffector_frame_ + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + + control_frame_ + "' to '" + endeffector_frame_ + "'."); } - state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; + state_message.admittance_rule_calculated_values = admittance_rule_calculated_values_; + state_message.current_pose = current_pose_ik_base_frame_; state_message.desired_pose = desired_pose_ik_base_frame_; state_message.relative_desired_pose = relative_desired_pose_; @@ -355,7 +375,8 @@ controller_interface::return_type AdmittanceRule::get_pose_of_control_frame_in_b pose.pose.orientation= transform.transform.rotation; } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + control_frame_ + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + + control_frame_ + "' to '" + ik_base_frame_ + "'."); return controller_interface::return_type::ERROR; } return controller_interface::return_type::OK; @@ -372,11 +393,11 @@ void AdmittanceRule::process_wrench_measurements( // // get current states, and transform those into controller frame // measured_wrench_.wrench = measured_wrench; // try { - // auto transform = tf_buffer_->lookupTransform(fixed_world_frame_, measured_wrench_.header.frame_id, tf2::TimePointZero); - // auto transform_back = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); + // auto transform_to_world = tf_buffer_->lookupTransform(fixed_world_frame_, measured_wrench_.header.frame_id, tf2::TimePointZero); + // auto transform_to_sensor = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); // geometry_msgs::msg::WrenchStamped measured_wrench_transformed; - // tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform); + // tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform_to_world); // geometry_msgs::msg::Vector3Stamped cog_transformed; // for (const auto & params : gravity_compensation_params_) { @@ -388,10 +409,8 @@ void AdmittanceRule::process_wrench_measurements( // measured_wrench_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); // } - // tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_back); + // tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_to_sensor); - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), transform.transform.translation.y); - // RCLCPP_ERROR_STREAM(rclcpp::get_logger("AdmittanceRule"), transform.transform.translation.z); // } catch (const tf2::TransformException & e) { // // TODO(destogl): Use RCLCPP_ERROR_THROTTLE // RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_wrench_.header.frame_id + "' or ''."); @@ -430,8 +449,10 @@ void AdmittanceRule::calculate_admittance_rule( desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; -// RCLCPP_INFO(rclcpp::get_logger("AR"), "%e = %e - (1/M(%.1f)) (%e - D(%.1f)*%e - S(%.1f)*%e)", -// acceleration, feedforward_acceleration[i], mass_[i], measured_wrench[i], damping_[i], desired_velocity_arr_[i], stiffness_[i], pose_error[i]); + admittance_rule_calculated_values_.positions[i] = pose_error[i]; + admittance_rule_calculated_values_.velocities[i] = desired_velocity_arr_[i]; + admittance_rule_calculated_values_.accelerations[i] = acceleration; + admittance_rule_calculated_values_.effort[i] = measured_wrench[i]; } } } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 035cb4c3b1..15e3e0b2d4 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -404,15 +404,15 @@ TEST_F(AdmittanceControllerTest, publish_status_success) msg.joint_names.begin(), msg.joint_names.end(), joint_names_.begin(), joint_names_.end())); ASSERT_TRUE(std::equal( - msg.actual_joint_states.positions.begin(), msg.actual_joint_states.positions.end(), + 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_states.positions.begin(), msg.actual_joint_states.positions.end(), + 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_states.positions.begin(), msg.desired_joint_states.positions.end(), + 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(), From 877b6debb80e2d4cd775551f9878bce15402eec7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 3 Jun 2021 15:56:14 +0200 Subject: [PATCH 70/99] Rename feedforward_pose variable to target_from_joint_deltas --- .../admittance_controller/admittance_rule.hpp | 2 +- admittance_controller/src/admittance_rule.cpp | 22 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 9e275962d3..717e45fc24 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -161,7 +161,7 @@ class AdmittanceRule geometry_msgs::msg::PoseStamped current_pose_ik_base_frame_; // This is the feedforward pose. Where should the end effector be with no wrench applied? - geometry_msgs::msg::PoseStamped feedforward_pose_ik_base_frame_; + geometry_msgs::msg::PoseStamped target_pose_from_joint_deltas_ik_base_frame_; std::array feedforward_velocity_ik_base_frame_; // Need to save the previous velocity to calculate acceleration std::array prev_feedforward_velocity_ik_base_frame_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 68f601e430..5b7885b495 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -194,7 +194,7 @@ controller_interface::return_type AdmittanceRule::reset() desired_acceleration_previous_arr_.fill(0.0); get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); - feedforward_pose_ik_base_frame_ = current_pose_ik_base_frame_; + target_pose_from_joint_deltas_ik_base_frame_ = current_pose_ik_base_frame_; // "Open-loop" controller uses old desired pose as current pose: current_pose(K) = desired_pose(K-1) // Therefore desired pose has to be set before calling *update*-method @@ -301,22 +301,22 @@ controller_interface::return_type AdmittanceRule::update( // Add deltas to previously-desired pose to get the next desired pose // FIXME: Why not use convert_to_array method? - // FIXME: (?) Does this variable have a wrong name? Shouldn't it be target_pose_ik_base_frame? - feedforward_pose_ik_base_frame_.pose.position.x += target_deltas_vec_ik_base.at(0); - feedforward_pose_ik_base_frame_.pose.position.y += target_deltas_vec_ik_base.at(1); - feedforward_pose_ik_base_frame_.pose.position.z += target_deltas_vec_ik_base.at(2); + target_pose_from_joint_deltas_ik_base_frame_.pose.position.x += target_deltas_vec_ik_base.at(0); + target_pose_from_joint_deltas_ik_base_frame_.pose.position.y += target_deltas_vec_ik_base.at(1); + target_pose_from_joint_deltas_ik_base_frame_.pose.position.z += target_deltas_vec_ik_base.at(2); - tf2::Quaternion q(feedforward_pose_ik_base_frame_.pose.orientation.x, feedforward_pose_ik_base_frame_.pose.orientation.y, feedforward_pose_ik_base_frame_.pose.orientation.z, feedforward_pose_ik_base_frame_.pose.orientation.w); + tf2::Quaternion q(target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x, target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y, target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z, target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; q_rot.setRPY(target_deltas_vec_ik_base.at(3), target_deltas_vec_ik_base.at(4), target_deltas_vec_ik_base.at(5)); q = q_rot * q; q.normalize(); - feedforward_pose_ik_base_frame_.pose.orientation.w = q.w(); - feedforward_pose_ik_base_frame_.pose.orientation.x = q.x(); - feedforward_pose_ik_base_frame_.pose.orientation.y = q.y(); - feedforward_pose_ik_base_frame_.pose.orientation.z = q.z(); + target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w = q.w(); + target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x = q.x(); + target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y = q.y(); + target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z = q.z(); - return update(current_joint_state, measured_wrench, feedforward_pose_ik_base_frame_, period, desired_joint_state); + return update(current_joint_state, measured_wrench, target_pose_from_joint_deltas_ik_base_frame_, + period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( From 8b9607ad0cefde3c532f5709cbbf1d993d9fb8db Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 5 Jun 2021 12:15:25 +0200 Subject: [PATCH 71/99] Correct rotation issues. Stability is reduced. --- .../include/admittance_controller/admittance_rule.hpp | 1 + admittance_controller/src/admittance_rule.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 717e45fc24..b4b3415872 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -180,6 +180,7 @@ class AdmittanceRule std::array angles_error_; std::array relative_desired_pose_arr_; + std::array desired_pose_ik_base_frame_arr_; std::array desired_velocity_arr_; std::array desired_velocity_previous_arr_; std::array desired_acceleration_previous_arr_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 5b7885b495..1d28cb7be4 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -167,6 +167,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared measured_wrench_.header.frame_id = sensor_frame_; relative_desired_pose_.header.frame_id = control_frame_; + relative_desired_pose_.child_frame_id = control_frame_; identity_transform_.transform.rotation.w = 1; @@ -200,6 +201,7 @@ controller_interface::return_type AdmittanceRule::reset() // Therefore desired pose has to be set before calling *update*-method if (open_loop_control_) { get_pose_of_control_frame_in_base_frame(desired_pose_ik_base_frame_); + convert_message_to_array(desired_pose_ik_base_frame_, desired_pose_ik_base_frame_arr_); } // Initialize ik_tip and tool_frame transformations - those are fixed transformations @@ -264,7 +266,8 @@ controller_interface::return_type AdmittanceRule::update( // This works in all cases because not current TF data are used // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); + +// tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } @@ -445,6 +448,7 @@ void AdmittanceRule::calculate_admittance_rule( desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); desired_relative_pose[i] = (desired_velocity_previous_arr_[i] + desired_velocity_arr_[i]) * 0.5 * period.seconds(); + desired_pose_ik_base_frame_arr_[i] = current_pose_ik_base_frame_arr_[i] + desired_relative_pose[i]; desired_acceleration_previous_arr_[i] = acceleration; desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; From 4be5909be0472d5b3c901936d1f950c6c8d8a4d7 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 3 Jun 2021 16:10:16 -0500 Subject: [PATCH 72/99] Implement get_link_transform(). Rename IncrementalKinematics --- .../admittance_controller/admittance_rule.hpp | 2 +- .../incremental_kinematics.hpp | 13 +++++++++---- admittance_controller/src/admittance_rule.cpp | 6 +++--- .../src/incremental_kinematics.cpp | 15 ++++++++++++--- 4 files changed, 25 insertions(+), 11 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index b4b3415872..277084ae9e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -135,7 +135,7 @@ class AdmittanceRule ); // IK variables - std::shared_ptr ik_; + std::shared_ptr ik_; // Filters // using GravityCompensatorType = diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index 6b86c64c7e..ab7bc368ca 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -25,14 +25,14 @@ namespace admittance_controller { -class IncrementalKinematics +class MoveItKinematics { public: /** * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ - IncrementalKinematics(const std::shared_ptr & node, const std::string & group_name); + MoveItKinematics(const std::shared_ptr & node, const std::string & group_name); /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. @@ -42,7 +42,7 @@ class IncrementalKinematics * \return true if successful */ bool - convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec); + convert_cartesian_deltas_to_joint_deltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec); /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. @@ -52,7 +52,12 @@ class IncrementalKinematics * \return true if successful */ bool - convertJointDeltasToCartesianDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec); + convert_joint_deltas_to_cartesian_deltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec); + + /** + * \brief Get a link transform in MoveIt's reference frame, ik_base + */ + bool get_link_transform(const std::string& link_name, const trajectory_msgs::msg::JointTrajectoryPoint & joint_state, Eigen::Isometry3d link_transform); bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 1d28cb7be4..f23af94e79 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -179,7 +179,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared admittance_rule_calculated_values_.effort.resize(6, 0.0); // Initialize IK - ik_ = std::make_shared(node, ik_group_name_); + ik_ = std::make_shared(node, ik_group_name_); return controller_interface::return_type::OK; } @@ -288,7 +288,7 @@ controller_interface::return_type AdmittanceRule::update( identity_transform_.header.frame_id = ik_base_frame_; ik_->update_robot_state(current_joint_state); // FIXME: Do we need if here? Can we simply use if (!ik_->...)? - if (ik_->convertJointDeltasToCartesianDeltas(target_joint_deltas_vec, identity_transform_, target_deltas_vec_ik_base)) { + if (ik_->convert_joint_deltas_to_cartesian_deltas(target_joint_deltas_vec, identity_transform_, target_deltas_vec_ik_base)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint" @@ -474,7 +474,7 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); - if (ik_->convertCartesianDeltasToJointDeltas( + if (ik_->convert_cartesian_deltas_to_joint_deltas( relative_desired_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index cc547e02df..1e35843f9b 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -20,7 +20,7 @@ namespace admittance_controller { -IncrementalKinematics::IncrementalKinematics(const std::shared_ptr & node, const std::string & group_name) : node_(node) +MoveItKinematics::MoveItKinematics(const std::shared_ptr & node, const std::string & group_name) : node_(node) { // TODO(andyz): Parameterize robot description and joint group std::unique_ptr model_loader_ptr = @@ -33,7 +33,7 @@ IncrementalKinematics::IncrementalKinematics(const std::shared_ptr // By default, the MoveIt Jacobian frame is the last link } -bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec) +bool MoveItKinematics::convert_cartesian_deltas_to_joint_deltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); @@ -82,7 +82,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec) +bool MoveItKinematics::convert_joint_deltas_to_cartesian_deltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_theta = Eigen::Map(&delta_theta_vec[0], delta_theta_vec.size()); @@ -129,4 +129,13 @@ bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(std::vectorgetGlobalLinkTransform(link_name); + return true; +} + } // namespace admittance_controller From e2cb89a447acd5392c6612b3098295006f5db355 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 3 Jun 2021 16:36:30 -0500 Subject: [PATCH 73/99] Renaming of incremental_kinematics -> moveit_kinematics --- admittance_controller/CMakeLists.txt | 2 +- .../include/admittance_controller/admittance_rule.hpp | 2 +- ...ncremental_kinematics.hpp => moveit_kinematics.hpp} | 4 ++-- admittance_controller/src/admittance_controller.cpp | 1 - ...ncremental_kinematics.cpp => moveit_kinematics.cpp} | 10 +++------- 5 files changed, 7 insertions(+), 12 deletions(-) rename admittance_controller/include/admittance_controller/{incremental_kinematics.hpp => moveit_kinematics.hpp} (93%) rename admittance_controller/src/{incremental_kinematics.cpp => moveit_kinematics.cpp} (94%) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 3c31726593..cd80d5c8c9 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -32,7 +32,7 @@ add_library( SHARED src/admittance_controller.cpp src/admittance_rule.cpp - src/incremental_kinematics.cpp + src/moveit_kinematics.cpp ) target_include_directories( admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 277084ae9e..ccff9d6466 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -17,7 +17,7 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ -#include "admittance_controller/incremental_kinematics.hpp" +#include "admittance_controller/moveit_kinematics.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/quaternion.hpp" diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp similarity index 93% rename from admittance_controller/include/admittance_controller/incremental_kinematics.hpp rename to admittance_controller/include/admittance_controller/moveit_kinematics.hpp index ab7bc368ca..0fd9f40bfb 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp @@ -57,7 +57,7 @@ class MoveItKinematics /** * \brief Get a link transform in MoveIt's reference frame, ik_base */ - bool get_link_transform(const std::string& link_name, const trajectory_msgs::msg::JointTrajectoryPoint & joint_state, Eigen::Isometry3d link_transform); + Eigen::Isometry3d get_link_transform(const std::string& link_name); bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { @@ -72,7 +72,7 @@ class MoveItKinematics } private: - // MoveIt setup, required to retrieve the Jacobian + // MoveIt setup const moveit::core::JointModelGroup* joint_model_group_; moveit::core::RobotStatePtr kinematic_state_; std::shared_ptr node_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 15aa692009..b26730cc9a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -23,7 +23,6 @@ #include "angles/angles.h" #include "admittance_controller/admittance_controller.hpp" -#include "admittance_controller/incremental_kinematics.hpp" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/moveit_kinematics.cpp similarity index 94% rename from admittance_controller/src/incremental_kinematics.cpp rename to admittance_controller/src/moveit_kinematics.cpp index 1e35843f9b..bcae348fa9 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/moveit_kinematics.cpp @@ -14,7 +14,7 @@ // /// \author: Andy Zelenak -#include "admittance_controller/incremental_kinematics.hpp" +#include "admittance_controller/moveit_kinematics.hpp" #include "tf2_eigen/tf2_eigen.h" @@ -129,13 +129,9 @@ bool MoveItKinematics::convert_joint_deltas_to_cartesian_deltas(std::vectorgetGlobalLinkTransform(link_name); - return true; + return kinematic_state_->getGlobalLinkTransform(link_name); } } // namespace admittance_controller From 0c09d4e3b02d018cd8019a0faeda213fbb353c69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 5 Jun 2021 12:54:18 +0200 Subject: [PATCH 74/99] Added Andy as author and correct include style. --- .../include/admittance_controller/admittance_controller.hpp | 2 +- .../include/admittance_controller/admittance_rule.hpp | 2 +- admittance_controller/package.xml | 1 + admittance_controller/src/admittance_controller.cpp | 5 +++-- admittance_controller/src/admittance_rule.cpp | 4 ++-- 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 80b4fa1bf4..99ad7c2025 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Denis Stogl +/// \authors: Denis Stogl, Andy Zelenak #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define 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 index ccff9d6466..2bde3495f0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Denis Stogl +/// \authors: Denis Stogl, Andy Zelenak #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 0d6964496f..a89f54a7fc 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -5,6 +5,7 @@ 0.0.0 Implemenation of admittance controllers for different input and output interface. Denis Štogl + Andy Zelenak Apache License 2.0 ament_cmake diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b26730cc9a..6e51deb36c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Denis Stogl +/// \authors: Denis Stogl, Andy Zelenak + +#include "admittance_controller/admittance_controller.hpp" #include #include @@ -22,7 +24,6 @@ #include #include "angles/angles.h" -#include "admittance_controller/admittance_controller.hpp" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index f23af94e79..e43af2c75e 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. // -/// \author: Denis Stogl +/// \authors: Denis Stogl, Andy Zelenak #include "admittance_controller/admittance_rule.hpp" @@ -26,7 +26,7 @@ #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" #include "tf2/utils.h" -#include +#include "tf2_eigen/tf2_eigen.h" namespace { // Utility namespace From 520f5736fcc91ea7ad07eba363bac25f81601151 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 6 Jun 2021 11:39:57 +0200 Subject: [PATCH 75/99] Update moveit kinematics accepting current joint_state --- .../include/admittance_controller/admittance_rule.hpp | 2 -- .../include/admittance_controller/moveit_kinematics.hpp | 2 +- admittance_controller/src/admittance_controller.cpp | 2 +- admittance_controller/src/admittance_rule.cpp | 2 +- admittance_controller/src/moveit_kinematics.cpp | 4 +++- 5 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 2bde3495f0..b2815e41c7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -177,8 +177,6 @@ class AdmittanceRule std::array target_pose_ik_base_frame_arr_; std::array current_pose_ik_base_frame_arr_; - std::array angles_error_; - std::array relative_desired_pose_arr_; std::array desired_pose_ik_base_frame_arr_; std::array desired_velocity_arr_; diff --git a/admittance_controller/include/admittance_controller/moveit_kinematics.hpp b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp index 0fd9f40bfb..b9a9479494 100644 --- a/admittance_controller/include/admittance_controller/moveit_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp @@ -57,7 +57,7 @@ class MoveItKinematics /** * \brief Get a link transform in MoveIt's reference frame, ik_base */ - Eigen::Isometry3d get_link_transform(const std::string& link_name); + Eigen::Isometry3d get_link_transform(const std::string& link_name, const trajectory_msgs::msg::JointTrajectoryPoint & joint_state); bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e51deb36c..5d8f710337 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -496,7 +496,7 @@ controller_interface::return_type AdmittanceController::update() auto input_joint_cmd = input_joint_command_.readFromRT(); auto input_pose_cmd = input_pose_command_.readFromRT(); - // Position has to always there + // Position has to always be there auto num_joints = joint_state_interface_[0].size(); trajectory_msgs::msg::JointTrajectoryPoint current_joint_states; current_joint_states.positions.resize(num_joints, 0.0); diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index e43af2c75e..9477d9bfc3 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -189,7 +189,6 @@ controller_interface::return_type AdmittanceRule::reset() measured_wrench_control_frame_arr_.fill(0.0); target_pose_ik_base_frame_arr_.fill(0.0); current_pose_ik_base_frame_arr_.fill(0.0); - angles_error_.fill(0.0); desired_velocity_arr_.fill(0.0); desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); @@ -474,6 +473,7 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); + ik_->update_robot_state(current_joint_state); if (ik_->convert_cartesian_deltas_to_joint_deltas( relative_desired_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { diff --git a/admittance_controller/src/moveit_kinematics.cpp b/admittance_controller/src/moveit_kinematics.cpp index bcae348fa9..e6f8730e75 100644 --- a/admittance_controller/src/moveit_kinematics.cpp +++ b/admittance_controller/src/moveit_kinematics.cpp @@ -129,8 +129,10 @@ bool MoveItKinematics::convert_joint_deltas_to_cartesian_deltas(std::vectorgetGlobalLinkTransform(link_name); } From b5591286d6b100539cdbffc433748431e8546626 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Jun 2021 11:52:40 +0200 Subject: [PATCH 76/99] Adjust formating of moveit kinematics file. --- .../admittance_controller/moveit_kinematics.hpp | 14 +++++++++++--- admittance_controller/src/moveit_kinematics.cpp | 13 ++++++++++--- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/admittance_controller/include/admittance_controller/moveit_kinematics.hpp b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp index b9a9479494..86e37e5758 100644 --- a/admittance_controller/include/admittance_controller/moveit_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp @@ -42,7 +42,10 @@ class MoveItKinematics * \return true if successful */ bool - convert_cartesian_deltas_to_joint_deltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec); + convert_cartesian_deltas_to_joint_deltas( + std::vector & delta_x_vec, + const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, + std::vector & delta_theta_vec); /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. @@ -52,12 +55,17 @@ class MoveItKinematics * \return true if successful */ bool - convert_joint_deltas_to_cartesian_deltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec); + convert_joint_deltas_to_cartesian_deltas( + std::vector & delta_theta_vec, + const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, + std::vector & delta_x_vec); /** * \brief Get a link transform in MoveIt's reference frame, ik_base */ - Eigen::Isometry3d get_link_transform(const std::string& link_name, const trajectory_msgs::msg::JointTrajectoryPoint & joint_state); + Eigen::Isometry3d + get_link_transform( + const std::string& link_name, const trajectory_msgs::msg::JointTrajectoryPoint & joint_state); bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { diff --git a/admittance_controller/src/moveit_kinematics.cpp b/admittance_controller/src/moveit_kinematics.cpp index e6f8730e75..2b72ff75b6 100644 --- a/admittance_controller/src/moveit_kinematics.cpp +++ b/admittance_controller/src/moveit_kinematics.cpp @@ -33,7 +33,10 @@ MoveItKinematics::MoveItKinematics(const std::shared_ptr & node, c // By default, the MoveIt Jacobian frame is the last link } -bool MoveItKinematics::convert_cartesian_deltas_to_joint_deltas(std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, std::vector & delta_theta_vec) +bool MoveItKinematics::convert_cartesian_deltas_to_joint_deltas( + std::vector & delta_x_vec, + const geometry_msgs::msg::TransformStamped & control_frame_to_ik_base, + std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); @@ -82,7 +85,10 @@ bool MoveItKinematics::convert_cartesian_deltas_to_joint_deltas(std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, std::vector & delta_x_vec) +bool MoveItKinematics::convert_joint_deltas_to_cartesian_deltas( + std::vector & delta_theta_vec, + const geometry_msgs::msg::TransformStamped & tf_ik_base_to_desired_cartesian_frame, + std::vector & delta_x_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXd delta_theta = Eigen::Map(&delta_theta_vec[0], delta_theta_vec.size()); @@ -129,7 +135,8 @@ bool MoveItKinematics::convert_joint_deltas_to_cartesian_deltas(std::vector Date: Sun, 6 Jun 2021 12:47:43 +0200 Subject: [PATCH 77/99] Rename desired to reference and interal variables with prefix admittance --- .../admittance_controller/admittance_rule.hpp | 23 +++-- .../src/admittance_controller.cpp | 4 - admittance_controller/src/admittance_rule.cpp | 91 ++++++++++--------- 3 files changed, 61 insertions(+), 57 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index b2815e41c7..465cba4234 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -52,7 +52,7 @@ class AdmittanceRule 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 & target_pose, + const geometry_msgs::msg::PoseStamped & reference_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states ); @@ -60,7 +60,7 @@ class AdmittanceRule controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, - const std::array & target_joint_deltas, + const std::array & reference_joint_deltas, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states ); @@ -68,8 +68,8 @@ class AdmittanceRule 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 & target_pose, - const geometry_msgs::msg::WrenchStamped & target_force, + 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 ); @@ -123,7 +123,6 @@ class AdmittanceRule void calculate_admittance_rule( const std::array & measured_wrench, const std::array & pose_error, - const std::array & feedforward_acceleration, const rclcpp::Duration & period, std::array & desired_relative_pose ); @@ -161,27 +160,27 @@ class AdmittanceRule geometry_msgs::msg::PoseStamped current_pose_ik_base_frame_; // This is the feedforward pose. Where should the end effector be with no wrench applied? - geometry_msgs::msg::PoseStamped target_pose_from_joint_deltas_ik_base_frame_; + geometry_msgs::msg::PoseStamped reference_pose_from_joint_deltas_ik_base_frame_; std::array feedforward_velocity_ik_base_frame_; // Need to save the previous velocity to calculate acceleration std::array prev_feedforward_velocity_ik_base_frame_; - geometry_msgs::msg::WrenchStamped target_force_ik_base_frame_; - geometry_msgs::msg::PoseStamped target_pose_ik_base_frame_; + geometry_msgs::msg::WrenchStamped reference_force_ik_base_frame_; + geometry_msgs::msg::PoseStamped reference_pose_ik_base_frame_; geometry_msgs::msg::PoseStamped desired_pose_ik_base_frame_; geometry_msgs::msg::TransformStamped relative_desired_pose_; // Pre-reserved update-loop variables std::array measured_wrench_control_frame_arr_; - std::array target_pose_ik_base_frame_arr_; + std::array reference_pose_ik_base_frame_arr_; std::array current_pose_ik_base_frame_arr_; std::array relative_desired_pose_arr_; std::array desired_pose_ik_base_frame_arr_; - std::array desired_velocity_arr_; - std::array desired_velocity_previous_arr_; - std::array desired_acceleration_previous_arr_; + std::array admittance_velocity_arr_; + std::array admittance_velocity_previous_arr_; + std::array admittance_acceleration_previous_arr_; std::vector relative_desired_joint_state_vec_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5d8f710337..3f8c1c13ac 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -537,10 +537,6 @@ controller_interface::return_type AdmittanceController::update() joint_deltas[index] = angles::shortest_angular_distance(current_joint_states.positions[index], (*input_joint_cmd)->points[0].positions[index]); } } -// RCLCPP_INFO(get_node()->get_logger(), "JointDeltas: [%e, %e, %e, %e, %e, %e]", -// joint_deltas[0], joint_deltas[1], joint_deltas[2], joint_deltas[3], -// joint_deltas[4], joint_deltas[5] -// ); admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); } else { diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 9477d9bfc3..31b912224d 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -187,14 +187,14 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared controller_interface::return_type AdmittanceRule::reset() { measured_wrench_control_frame_arr_.fill(0.0); - target_pose_ik_base_frame_arr_.fill(0.0); + reference_pose_ik_base_frame_arr_.fill(0.0); current_pose_ik_base_frame_arr_.fill(0.0); - desired_velocity_arr_.fill(0.0); - desired_velocity_previous_arr_.fill(0.0); - desired_acceleration_previous_arr_.fill(0.0); + admittance_velocity_arr_.fill(0.0); + admittance_velocity_previous_arr_.fill(0.0); + admittance_acceleration_previous_arr_.fill(0.0); get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); - target_pose_from_joint_deltas_ik_base_frame_ = current_pose_ik_base_frame_; + reference_pose_from_joint_deltas_ik_base_frame_ = current_pose_ik_base_frame_; // "Open-loop" controller uses old desired pose as current pose: current_pose(K) = desired_pose(K-1) // Therefore desired pose has to be set before calling *update*-method @@ -224,13 +224,13 @@ controller_interface::return_type AdmittanceRule::reset() controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, - const geometry_msgs::msg::PoseStamped & target_pose, + const geometry_msgs::msg::PoseStamped & reference_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { // Convert inputs to ik_base frame (assumed stationary) - transform_message_to_ik_base_frame(target_pose, target_pose_ik_base_frame_); + transform_message_to_ik_base_frame(reference_pose, reference_pose_ik_base_frame_); if (!open_loop_control_) { get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); @@ -239,15 +239,15 @@ controller_interface::return_type AdmittanceRule::update( } // Convert all data to arrays for simpler calculation - convert_message_to_array(target_pose_ik_base_frame_, target_pose_ik_base_frame_arr_); + convert_message_to_array(reference_pose_ik_base_frame_, reference_pose_ik_base_frame_arr_); convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); std::array pose_error; - // Estimate feedforward acceleration from target_pose_ik_base_frame_arr_ and previous + // Estimate feedforward acceleration from reference_pose_ik_base_frame_arr_ and previous std::array feedforward_acceleration; for (auto i = 0u; i < 6; ++i) { - pose_error[i] = current_pose_ik_base_frame_arr_[i] - target_pose_ik_base_frame_arr_[i]; + pose_error[i] = current_pose_ik_base_frame_arr_[i] - reference_pose_ik_base_frame_arr_[i]; if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } @@ -259,12 +259,13 @@ controller_interface::return_type AdmittanceRule::update( process_wrench_measurements(measured_wrench); - calculate_admittance_rule(measured_wrench_control_frame_arr_, pose_error, feedforward_acceleration, period, - relative_desired_pose_arr_); + calculate_admittance_rule( + measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); // This works in all cases because not current TF data are used // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); + convert_array_to_message(desired_pose_ik_base_frame_arr_, desired_pose_ik_base_frame_); // tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); @@ -275,19 +276,20 @@ controller_interface::return_type AdmittanceRule::update( controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const geometry_msgs::msg::Wrench & measured_wrench, - const std::array & target_joint_deltas, + const std::array & reference_joint_deltas, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { - std::vector target_joint_deltas_vec(target_joint_deltas.begin(), target_joint_deltas.end()); - std::vector target_deltas_vec_ik_base(6); + std::vector reference_joint_deltas_vec( + reference_joint_deltas.begin(), reference_joint_deltas.end()); + std::vector reference_deltas_vec_ik_base(6); // Get feed-forward cartesian deltas in the ik_base frame. // Since ik_base is MoveIt's working frame, the transform is identity. identity_transform_.header.frame_id = ik_base_frame_; ik_->update_robot_state(current_joint_state); // FIXME: Do we need if here? Can we simply use if (!ik_->...)? - if (ik_->convert_joint_deltas_to_cartesian_deltas(target_joint_deltas_vec, identity_transform_, target_deltas_vec_ik_base)) { + if (ik_->convert_joint_deltas_to_cartesian_deltas(reference_joint_deltas_vec, identity_transform_, reference_deltas_vec_ik_base)) { } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint" @@ -298,34 +300,34 @@ controller_interface::return_type AdmittanceRule::update( } for (auto i = 0u; i < 6; ++i) { - feedforward_velocity_ik_base_frame_[i] = target_deltas_vec_ik_base.at(i) / period.seconds(); + feedforward_velocity_ik_base_frame_[i] = reference_deltas_vec_ik_base.at(i) / period.seconds(); } // Add deltas to previously-desired pose to get the next desired pose // FIXME: Why not use convert_to_array method? - target_pose_from_joint_deltas_ik_base_frame_.pose.position.x += target_deltas_vec_ik_base.at(0); - target_pose_from_joint_deltas_ik_base_frame_.pose.position.y += target_deltas_vec_ik_base.at(1); - target_pose_from_joint_deltas_ik_base_frame_.pose.position.z += target_deltas_vec_ik_base.at(2); + reference_pose_from_joint_deltas_ik_base_frame_.pose.position.x += reference_deltas_vec_ik_base.at(0); + reference_pose_from_joint_deltas_ik_base_frame_.pose.position.y += reference_deltas_vec_ik_base.at(1); + reference_pose_from_joint_deltas_ik_base_frame_.pose.position.z += reference_deltas_vec_ik_base.at(2); - tf2::Quaternion q(target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x, target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y, target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z, target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w); + tf2::Quaternion q(reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x, reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y, reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z, reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; - q_rot.setRPY(target_deltas_vec_ik_base.at(3), target_deltas_vec_ik_base.at(4), target_deltas_vec_ik_base.at(5)); + q_rot.setRPY(reference_deltas_vec_ik_base.at(3), reference_deltas_vec_ik_base.at(4), reference_deltas_vec_ik_base.at(5)); q = q_rot * q; q.normalize(); - target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w = q.w(); - target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x = q.x(); - target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y = q.y(); - target_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z = q.z(); + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w = q.w(); + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x = q.x(); + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y = q.y(); + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z = q.z(); - return update(current_joint_state, measured_wrench, target_pose_from_joint_deltas_ik_base_frame_, + return update(current_joint_state, measured_wrench, reference_pose_from_joint_deltas_ik_base_frame_, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_state*/, const geometry_msgs::msg::Wrench & /*measured_wrench*/, - const geometry_msgs::msg::PoseStamped & /*target_pose*/, - const geometry_msgs::msg::WrenchStamped & /*target_force*/, + const geometry_msgs::msg::PoseStamped & /*reference_pose*/, + const geometry_msgs::msg::WrenchStamped & /*reference_force*/, const rclcpp::Duration & /*period*/, trajectory_msgs::msg::JointTrajectoryPoint & /*desired_joint_state*/ ) @@ -340,8 +342,8 @@ controller_interface::return_type AdmittanceRule::update( controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message) { - // state_message.input_wrench_control_frame = target_wrench_control_frame_; - state_message.input_pose_control_frame = target_pose_ik_base_frame_; + // 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_control_frame_; @@ -432,7 +434,6 @@ void AdmittanceRule::process_wrench_measurements( void AdmittanceRule::calculate_admittance_rule( const std::array & measured_wrench, const std::array & pose_error, - const std::array & feedforward_acceleration, const rclcpp::Duration & period, std::array & desired_relative_pose ) @@ -441,20 +442,28 @@ void AdmittanceRule::calculate_admittance_rule( for (auto i = 0u; i < 6; ++i) { if (selected_axes_[i]) { // TODO(destogl): check if velocity is measured from hardware - const double acceleration = feedforward_acceleration[i] + (1 / mass_[i]) * - (measured_wrench[i] - damping_[i] * desired_velocity_arr_[i] - stiffness_[i] * pose_error[i]); + const double admittance_acceleration = (1 / mass_[i]) * (measured_wrench[i] - + damping_[i] * admittance_velocity_arr_[i] - + stiffness_[i] * pose_error[i]); - desired_velocity_arr_[i] += (desired_acceleration_previous_arr_[i] + acceleration) * 0.5 * period.seconds(); + // TODO(destogl): Maybe on contact we should not use forward difference + admittance_velocity_arr_[i] += + (admittance_acceleration_previous_arr_[i] + admittance_acceleration) * 0.5 * period.seconds(); - desired_relative_pose[i] = (desired_velocity_previous_arr_[i] + desired_velocity_arr_[i]) * 0.5 * period.seconds(); - desired_pose_ik_base_frame_arr_[i] = current_pose_ik_base_frame_arr_[i] + desired_relative_pose[i]; + // Calculate position + desired_relative_pose[i] = + (admittance_velocity_previous_arr_[i] + admittance_velocity_arr_[i]) * 0.5 * period.seconds(); + desired_pose_ik_base_frame_arr_[i] = + current_pose_ik_base_frame_arr_[i] + desired_relative_pose[i]; - desired_acceleration_previous_arr_[i] = acceleration; - desired_velocity_previous_arr_[i] = desired_velocity_arr_[i]; + // Store values for the next run + admittance_acceleration_previous_arr_[i] = admittance_acceleration; + admittance_velocity_previous_arr_[i] = admittance_velocity_arr_[i]; + // Store data for publishing to state variable admittance_rule_calculated_values_.positions[i] = pose_error[i]; - admittance_rule_calculated_values_.velocities[i] = desired_velocity_arr_[i]; - admittance_rule_calculated_values_.accelerations[i] = acceleration; + admittance_rule_calculated_values_.velocities[i] = admittance_velocity_arr_[i]; + admittance_rule_calculated_values_.accelerations[i] = admittance_acceleration; admittance_rule_calculated_values_.effort[i] = measured_wrench[i]; } } From 2826ad60c6ba18770e339ae05bc2790a6bfe53dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Jun 2021 15:34:10 +0200 Subject: [PATCH 78/99] Seems working. Neede one more round of checks. --- .../admittance_controller/admittance_rule.hpp | 15 ++++ admittance_controller/src/admittance_rule.cpp | 75 ++++++++++++++++--- 2 files changed, 81 insertions(+), 9 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 465cba4234..444967a360 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -82,6 +82,8 @@ class AdmittanceRule public: bool open_loop_control_ = false; + // TODO(destogl): Add parameter for this + bool feedforward_commanded_input_ = true; // IK related parameters // ik_base_frame should be stationary so vel/accel calculations are correct @@ -171,6 +173,8 @@ class AdmittanceRule geometry_msgs::msg::PoseStamped desired_pose_ik_base_frame_; geometry_msgs::msg::TransformStamped relative_desired_pose_; + bool movement_caused_by_wrench_ = false; + // Pre-reserved update-loop variables std::array measured_wrench_control_frame_arr_; std::array reference_pose_ik_base_frame_arr_; @@ -223,6 +227,17 @@ class AdmittanceRule output_tf = input_tf * transform; tf2::toMsg(output_tf, output); } + + // TODO(destogl): As of C++17 use transform_reduce: + // https://stackoverflow.com/questions/58266717/accumulate-absolute-values-of-a-vector + template + double accumulate_absolute(const Type & container) { + double accumulator = 0.0; + for (auto i = 0ul; i < container.size(); i++) { + accumulator += std::fabs(container[i]); + } + return accumulator; + } }; } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 31b912224d..c0d8cec4d6 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -30,6 +30,13 @@ 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; +static constexpr double VELOCITITY_EPSILON = 1e-20; +static constexpr double ACCELERATION_EPSILON = 1e-20; + template void convert_message_to_array(const geometry_msgs::msg::Pose & msg, Type & vector_out) { @@ -243,6 +250,7 @@ controller_interface::return_type AdmittanceRule::update( convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); std::array pose_error; + // TODO(destogl): remove this feed-forward term from here // Estimate feedforward acceleration from reference_pose_ik_base_frame_arr_ and previous std::array feedforward_acceleration; @@ -251,7 +259,10 @@ controller_interface::return_type AdmittanceRule::update( if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } - + if (std::fabs(pose_error[i]) < POSE_ERROR_EPSILON) { + pose_error[i] = 0.0; + } + // TODO(destogl): remove this feed-forward term from here // Estimate feedforward acceleration feedforward_acceleration[i] = (feedforward_velocity_ik_base_frame_[i] - prev_feedforward_velocity_ik_base_frame_[i]) / period.seconds(); } @@ -288,9 +299,9 @@ controller_interface::return_type AdmittanceRule::update( // Since ik_base is MoveIt's working frame, the transform is identity. identity_transform_.header.frame_id = ik_base_frame_; ik_->update_robot_state(current_joint_state); - // FIXME: Do we need if here? Can we simply use if (!ik_->...)? - if (ik_->convert_joint_deltas_to_cartesian_deltas(reference_joint_deltas_vec, identity_transform_, reference_deltas_vec_ik_base)) { - } else { + if (!ik_->convert_joint_deltas_to_cartesian_deltas( + reference_joint_deltas_vec, identity_transform_, reference_deltas_vec_ik_base)) + { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint" " values to the robot."); @@ -299,6 +310,7 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } + // TODO(destogl): remove this feed-forward term from here for (auto i = 0u; i < 6; ++i) { feedforward_velocity_ik_base_frame_[i] = reference_deltas_vec_ik_base.at(i) / period.seconds(); } @@ -319,8 +331,41 @@ controller_interface::return_type AdmittanceRule::update( reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y = q.y(); reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z = q.z(); - return update(current_joint_state, measured_wrench, reference_pose_from_joint_deltas_ik_base_frame_, - period, desired_joint_state); + update(current_joint_state, measured_wrench, reference_pose_from_joint_deltas_ik_base_frame_, + period, desired_joint_state); + + auto is_measured_wrench_zero = [&]() { + std::array measured_wrench_arr; + convert_message_to_array(measured_wrench, measured_wrench_arr); + double accumulated = accumulate_absolute(measured_wrench_arr); + return (accumulated < WRENCH_EPSILON || std::isnan(accumulated)); + }; + + auto is_relative_admittance_pose_zero = [&]() { + return (accumulate_absolute(relative_desired_pose_arr_) < POSE_EPSILON); + }; + + // FIXME(destogl): (?) This logic could cause "joy" (large jerk) on contact + if (feedforward_commanded_input_) { + if (is_measured_wrench_zero() && !movement_caused_by_wrench_) { + for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { + desired_joint_state.positions[i] = current_joint_state.positions[i] + reference_joint_deltas[i]; + desired_joint_state.velocities[i] = reference_joint_deltas[i] / period.seconds(); + } + } else { + for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { + desired_joint_state.positions[i] += reference_joint_deltas[i]; + desired_joint_state.velocities[i] += reference_joint_deltas[i] / period.seconds(); + } + if (is_relative_admittance_pose_zero()) { + movement_caused_by_wrench_ = false; + } else { + movement_caused_by_wrench_ = true; + } + } + } + + return controller_interface::return_type::OK; } controller_interface::return_type AdmittanceRule::update( @@ -348,6 +393,7 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_wrench_filtered = measured_wrench_filtered_; state_message.measured_wrench_control_frame = measured_wrench_control_frame_; + // FIXME(destogl): Something is wrong with this transformation - check frames... try { auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); tf2::doTransform(measured_wrench_control_frame_, measured_wrench_endeffector_frame_, transform); @@ -425,10 +471,18 @@ void AdmittanceRule::process_wrench_measurements( transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_control_frame_); convert_message_to_array(measured_wrench_control_frame_, measured_wrench_control_frame_arr_); + // TODO(destogl): optimize this checks! // If at least one measured force is nan set all to 0 if (std::find_if(measured_wrench_control_frame_arr_.begin(), measured_wrench_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_wrench_control_frame_arr_.end()) { measured_wrench_control_frame_arr_.fill(0.0); } + + // If a force or a torque is very small set it to 0 + for (auto i = 0u; i < measured_wrench_control_frame_arr_.size(); ++i) { + if (std::fabs(measured_wrench_control_frame_arr_[i]) < WRENCH_EPSILON) { + measured_wrench_control_frame_arr_[i] = 0.0; + } + } } void AdmittanceRule::calculate_admittance_rule( @@ -453,8 +507,11 @@ void AdmittanceRule::calculate_admittance_rule( // Calculate position desired_relative_pose[i] = (admittance_velocity_previous_arr_[i] + admittance_velocity_arr_[i]) * 0.5 * period.seconds(); + if (std::fabs(desired_relative_pose[i]) < POSE_EPSILON) { + desired_relative_pose[i] = 0.0; + } desired_pose_ik_base_frame_arr_[i] = - current_pose_ik_base_frame_arr_[i] + desired_relative_pose[i]; + current_pose_ik_base_frame_arr_[i] + desired_relative_pose[i]; // Store values for the next run admittance_acceleration_previous_arr_[i] = admittance_acceleration; @@ -486,9 +543,9 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( if (ik_->convert_cartesian_deltas_to_joint_deltas( relative_desired_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { - desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; + desired_joint_state.positions[i] = + current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds(); - // RCLCPP_INFO(rclcpp::get_logger("AR"), "joint states [%zu]: %f + %f = %f", i, current_joint_state.positions[i], relative_desired_joint_state_vec_[i], desired_joint_state.positions[i]); } } else { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot."); From 1cfbf0cc011f37f52b03fc87bc36949040f9282a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Jun 2021 16:39:06 +0200 Subject: [PATCH 79/99] Cleaning of the not used feedforward variables. --- admittance_controller/src/admittance_rule.cpp | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index c0d8cec4d6..3b61730954 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -250,9 +250,6 @@ controller_interface::return_type AdmittanceRule::update( convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); std::array pose_error; - // TODO(destogl): remove this feed-forward term from here - // Estimate feedforward acceleration from reference_pose_ik_base_frame_arr_ and previous - std::array feedforward_acceleration; for (auto i = 0u; i < 6; ++i) { pose_error[i] = current_pose_ik_base_frame_arr_[i] - reference_pose_ik_base_frame_arr_[i]; @@ -262,11 +259,7 @@ controller_interface::return_type AdmittanceRule::update( if (std::fabs(pose_error[i]) < POSE_ERROR_EPSILON) { pose_error[i] = 0.0; } - // TODO(destogl): remove this feed-forward term from here - // Estimate feedforward acceleration - feedforward_acceleration[i] = (feedforward_velocity_ik_base_frame_[i] - prev_feedforward_velocity_ik_base_frame_[i]) / period.seconds(); } - prev_feedforward_velocity_ik_base_frame_ = feedforward_velocity_ik_base_frame_; process_wrench_measurements(measured_wrench); @@ -310,13 +303,7 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // TODO(destogl): remove this feed-forward term from here - for (auto i = 0u; i < 6; ++i) { - feedforward_velocity_ik_base_frame_[i] = reference_deltas_vec_ik_base.at(i) / period.seconds(); - } - // Add deltas to previously-desired pose to get the next desired pose - // FIXME: Why not use convert_to_array method? reference_pose_from_joint_deltas_ik_base_frame_.pose.position.x += reference_deltas_vec_ik_base.at(0); reference_pose_from_joint_deltas_ik_base_frame_.pose.position.y += reference_deltas_vec_ik_base.at(1); reference_pose_from_joint_deltas_ik_base_frame_.pose.position.z += reference_deltas_vec_ik_base.at(2); @@ -346,6 +333,7 @@ controller_interface::return_type AdmittanceRule::update( }; // FIXME(destogl): (?) This logic could cause "joy" (large jerk) on contact + // This logic enables if (feedforward_commanded_input_) { if (is_measured_wrench_zero() && !movement_caused_by_wrench_) { for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { From a5a11fc1cdc865befefad29ff9ac3046fc68e4b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Jun 2021 17:02:50 +0200 Subject: [PATCH 80/99] Change approach of calculating absolute desired pose. --- admittance_controller/src/admittance_rule.cpp | 28 +++++++++++++++---- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 3b61730954..2ba73162c3 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -207,7 +207,6 @@ controller_interface::return_type AdmittanceRule::reset() // Therefore desired pose has to be set before calling *update*-method if (open_loop_control_) { get_pose_of_control_frame_in_base_frame(desired_pose_ik_base_frame_); - convert_message_to_array(desired_pose_ik_base_frame_, desired_pose_ik_base_frame_arr_); } // Initialize ik_tip and tool_frame transformations - those are fixed transformations @@ -269,7 +268,25 @@ controller_interface::return_type AdmittanceRule::update( // This works in all cases because not current TF data are used // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - convert_array_to_message(desired_pose_ik_base_frame_arr_, desired_pose_ik_base_frame_); + + // Add deltas to previously-desired pose to get the next desired pose + // TODO(destogl): This houdl also work with transform below... + desired_pose_ik_base_frame_.pose.position.x += relative_desired_pose_arr_.at(0); + desired_pose_ik_base_frame_.pose.position.y += relative_desired_pose_arr_.at(1); + desired_pose_ik_base_frame_.pose.position.z += relative_desired_pose_arr_.at(2); + + tf2::Quaternion q(desired_pose_ik_base_frame_.pose.orientation.x, + desired_pose_ik_base_frame_.pose.orientation.y, + desired_pose_ik_base_frame_.pose.orientation.z, + desired_pose_ik_base_frame_.pose.orientation.w); + tf2::Quaternion q_rot; + q_rot.setRPY(relative_desired_pose_arr_.at(3), relative_desired_pose_arr_.at(4), relative_desired_pose_arr_.at(5)); + q = q_rot * q; + q.normalize(); + desired_pose_ik_base_frame_.pose.orientation.w = q.w(); + desired_pose_ik_base_frame_.pose.orientation.x = q.x(); + desired_pose_ik_base_frame_.pose.orientation.y = q.y(); + desired_pose_ik_base_frame_.pose.orientation.z = q.z(); // tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); @@ -308,7 +325,10 @@ controller_interface::return_type AdmittanceRule::update( reference_pose_from_joint_deltas_ik_base_frame_.pose.position.y += reference_deltas_vec_ik_base.at(1); reference_pose_from_joint_deltas_ik_base_frame_.pose.position.z += reference_deltas_vec_ik_base.at(2); - tf2::Quaternion q(reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x, reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y, reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z, reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w); + tf2::Quaternion q(reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x, + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y, + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z, + reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; q_rot.setRPY(reference_deltas_vec_ik_base.at(3), reference_deltas_vec_ik_base.at(4), reference_deltas_vec_ik_base.at(5)); q = q_rot * q; @@ -498,8 +518,6 @@ void AdmittanceRule::calculate_admittance_rule( if (std::fabs(desired_relative_pose[i]) < POSE_EPSILON) { desired_relative_pose[i] = 0.0; } - desired_pose_ik_base_frame_arr_[i] = - current_pose_ik_base_frame_arr_[i] + desired_relative_pose[i]; // Store values for the next run admittance_acceleration_previous_arr_[i] = admittance_acceleration; From 30cd6f33a364af6c73f017f5139bfe0514056b3f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Jun 2021 17:27:26 +0200 Subject: [PATCH 81/99] Use logic to force stable behavior when no external forces are measured. --- admittance_controller/src/admittance_rule.cpp | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 2ba73162c3..ee9fb60cc8 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -270,15 +270,18 @@ controller_interface::return_type AdmittanceRule::update( convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); // Add deltas to previously-desired pose to get the next desired pose - // TODO(destogl): This houdl also work with transform below... - desired_pose_ik_base_frame_.pose.position.x += relative_desired_pose_arr_.at(0); - desired_pose_ik_base_frame_.pose.position.y += relative_desired_pose_arr_.at(1); - desired_pose_ik_base_frame_.pose.position.z += relative_desired_pose_arr_.at(2); - - tf2::Quaternion q(desired_pose_ik_base_frame_.pose.orientation.x, - desired_pose_ik_base_frame_.pose.orientation.y, - desired_pose_ik_base_frame_.pose.orientation.z, - desired_pose_ik_base_frame_.pose.orientation.w); + // TODO(destogl): This should also work with transform below... + desired_pose_ik_base_frame_.pose.position.x = current_pose_ik_base_frame_.pose.position.x + + relative_desired_pose_arr_.at(0); + desired_pose_ik_base_frame_.pose.position.y = current_pose_ik_base_frame_.pose.position.y + + relative_desired_pose_arr_.at(1); + desired_pose_ik_base_frame_.pose.position.z = current_pose_ik_base_frame_.pose.position.z + + relative_desired_pose_arr_.at(2); + + tf2::Quaternion q(current_pose_ik_base_frame_.pose.orientation.x, + current_pose_ik_base_frame_.pose.orientation.y, + current_pose_ik_base_frame_.pose.orientation.z, + current_pose_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; q_rot.setRPY(relative_desired_pose_arr_.at(3), relative_desired_pose_arr_.at(4), relative_desired_pose_arr_.at(5)); q = q_rot * q; @@ -353,7 +356,8 @@ controller_interface::return_type AdmittanceRule::update( }; // FIXME(destogl): (?) This logic could cause "joy" (large jerk) on contact - // This logic enables + // Please do not delete until we find a solution + // This logic enables to execute feedforward movements without triggering admittance calculation if (feedforward_commanded_input_) { if (is_measured_wrench_zero() && !movement_caused_by_wrench_) { for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { From d616f73dcf550dd685cbd2792f75674b8efbf27e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 7 Jun 2021 08:02:50 -0500 Subject: [PATCH 82/99] Rename the wrench variable (ik_base, not control frame) --- .../admittance_controller/admittance_rule.hpp | 4 ++-- admittance_controller/src/admittance_rule.cpp | 22 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 444967a360..5fe01c475b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -156,7 +156,7 @@ class AdmittanceRule geometry_msgs::msg::WrenchStamped measured_wrench_; geometry_msgs::msg::WrenchStamped measured_wrench_filtered_; - geometry_msgs::msg::WrenchStamped measured_wrench_control_frame_; + geometry_msgs::msg::WrenchStamped measured_wrench_ik_base_frame_; geometry_msgs::msg::WrenchStamped measured_wrench_endeffector_frame_; geometry_msgs::msg::PoseStamped current_pose_ik_base_frame_; @@ -176,7 +176,7 @@ class AdmittanceRule bool movement_caused_by_wrench_ = false; // Pre-reserved update-loop variables - std::array measured_wrench_control_frame_arr_; + std::array measured_wrench_ik_base_frame_arr_; std::array reference_pose_ik_base_frame_arr_; std::array current_pose_ik_base_frame_arr_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index ee9fb60cc8..cd61d1ba47 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -193,7 +193,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared controller_interface::return_type AdmittanceRule::reset() { - measured_wrench_control_frame_arr_.fill(0.0); + measured_wrench_ik_base_frame_arr_.fill(0.0); reference_pose_ik_base_frame_arr_.fill(0.0); current_pose_ik_base_frame_arr_.fill(0.0); admittance_velocity_arr_.fill(0.0); @@ -263,7 +263,7 @@ controller_interface::return_type AdmittanceRule::update( process_wrench_measurements(measured_wrench); calculate_admittance_rule( - measured_wrench_control_frame_arr_, pose_error, period, relative_desired_pose_arr_); + measured_wrench_ik_base_frame_arr_, pose_error, period, relative_desired_pose_arr_); // This works in all cases because not current TF data are used // Do clean conversion to the goal pose using transform and not messing with Euler angles @@ -403,12 +403,12 @@ controller_interface::return_type AdmittanceRule::get_controller_state( 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_control_frame_; + state_message.measured_wrench_control_frame = measured_wrench_ik_base_frame_; // FIXME(destogl): Something is wrong with this transformation - check frames... try { auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); - tf2::doTransform(measured_wrench_control_frame_, measured_wrench_endeffector_frame_, transform); + tf2::doTransform(measured_wrench_ik_base_frame_, measured_wrench_endeffector_frame_, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + @@ -480,19 +480,19 @@ void AdmittanceRule::process_wrench_measurements( // measured_wrench_filtered_ = measured_wrench_; // } - transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_control_frame_); - convert_message_to_array(measured_wrench_control_frame_, measured_wrench_control_frame_arr_); + transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_); + convert_message_to_array(measured_wrench_ik_base_frame_, measured_wrench_ik_base_frame_arr_); // TODO(destogl): optimize this checks! // If at least one measured force is nan set all to 0 - if (std::find_if(measured_wrench_control_frame_arr_.begin(), measured_wrench_control_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_wrench_control_frame_arr_.end()) { - measured_wrench_control_frame_arr_.fill(0.0); + if (std::find_if(measured_wrench_ik_base_frame_arr_.begin(), measured_wrench_ik_base_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_wrench_ik_base_frame_arr_.end()) { + measured_wrench_ik_base_frame_arr_.fill(0.0); } // If a force or a torque is very small set it to 0 - for (auto i = 0u; i < measured_wrench_control_frame_arr_.size(); ++i) { - if (std::fabs(measured_wrench_control_frame_arr_[i]) < WRENCH_EPSILON) { - measured_wrench_control_frame_arr_[i] = 0.0; + for (auto i = 0u; i < measured_wrench_ik_base_frame_arr_.size(); ++i) { + if (std::fabs(measured_wrench_ik_base_frame_arr_[i]) < WRENCH_EPSILON) { + measured_wrench_ik_base_frame_arr_[i] = 0.0; } } } From ec65af43bbc47e0da19c12427f528878134c8e15 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 7 Jun 2021 08:41:25 -0500 Subject: [PATCH 83/99] Use simple forward diff formula. Delete unused vars. Set admittance vel to 0 in ff mode. --- .../admittance_controller/admittance_rule.hpp | 2 -- admittance_controller/src/admittance_rule.cpp | 14 +++----------- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 5fe01c475b..8e69a82d16 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -183,8 +183,6 @@ class AdmittanceRule std::array relative_desired_pose_arr_; std::array desired_pose_ik_base_frame_arr_; std::array admittance_velocity_arr_; - std::array admittance_velocity_previous_arr_; - std::array admittance_acceleration_previous_arr_; std::vector relative_desired_joint_state_vec_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index cd61d1ba47..0e140fbf7a 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -197,8 +197,6 @@ controller_interface::return_type AdmittanceRule::reset() reference_pose_ik_base_frame_arr_.fill(0.0); current_pose_ik_base_frame_arr_.fill(0.0); admittance_velocity_arr_.fill(0.0); - admittance_velocity_previous_arr_.fill(0.0); - admittance_acceleration_previous_arr_.fill(0.0); get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); reference_pose_from_joint_deltas_ik_base_frame_ = current_pose_ik_base_frame_; @@ -363,6 +361,7 @@ controller_interface::return_type AdmittanceRule::update( for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + reference_joint_deltas[i]; desired_joint_state.velocities[i] = reference_joint_deltas[i] / period.seconds(); + admittance_velocity_arr_[i] = 0; } } else { for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { @@ -512,21 +511,14 @@ void AdmittanceRule::calculate_admittance_rule( damping_[i] * admittance_velocity_arr_[i] - stiffness_[i] * pose_error[i]); - // TODO(destogl): Maybe on contact we should not use forward difference - admittance_velocity_arr_[i] += - (admittance_acceleration_previous_arr_[i] + admittance_acceleration) * 0.5 * period.seconds(); + admittance_velocity_arr_[i] += admittance_acceleration * period.seconds(); // Calculate position - desired_relative_pose[i] = - (admittance_velocity_previous_arr_[i] + admittance_velocity_arr_[i]) * 0.5 * period.seconds(); + desired_relative_pose[i] = admittance_velocity_arr_[i] * period.seconds(); if (std::fabs(desired_relative_pose[i]) < POSE_EPSILON) { desired_relative_pose[i] = 0.0; } - // Store values for the next run - admittance_acceleration_previous_arr_[i] = admittance_acceleration; - admittance_velocity_previous_arr_[i] = admittance_velocity_arr_[i]; - // Store data for publishing to state variable admittance_rule_calculated_values_.positions[i] = pose_error[i]; admittance_rule_calculated_values_.velocities[i] = admittance_velocity_arr_[i]; From 478a0e96719978ba11d1124617a9fc341d6b97b7 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 7 Jun 2021 10:28:15 -0500 Subject: [PATCH 84/99] Different "pose error" calculation for open-loop mode. Prevents rotation weirdness. --- .../admittance_controller/admittance_rule.hpp | 10 +-- admittance_controller/src/admittance_rule.cpp | 61 ++++++++----------- 2 files changed, 32 insertions(+), 39 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 8e69a82d16..50c5468de8 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -170,8 +170,8 @@ class AdmittanceRule geometry_msgs::msg::WrenchStamped reference_force_ik_base_frame_; geometry_msgs::msg::PoseStamped reference_pose_ik_base_frame_; - geometry_msgs::msg::PoseStamped desired_pose_ik_base_frame_; - geometry_msgs::msg::TransformStamped relative_desired_pose_; + geometry_msgs::msg::PoseStamped admittance_pose_ik_base_frame_; + geometry_msgs::msg::TransformStamped relative_admittance_pose_; bool movement_caused_by_wrench_ = false; @@ -180,9 +180,11 @@ class AdmittanceRule std::array reference_pose_ik_base_frame_arr_; std::array current_pose_ik_base_frame_arr_; - std::array relative_desired_pose_arr_; - std::array desired_pose_ik_base_frame_arr_; + std::array relative_admittance_pose_arr_; + std::array admittance_pose_ik_base_frame_arr_; std::array admittance_velocity_arr_; + // Keep a running tally of motion due to admittance, to calculate spring force in open-loop mode + std::array sum_of_admittance_displacements_; std::vector relative_desired_joint_state_vec_; diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 0e140fbf7a..55ff13f1e4 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -173,8 +173,8 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize variables used in the update loop measured_wrench_.header.frame_id = sensor_frame_; - relative_desired_pose_.header.frame_id = control_frame_; - relative_desired_pose_.child_frame_id = control_frame_; + relative_admittance_pose_.header.frame_id = control_frame_; + relative_admittance_pose_.child_frame_id = control_frame_; identity_transform_.transform.rotation.w = 1; @@ -197,6 +197,7 @@ controller_interface::return_type AdmittanceRule::reset() reference_pose_ik_base_frame_arr_.fill(0.0); current_pose_ik_base_frame_arr_.fill(0.0); admittance_velocity_arr_.fill(0.0); + sum_of_admittance_displacements_.fill(0.0); get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); reference_pose_from_joint_deltas_ik_base_frame_ = current_pose_ik_base_frame_; @@ -204,7 +205,8 @@ controller_interface::return_type AdmittanceRule::reset() // "Open-loop" controller uses old desired pose as current pose: current_pose(K) = desired_pose(K-1) // Therefore desired pose has to be set before calling *update*-method if (open_loop_control_) { - get_pose_of_control_frame_in_base_frame(desired_pose_ik_base_frame_); + get_pose_of_control_frame_in_base_frame(admittance_pose_ik_base_frame_); + convert_message_to_array(admittance_pose_ik_base_frame_, admittance_pose_ik_base_frame_arr_); } // Initialize ik_tip and tool_frame transformations - those are fixed transformations @@ -239,7 +241,8 @@ controller_interface::return_type AdmittanceRule::update( if (!open_loop_control_) { get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); } else { - current_pose_ik_base_frame_ = desired_pose_ik_base_frame_; + // In open-loop mode, assume the user's requested pose was exactly achieved + current_pose_ik_base_frame_ = reference_pose_ik_base_frame_; } // Convert all data to arrays for simpler calculation @@ -249,7 +252,14 @@ controller_interface::return_type AdmittanceRule::update( std::array pose_error; for (auto i = 0u; i < 6; ++i) { - pose_error[i] = current_pose_ik_base_frame_arr_[i] - reference_pose_ik_base_frame_arr_[i]; + + if (!open_loop_control_) { + pose_error[i] = current_pose_ik_base_frame_arr_[i] - reference_pose_ik_base_frame_arr_[i]; + } else { + // In open-loop mode, spring force is related to the accumulated "admittance displacement" + pose_error[i] = sum_of_admittance_displacements_[i]; + } + if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } @@ -261,35 +271,14 @@ controller_interface::return_type AdmittanceRule::update( process_wrench_measurements(measured_wrench); calculate_admittance_rule( - measured_wrench_ik_base_frame_arr_, pose_error, period, relative_desired_pose_arr_); + measured_wrench_ik_base_frame_arr_, pose_error, period, relative_admittance_pose_arr_); // This works in all cases because not current TF data are used // Do clean conversion to the goal pose using transform and not messing with Euler angles - convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); + convert_array_to_message(relative_admittance_pose_arr_, relative_admittance_pose_); // Add deltas to previously-desired pose to get the next desired pose - // TODO(destogl): This should also work with transform below... - desired_pose_ik_base_frame_.pose.position.x = current_pose_ik_base_frame_.pose.position.x + - relative_desired_pose_arr_.at(0); - desired_pose_ik_base_frame_.pose.position.y = current_pose_ik_base_frame_.pose.position.y + - relative_desired_pose_arr_.at(1); - desired_pose_ik_base_frame_.pose.position.z = current_pose_ik_base_frame_.pose.position.z + - relative_desired_pose_arr_.at(2); - - tf2::Quaternion q(current_pose_ik_base_frame_.pose.orientation.x, - current_pose_ik_base_frame_.pose.orientation.y, - current_pose_ik_base_frame_.pose.orientation.z, - current_pose_ik_base_frame_.pose.orientation.w); - tf2::Quaternion q_rot; - q_rot.setRPY(relative_desired_pose_arr_.at(3), relative_desired_pose_arr_.at(4), relative_desired_pose_arr_.at(5)); - q = q_rot * q; - q.normalize(); - desired_pose_ik_base_frame_.pose.orientation.w = q.w(); - desired_pose_ik_base_frame_.pose.orientation.x = q.x(); - desired_pose_ik_base_frame_.pose.orientation.y = q.y(); - desired_pose_ik_base_frame_.pose.orientation.z = q.z(); - -// tf2::doTransform(current_pose_ik_base_frame_, desired_pose_ik_base_frame_, relative_desired_pose_); + tf2::doTransform(current_pose_ik_base_frame_, admittance_pose_ik_base_frame_, relative_admittance_pose_); return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); } @@ -350,7 +339,7 @@ controller_interface::return_type AdmittanceRule::update( }; auto is_relative_admittance_pose_zero = [&]() { - return (accumulate_absolute(relative_desired_pose_arr_) < POSE_EPSILON); + return (accumulate_absolute(relative_admittance_pose_arr_) < POSE_EPSILON); }; // FIXME(destogl): (?) This logic could cause "joy" (large jerk) on contact @@ -418,8 +407,8 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.admittance_rule_calculated_values = admittance_rule_calculated_values_; state_message.current_pose = current_pose_ik_base_frame_; - state_message.desired_pose = desired_pose_ik_base_frame_; - state_message.relative_desired_pose = relative_desired_pose_; + state_message.desired_pose = admittance_pose_ik_base_frame_; + state_message.relative_desired_pose = relative_admittance_pose_; return controller_interface::return_type::OK; } @@ -519,6 +508,8 @@ void AdmittanceRule::calculate_admittance_rule( desired_relative_pose[i] = 0.0; } + sum_of_admittance_displacements_[i] += admittance_velocity_arr_[i] * period.seconds(); + // Store data for publishing to state variable admittance_rule_calculated_values_.positions[i] = pose_error[i]; admittance_rule_calculated_values_.velocities[i] = admittance_velocity_arr_[i]; @@ -534,16 +525,16 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); + convert_array_to_message(relative_admittance_pose_arr_, relative_admittance_pose_); // Since ik_base is MoveIt's working frame, the transform is identity. identity_transform_.header.frame_id = ik_base_frame_; // Use Jacobian-based IK - std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); + std::vector relative_admittance_pose_vec(relative_admittance_pose_arr_.begin(), relative_admittance_pose_arr_.end()); ik_->update_robot_state(current_joint_state); if (ik_->convert_cartesian_deltas_to_joint_deltas( - relative_desired_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ + relative_admittance_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; From 1b7a6791603361e1a7f0d37fde81922cdb661339 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 7 Jun 2021 23:25:14 +0200 Subject: [PATCH 85/99] Resort calculation of admittance displacement to have it in one place --- admittance_controller/src/admittance_rule.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 55ff13f1e4..92dc4b8009 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -242,6 +242,7 @@ controller_interface::return_type AdmittanceRule::update( get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); } else { // In open-loop mode, assume the user's requested pose was exactly achieved + // TODO(destogl): This will maybe now work when no feed-forward is used current_pose_ik_base_frame_ = reference_pose_ik_base_frame_; } @@ -256,6 +257,8 @@ controller_interface::return_type AdmittanceRule::update( if (!open_loop_control_) { pose_error[i] = current_pose_ik_base_frame_arr_[i] - reference_pose_ik_base_frame_arr_[i]; } else { + // Sum admittance displacement from the previous relative poses + sum_of_admittance_displacements_[i] += relative_admittance_pose_arr_[i]; // In open-loop mode, spring force is related to the accumulated "admittance displacement" pose_error[i] = sum_of_admittance_displacements_[i]; } @@ -508,8 +511,6 @@ void AdmittanceRule::calculate_admittance_rule( desired_relative_pose[i] = 0.0; } - sum_of_admittance_displacements_[i] += admittance_velocity_arr_[i] * period.seconds(); - // Store data for publishing to state variable admittance_rule_calculated_values_.positions[i] = pose_error[i]; admittance_rule_calculated_values_.velocities[i] = admittance_velocity_arr_[i]; From ef0cda8b027ab01b11c2c7e4aa192f5cbdd295e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 7 Jun 2021 23:49:30 +0200 Subject: [PATCH 86/99] Do not initialize memory in update joint-deltas method. Simplify calculation of the refrence pose from the reference deltas. --- .../admittance_controller/admittance_rule.hpp | 5 +++ admittance_controller/src/admittance_rule.cpp | 32 +++++++------------ 2 files changed, 17 insertions(+), 20 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 50c5468de8..f8c660b466 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -173,6 +173,11 @@ class AdmittanceRule geometry_msgs::msg::PoseStamped admittance_pose_ik_base_frame_; geometry_msgs::msg::TransformStamped relative_admittance_pose_; + // Joint deltas calculation variables + std::vector reference_joint_deltas_vec_; + std::vector reference_deltas_vec_ik_base_; + geometry_msgs::msg::TransformStamped reference_deltas_ik_base_; + bool movement_caused_by_wrench_ = false; // Pre-reserved update-loop variables diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 92dc4b8009..42705b9ef2 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -176,6 +176,11 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared relative_admittance_pose_.header.frame_id = control_frame_; relative_admittance_pose_.child_frame_id = control_frame_; + reference_joint_deltas_vec_.reserve(6); + reference_deltas_vec_ik_base_.reserve(6); + reference_deltas_ik_base_.header.frame_id = ik_base_frame_; + reference_deltas_ik_base_.child_frame_id = ik_base_frame_; + identity_transform_.transform.rotation.w = 1; relative_desired_joint_state_vec_.reserve(6); @@ -294,16 +299,14 @@ controller_interface::return_type AdmittanceRule::update( const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) { - std::vector reference_joint_deltas_vec( - reference_joint_deltas.begin(), reference_joint_deltas.end()); - std::vector reference_deltas_vec_ik_base(6); + reference_joint_deltas_vec_.assign(reference_joint_deltas.begin(), reference_joint_deltas.end()); // Get feed-forward cartesian deltas in the ik_base frame. // Since ik_base is MoveIt's working frame, the transform is identity. identity_transform_.header.frame_id = ik_base_frame_; ik_->update_robot_state(current_joint_state); if (!ik_->convert_joint_deltas_to_cartesian_deltas( - reference_joint_deltas_vec, identity_transform_, reference_deltas_vec_ik_base)) + reference_joint_deltas_vec_, identity_transform_, reference_deltas_vec_ik_base_)) { RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of joint deltas to Cartesian deltas failed. Sending current joint" @@ -313,23 +316,12 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } + convert_array_to_message(reference_deltas_vec_ik_base_, reference_deltas_ik_base_); + // Add deltas to previously-desired pose to get the next desired pose - reference_pose_from_joint_deltas_ik_base_frame_.pose.position.x += reference_deltas_vec_ik_base.at(0); - reference_pose_from_joint_deltas_ik_base_frame_.pose.position.y += reference_deltas_vec_ik_base.at(1); - reference_pose_from_joint_deltas_ik_base_frame_.pose.position.z += reference_deltas_vec_ik_base.at(2); - - tf2::Quaternion q(reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x, - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y, - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z, - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w); - tf2::Quaternion q_rot; - q_rot.setRPY(reference_deltas_vec_ik_base.at(3), reference_deltas_vec_ik_base.at(4), reference_deltas_vec_ik_base.at(5)); - q = q_rot * q; - q.normalize(); - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.w = q.w(); - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.x = q.x(); - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.y = q.y(); - reference_pose_from_joint_deltas_ik_base_frame_.pose.orientation.z = q.z(); + tf2::doTransform(reference_pose_from_joint_deltas_ik_base_frame_, + reference_pose_from_joint_deltas_ik_base_frame_, + reference_deltas_ik_base_); update(current_joint_state, measured_wrench, reference_pose_from_joint_deltas_ik_base_frame_, period, desired_joint_state); From 3a5e1113737bb358ade68b9430738c4130c7d2ad Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 8 Jun 2021 09:42:06 -0500 Subject: [PATCH 87/99] Comment gravity comp parameter parsing, for now --- .../src/admittance_controller.cpp | 58 +++++++++---------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 3f8c1c13ac..f6fe10aa53 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -92,19 +92,19 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("admittance.stiffness.ry", std::numeric_limits::quiet_NaN()); get_node()->declare_parameter("admittance.stiffness.rz", std::numeric_limits::quiet_NaN()); - // TODO(destogl): use filters here to make it more flexible - // declare and read Gravity compensation parameters - for (const auto & name : std::vector{"wrist", "tool"}) { - get_node()->declare_parameter("gravity_compensation." + name + ".CoG_x", - std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("gravity_compensation." + name + ".CoG_y", - std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("gravity_compensation." + name + ".CoG_z", - std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("gravity_compensation." + name + ".force", - std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("gravity_compensation." + name + ".frame_id", ""); - } + // // TODO(destogl): use filters here to make it more flexible + // // declare and read Gravity compensation parameters + // for (const auto & name : std::vector{"wrist", "tool"}) { + // get_node()->declare_parameter("gravity_compensation." + name + ".CoG_x", + // std::numeric_limits::quiet_NaN()); + // get_node()->declare_parameter("gravity_compensation." + name + ".CoG_y", + // std::numeric_limits::quiet_NaN()); + // get_node()->declare_parameter("gravity_compensation." + name + ".CoG_z", + // std::numeric_limits::quiet_NaN()); + // get_node()->declare_parameter("gravity_compensation." + name + ".force", + // std::numeric_limits::quiet_NaN()); + // get_node()->declare_parameter("gravity_compensation." + name + ".frame_id", ""); + // } // get_node()->declare_parameter>("") } catch (const std::exception & e) { @@ -205,22 +205,22 @@ CallbackReturn AdmittanceController::on_configure( return CallbackReturn::ERROR; } - admittance_->gravity_compensation_params_.reserve(2); - for (const auto & name : std::vector{"wrist", "tool"}) { - GravityCompensationParameters params; - if ( - get_double_param_and_error_if_empty(params.cog_.vector.x, ("gravity_compensation." + name + ".CoG_x").c_str()) || - get_double_param_and_error_if_empty(params.cog_.vector.y, ("gravity_compensation." + name + ".CoG_y").c_str()) || - get_double_param_and_error_if_empty(params.cog_.vector.z, ("gravity_compensation." + name + ".CoG_z").c_str()) || - get_double_param_and_error_if_empty(params.force_, ("gravity_compensation." + name + ".force").c_str()) || - get_string_param_and_error_if_empty(params.cog_.header.frame_id, ("gravity_compensation." + name + ".frame_id").c_str()) - ) - { - return CallbackReturn::ERROR; - } - - admittance_->gravity_compensation_params_.emplace_back(params); - } + // admittance_->gravity_compensation_params_.reserve(2); + // for (const auto & name : std::vector{"wrist", "tool"}) { + // GravityCompensationParameters params; + // if ( + // get_double_param_and_error_if_empty(params.cog_.vector.x, ("gravity_compensation." + name + ".CoG_x").c_str()) || + // get_double_param_and_error_if_empty(params.cog_.vector.y, ("gravity_compensation." + name + ".CoG_y").c_str()) || + // get_double_param_and_error_if_empty(params.cog_.vector.z, ("gravity_compensation." + name + ".CoG_z").c_str()) || + // get_double_param_and_error_if_empty(params.force_, ("gravity_compensation." + name + ".force").c_str()) || + // get_string_param_and_error_if_empty(params.cog_.header.frame_id, ("gravity_compensation." + name + ".frame_id").c_str()) + // ) + // { + // return CallbackReturn::ERROR; + // } + + // admittance_->gravity_compensation_params_.emplace_back(params); + // } // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation From 62f7671145ae6bb33ce616f05e2047d44a2c9a1c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 8 Jun 2021 16:05:10 -0500 Subject: [PATCH 88/99] Delete unused EPSILON variables --- admittance_controller/src/admittance_rule.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 42705b9ef2..fbd90085bd 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -34,8 +34,6 @@ 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; -static constexpr double VELOCITITY_EPSILON = 1e-20; -static constexpr double ACCELERATION_EPSILON = 1e-20; template void convert_message_to_array(const geometry_msgs::msg::Pose & msg, Type & vector_out) @@ -463,6 +461,7 @@ void AdmittanceRule::process_wrench_measurements( // measured_wrench_filtered_ = measured_wrench_; // } + // TODO(andyz): This is not flexible to work in other control frames besides ik_base transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_); convert_message_to_array(measured_wrench_ik_base_frame_, measured_wrench_ik_base_frame_arr_); From a0f275f359657d093758742a840f86c2b68d237b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 8 Jun 2021 00:42:30 +0200 Subject: [PATCH 89/99] Add admittance controller compile options --- admittance_controller/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index cd80d5c8c9..20e55d4e50 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -59,6 +59,7 @@ ament_target_dependencies( tf2_geometry_msgs tf2_ros ) +target_compile_options(admittance_controller PRIVATE -Wall -Wextra) # prevent pluginlib from using boost target_compile_definitions(admittance_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) From 726c1a763b79d4e99691acf1f8aed787ebb6338a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 8 Jun 2021 01:07:08 +0200 Subject: [PATCH 90/99] Add filter_chain_ definition --- admittance_controller/CMakeLists.txt | 7 ++++++- .../include/admittance_controller/admittance_rule.hpp | 5 +++-- admittance_controller/package.xml | 1 + 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 20e55d4e50..942bdd2afb 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) find_package(Eigen3 REQUIRED) +find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) find_package(moveit_ros_move_group REQUIRED) @@ -46,6 +47,7 @@ ament_target_dependencies( control_msgs controller_interface ${Eigen_LIBRARIES} + filters geometry_msgs hardware_interface moveit_ros_move_group @@ -59,7 +61,9 @@ ament_target_dependencies( tf2_geometry_msgs tf2_ros ) -target_compile_options(admittance_controller PRIVATE -Wall -Wextra) +# 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") # prevent pluginlib from using boost target_compile_definitions(admittance_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) @@ -112,6 +116,7 @@ ament_export_libraries( ament_export_dependencies( control_msgs controller_interface + filters geometry_msgs hardware_interface pluginlib diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index f8c660b466..699ba68954 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -20,6 +20,7 @@ #include "admittance_controller/moveit_kinematics.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -111,8 +112,8 @@ class AdmittanceRule std::array damping_; std::array stiffness_; - // Filters - std::vector gravity_compensation_params_; + // Filter chain for Wrench data + std::unique_ptr> filter_chain_; protected: void process_wrench_measurements( diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index a89f54a7fc..e65c2fc7c9 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -12,6 +12,7 @@ control_msgs controller_interface + filters geometry_msgs hardware_interface moveit_ros_move_group From 400a3746307e1c019e860f37cb82a20711a2a1b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 8 Jun 2021 01:10:09 +0200 Subject: [PATCH 91/99] Add filter chain to admittance controller --- .../src/admittance_controller.cpp | 37 +++++++++++-------- admittance_controller/src/admittance_rule.cpp | 8 ++-- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index f6fe10aa53..407b84efb0 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -25,6 +25,8 @@ #include "angles/angles.h" #include "controller_interface/helpers.hpp" +#include "filters/filter_chain.hpp" +#include "geometry_msgs/msg/wrench.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -205,22 +207,25 @@ CallbackReturn AdmittanceController::on_configure( return CallbackReturn::ERROR; } - // admittance_->gravity_compensation_params_.reserve(2); - // for (const auto & name : std::vector{"wrist", "tool"}) { - // GravityCompensationParameters params; - // if ( - // get_double_param_and_error_if_empty(params.cog_.vector.x, ("gravity_compensation." + name + ".CoG_x").c_str()) || - // get_double_param_and_error_if_empty(params.cog_.vector.y, ("gravity_compensation." + name + ".CoG_y").c_str()) || - // get_double_param_and_error_if_empty(params.cog_.vector.z, ("gravity_compensation." + name + ".CoG_z").c_str()) || - // get_double_param_and_error_if_empty(params.force_, ("gravity_compensation." + name + ".force").c_str()) || - // get_string_param_and_error_if_empty(params.cog_.header.frame_id, ("gravity_compensation." + name + ".frame_id").c_str()) - // ) - // { - // return CallbackReturn::ERROR; - // } - - // admittance_->gravity_compensation_params_.emplace_back(params); - // } + try { + admittance_->filter_chain_ = + std::make_unique>( + "geometry_msgs::msg::WrenchStamped"); + } catch (const std::exception & e) { + fprintf( + stderr, "Exception thrown during filter chain creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } + + if (!admittance_->filter_chain_->configure("input_wrench_filter_chain", + get_node()->get_node_logging_interface(), get_node()->get_node_parameters_interface())) + { + RCLCPP_ERROR(get_node()->get_logger(), + "Could not configure sensor filter chain, please check if the " + "parameters are provided correctly."); + return CallbackReturn::ERROR; + } // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index fbd90085bd..6cfa0b1f15 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -429,9 +429,8 @@ void AdmittanceRule::process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench ) { - // TODO(andyz): Implement gravity comp. For now, just pass the measured wrench through measured_wrench_.wrench = measured_wrench; - measured_wrench_filtered_ = measured_wrench_; + filter_chain_->update(measured_wrench_, measured_wrench_filtered_); // // get current states, and transform those into controller frame // measured_wrench_.wrench = measured_wrench; @@ -467,7 +466,10 @@ void AdmittanceRule::process_wrench_measurements( // TODO(destogl): optimize this checks! // If at least one measured force is nan set all to 0 - if (std::find_if(measured_wrench_ik_base_frame_arr_.begin(), measured_wrench_ik_base_frame_arr_.end(), [](const auto value){ return std::isnan(value); }) != measured_wrench_ik_base_frame_arr_.end()) { + if (std::find_if(measured_wrench_ik_base_frame_arr_.begin(), + measured_wrench_ik_base_frame_arr_.end(), + [](const auto value){ return std::isnan(value); }) != measured_wrench_ik_base_frame_arr_.end()) + { measured_wrench_ik_base_frame_arr_.fill(0.0); } From dde2cb204854420a96f073550872efae784aa489 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Tue, 8 Jun 2021 23:29:29 +0200 Subject: [PATCH 92/99] Enalbe filters by moving admittance_rule impl to a header file. --- admittance_controller/CMakeLists.txt | 1 - .../admittance_controller/admittance_rule_impl.hpp} | 0 admittance_controller/src/admittance_controller.cpp | 1 + 3 files changed, 1 insertion(+), 1 deletion(-) rename admittance_controller/{src/admittance_rule.cpp => include/admittance_controller/admittance_rule_impl.hpp} (100%) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 942bdd2afb..d9e1a65aef 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -32,7 +32,6 @@ add_library( admittance_controller SHARED src/admittance_controller.cpp - src/admittance_rule.cpp src/moveit_kinematics.cpp ) target_include_directories( diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp similarity index 100% rename from admittance_controller/src/admittance_rule.cpp rename to admittance_controller/include/admittance_controller/admittance_rule_impl.hpp diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 407b84efb0..c32bb9f3a0 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -23,6 +23,7 @@ #include #include +#include "admittance_controller/admittance_rule_impl.hpp" #include "angles/angles.h" #include "controller_interface/helpers.hpp" #include "filters/filter_chain.hpp" From 6d67a1a13f227c427f5693938b0fd4ca6d472fd8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 9 Jun 2021 13:17:26 +0200 Subject: [PATCH 93/99] Remove protection logic since it is not needed anymore --- .../admittance_rule_impl.hpp | 35 ++----------------- 1 file changed, 3 insertions(+), 32 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 6cfa0b1f15..2c35536200 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -324,38 +324,9 @@ controller_interface::return_type AdmittanceRule::update( update(current_joint_state, measured_wrench, reference_pose_from_joint_deltas_ik_base_frame_, period, desired_joint_state); - auto is_measured_wrench_zero = [&]() { - std::array measured_wrench_arr; - convert_message_to_array(measured_wrench, measured_wrench_arr); - double accumulated = accumulate_absolute(measured_wrench_arr); - return (accumulated < WRENCH_EPSILON || std::isnan(accumulated)); - }; - - auto is_relative_admittance_pose_zero = [&]() { - return (accumulate_absolute(relative_admittance_pose_arr_) < POSE_EPSILON); - }; - - // FIXME(destogl): (?) This logic could cause "joy" (large jerk) on contact - // Please do not delete until we find a solution - // This logic enables to execute feedforward movements without triggering admittance calculation - if (feedforward_commanded_input_) { - if (is_measured_wrench_zero() && !movement_caused_by_wrench_) { - for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { - desired_joint_state.positions[i] = current_joint_state.positions[i] + reference_joint_deltas[i]; - desired_joint_state.velocities[i] = reference_joint_deltas[i] / period.seconds(); - admittance_velocity_arr_[i] = 0; - } - } else { - for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { - desired_joint_state.positions[i] += reference_joint_deltas[i]; - desired_joint_state.velocities[i] += reference_joint_deltas[i] / period.seconds(); - } - if (is_relative_admittance_pose_zero()) { - movement_caused_by_wrench_ = false; - } else { - movement_caused_by_wrench_ = true; - } - } + for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { + desired_joint_state.positions[i] += reference_joint_deltas[i]; + desired_joint_state.velocities[i] += reference_joint_deltas[i] / period.seconds(); } return controller_interface::return_type::OK; From 6ae99efebe3fd298594b20d40abe748b54c32aa8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 9 Jun 2021 13:29:05 +0200 Subject: [PATCH 94/99] Remove unused code --- .../admittance_rule_impl.hpp | 28 ------------------- .../src/admittance_controller.cpp | 15 ---------- 2 files changed, 43 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 2c35536200..81afeedb90 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -403,34 +403,6 @@ void AdmittanceRule::process_wrench_measurements( measured_wrench_.wrench = measured_wrench; filter_chain_->update(measured_wrench_, measured_wrench_filtered_); - // // get current states, and transform those into controller frame - // measured_wrench_.wrench = measured_wrench; - // try { - // auto transform_to_world = tf_buffer_->lookupTransform(fixed_world_frame_, measured_wrench_.header.frame_id, tf2::TimePointZero); - // auto transform_to_sensor = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero); - - // geometry_msgs::msg::WrenchStamped measured_wrench_transformed; - // tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform_to_world); - - // geometry_msgs::msg::Vector3Stamped cog_transformed; - // for (const auto & params : gravity_compensation_params_) { - // auto transform_cog = tf_buffer_->lookupTransform(fixed_world_frame_, params.cog_.header.frame_id, tf2::TimePointZero); - // tf2::doTransform(params.cog_, cog_transformed, transform_cog); - - // measured_wrench_transformed.wrench.force.z += params.force_; - // measured_wrench_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y); - // measured_wrench_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x); - // } - - // tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_to_sensor); - - // } catch (const tf2::TransformException & e) { - // // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - // RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_wrench_.header.frame_id + "' or ''."); - // // If transform error just use measured force - // measured_wrench_filtered_ = measured_wrench_; - // } - // TODO(andyz): This is not flexible to work in other control frames besides ik_base transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_); convert_message_to_array(measured_wrench_ik_base_frame_, measured_wrench_ik_base_frame_arr_); diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c32bb9f3a0..3a52c4eebd 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -95,21 +95,6 @@ controller_interface::return_type AdmittanceController::init(const std::string & get_node()->declare_parameter("admittance.stiffness.ry", std::numeric_limits::quiet_NaN()); get_node()->declare_parameter("admittance.stiffness.rz", std::numeric_limits::quiet_NaN()); - // // TODO(destogl): use filters here to make it more flexible - // // declare and read Gravity compensation parameters - // for (const auto & name : std::vector{"wrist", "tool"}) { - // get_node()->declare_parameter("gravity_compensation." + name + ".CoG_x", - // std::numeric_limits::quiet_NaN()); - // get_node()->declare_parameter("gravity_compensation." + name + ".CoG_y", - // std::numeric_limits::quiet_NaN()); - // get_node()->declare_parameter("gravity_compensation." + name + ".CoG_z", - // std::numeric_limits::quiet_NaN()); - // get_node()->declare_parameter("gravity_compensation." + name + ".force", - // std::numeric_limits::quiet_NaN()); - // get_node()->declare_parameter("gravity_compensation." + name + ".frame_id", ""); - // } - -// get_node()->declare_parameter>("") } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; From ef394503c29f86a538c58e15f14681d9fd46e689 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 21 Oct 2021 16:03:09 +0200 Subject: [PATCH 95/99] Add Admittance Controller from 79a2dfe78c73f9a99e899f4bb400424783a202ca (destogl:ros2-controllers-extension5) branch Co-authored-by: Nathan Brooks Co-authored-by: AndyZe --- admittance_controller/CMakeLists.txt | 12 +- .../admittance_controller.hpp | 16 +- ...mittance_controller_cartesian_commands.hpp | 41 ++ .../admittance_controller/admittance_rule.hpp | 483 +++++++++++++++--- .../admittance_rule_impl.hpp | 304 ++++------- .../moveit_kinematics.hpp | 12 +- admittance_controller/package.xml | 3 +- .../src/admittance_controller.cpp | 237 ++++----- .../src/moveit_kinematics.cpp | 84 ++- .../test/test_admittance_controller.cpp | 44 +- .../test/test_admittance_controller.hpp | 2 +- 11 files changed, 788 insertions(+), 450 deletions(-) create mode 100644 admittance_controller/include/admittance_controller/admittance_controller_cartesian_commands.hpp diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index d9e1a65aef..4c10e4b0d1 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(Eigen3 REQUIRED) find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) +find_package(joint_limits REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -36,10 +37,8 @@ add_library( ) target_include_directories( admittance_controller - PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} + PRIVATE + include ) ament_target_dependencies( admittance_controller @@ -49,6 +48,7 @@ ament_target_dependencies( filters geometry_msgs hardware_interface + joint_limits moveit_ros_move_group moveit_ros_planning_interface pluginlib @@ -63,8 +63,7 @@ ament_target_dependencies( # 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") -# prevent pluginlib from using boost -target_compile_definitions(admittance_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) install( @@ -118,6 +117,7 @@ ament_export_dependencies( filters geometry_msgs hardware_interface + joint_limits pluginlib rclcpp rclcpp_lifecycle diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 99ad7c2025..1be4829e64 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -29,11 +29,14 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "semantic_components/force_torque_sensor.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.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" // 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" @@ -48,7 +51,7 @@ class AdmittanceController : public controller_interface::ControllerInterface AdmittanceController(); ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; + CallbackReturn on_init() override; ADMITTANCE_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -74,10 +77,16 @@ class AdmittanceController : public controller_interface::ControllerInterface std::vector state_interface_types_; std::string ft_sensor_name_; bool use_joint_commands_as_input_; + std::string joint_limiter_type_; bool hardware_state_has_offset_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + // joint limiter + using JointLimiter = joint_limits::JointLimiterInterface; + std::shared_ptr> joint_limiter_loader_; + std::unique_ptr joint_limiter_; + // Internal variables std::unique_ptr force_torque_sensor_; @@ -85,6 +94,9 @@ class AdmittanceController : public controller_interface::ControllerInterface std::unique_ptr admittance_; rclcpp::Time previous_time_; + // Callback for updating dynamic parameters + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_; + // Command subscribers and Controller State publisher using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; diff --git a/admittance_controller/include/admittance_controller/admittance_controller_cartesian_commands.hpp b/admittance_controller/include/admittance_controller/admittance_controller_cartesian_commands.hpp new file mode 100644 index 0000000000..590e9115b3 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_controller_cartesian_commands.hpp @@ -0,0 +1,41 @@ + +// Header!! +std::string ik_tip_frame_; + +// Frame which position should be controlled +std::string endeffector_frame_; + +tf2::Transform ik_tip_to_control_frame_tf_; +tf2::Transform control_frame_to_ik_tip_tf_; + +// CPP!! +controller_interface::return_type AdmittanceRule::reset() +{ + // Initialize ik_tip and tool_frame transformations - those are fixed transformations + tf2::Stamped tf2_transform; + try { + auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, control_frame_, tf2::TimePointZero); + tf2::fromMsg(transform, tf2_transform); + ik_tip_to_control_frame_tf_ = tf2_transform; + control_frame_to_ik_tip_tf_ = tf2_transform.inverse(); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); + return controller_interface::return_type::ERROR; + } +} + + +controller_interface::return_type AdmittanceRule::get_controller_state( + control_msgs::msg::AdmittanceControllerState & state_message) +{ + // FIXME(destogl): Something is wrong with this transformation - check frames... + try { + auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); + tf2::doTransform(measured_wrench_ik_base_frame_, measured_wrench_endeffector_frame_, transform); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); + } + state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_; +} diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 699ba68954..a0c48a1bad 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -17,30 +17,345 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#include + +#include "angles/angles.h" #include "admittance_controller/moveit_kinematics.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" +#include "controller_interface/controller_parameters.hpp" #include "filters/filter_chain.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -// TODO(destogl): Enable use of filters -// #include "iirob_filters/gravity_compensation.h" +#include "rcutils/logging_macros.h" #include "tf2_ros/transform_listener.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include + +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; + +template +void convert_message_to_array(const geometry_msgs::msg::Pose & msg, Type & vector_out) +{ + vector_out[0] = msg.position.x; + vector_out[1] = msg.position.y; + vector_out[2] = msg.position.z; + tf2::Quaternion q; + tf2::fromMsg(msg.orientation, q); + q.normalize(); + tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); + for (auto i = 3u; i < 6; ++i) { + vector_out[i] = angles::normalize_angle(vector_out[i]); + } +} + +template +void convert_message_to_array( + const geometry_msgs::msg::PoseStamped & msg, Type & vector_out) +{ + convert_message_to_array(msg.pose, vector_out); +} + +template +void convert_message_to_array(const geometry_msgs::msg::Transform & msg, Type & vector_out) +{ + vector_out[0] = msg.translation.x; + vector_out[1] = msg.translation.y; + vector_out[2] = msg.translation.z; + tf2::Quaternion q; + tf2::fromMsg(msg.rotation, q); + q.normalize(); + tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); + for (auto i = 3u; i < 6; ++i) { + vector_out[i] = angles::normalize_angle(vector_out[i]); + } +} + +template +void convert_message_to_array(const geometry_msgs::msg::TransformStamped & msg, Type & vector_out) +{ + convert_message_to_array(msg.transform, vector_out); +} + +template +void convert_message_to_array( + const geometry_msgs::msg::Wrench & msg, Type & vector_out) +{ + vector_out[0] = msg.force.x; + vector_out[1] = msg.force.y; + vector_out[2] = msg.force.z; + vector_out[3] = msg.torque.x; + vector_out[4] = msg.torque.y; + vector_out[5] = msg.torque.z; +} + +template +void convert_message_to_array( + const geometry_msgs::msg::WrenchStamped & msg, Type & vector_out) +{ + convert_message_to_array(msg.wrench, vector_out); +} + +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::Pose & msg_out) +{ + msg_out.position.x = vector[0]; + msg_out.position.y = vector[1]; + msg_out.position.z = vector[2]; + + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.orientation.x = q.x(); + msg_out.orientation.y = q.y(); + msg_out.orientation.z = q.z(); + msg_out.orientation.w = q.w(); +} + +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::PoseStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.pose); +} + +// template +// void convert_array_to_message(const Type & vector, geometry_msgs::msg::Wrench & msg_out) +// { +// msg_out.force.x = vector[0]; +// msg_out.force.y = vector[1]; +// msg_out.force.z = vector[2]; +// msg_out.torque.x = vector[3]; +// msg_out.torque.y = vector[4]; +// msg_out.torque.z = vector[5]; +// } + +// template +// void convert_array_to_message(const Type & vector, geometry_msgs::msg::WrenchStamped & msg_out) +// { +// convert_array_to_message(vector, msg_out.wrench); +// } + +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::Transform & msg_out) +{ + msg_out.translation.x = vector[0]; + msg_out.translation.y = vector[1]; + msg_out.translation.z = vector[2]; + + tf2::Quaternion q; + q.setRPY(vector[3], vector[4], vector[5]); + + msg_out.rotation.x = q.x(); + msg_out.rotation.y = q.y(); + msg_out.rotation.z = q.z(); + msg_out.rotation.w = q.w(); +} + +template +void convert_array_to_message(const Type & vector, geometry_msgs::msg::TransformStamped & msg_out) +{ + convert_array_to_message(vector, msg_out.transform); +} + +} // utility namespace namespace admittance_controller { -struct GravityCompensationParameters + +class AdmittanceParameters : public controller_interface::ControllerParameters { public: - std::string world_frame_; + AdmittanceParameters() : controller_interface::ControllerParameters(7, 24, 4) + { + add_string_parameter("IK.base", false); + add_string_parameter("IK.group_name", false); + add_string_parameter("control_frame", true); + add_string_parameter("sensor_frame", false); + + add_bool_parameter("open_loop_control", true); + + 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 = 1; + // check if parameters are all properly set for selected axes + for (auto i = 0ul; 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() 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()); + control_frame_ = string_parameters_[2].second; + RCUTILS_LOG_INFO_NAMED( + logger_name_.c_str(), + "Control frame: %s", control_frame_.c_str()); + sensor_frame_ = string_parameters_[3].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")); + + for (auto i = 0ul; i < 6; ++i) + { + selected_axes_[i] = bool_parameters_[i+1].second; // +1 because there is already one parameter + 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_; + // Admittance calculations (displacement etc) are done in this frame. + // Frame where wrench measurements are taken std::string sensor_frame_; - geometry_msgs::msg::Vector3Stamped cog_; - double force_; + // Depends on the scenario: usually base_link, tool or end-effector + std::string control_frame_; + + bool open_loop_control_; + + std::array damping_; + std::array damping_ratio_; + std::array mass_; + std::array selected_axes_; + std::array stiffness_; }; + class AdmittanceRule { public: @@ -82,35 +397,18 @@ class AdmittanceRule controller_interface::return_type get_pose_of_control_frame_in_base_frame(geometry_msgs::msg::PoseStamped & pose); public: - bool open_loop_control_ = false; // TODO(destogl): Add parameter for this bool feedforward_commanded_input_ = true; - // IK related parameters - // ik_base_frame should be stationary so vel/accel calculations are correct - std::string ik_base_frame_; - std::string ik_tip_frame_; - std::string ik_group_name_; - - // Frame which position should be controlled - std::string endeffector_frame_; - // Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - std::string control_frame_; - // Gravity points down (neg. Z) in the world frame - std::string fixed_world_frame_; - // Frame where wrench measurements are taken - std::string sensor_frame_; - // An identity matrix is needed in several places geometry_msgs::msg::TransformStamped identity_transform_; // Admittance parameters // TODO(destogl): unified mode does not have to be here bool unified_mode_ = false; // Unified mode enables simultaneous force and position goals - std::array selected_axes_; - std::array mass_; - std::array damping_; - std::array stiffness_; + + // Dynamic admittance parameters + AdmittanceParameters parameters_; // Filter chain for Wrench data std::unique_ptr> filter_chain_; @@ -132,6 +430,7 @@ class AdmittanceRule controller_interface::return_type calculate_desired_joint_state( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const std::array & relative_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ); @@ -139,28 +438,21 @@ class AdmittanceRule // IK variables std::shared_ptr ik_; - // Filters -// using GravityCompensatorType = -// iirob_filters::GravityCompensator; -// -// std::unique_ptr wrist_gravity_compensator_; -// std::unique_ptr tool_gravity_compensator_; + // Clock + rclcpp::Clock::SharedPtr clock_; // Transformation variables std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - tf2::Transform ik_tip_to_control_frame_tf_; - tf2::Transform control_frame_to_ik_tip_tf_; - // measured_wrench_ could arrive in any frame. It will be transformed geometry_msgs::msg::WrenchStamped measured_wrench_; geometry_msgs::msg::WrenchStamped measured_wrench_filtered_; geometry_msgs::msg::WrenchStamped measured_wrench_ik_base_frame_; - geometry_msgs::msg::WrenchStamped measured_wrench_endeffector_frame_; geometry_msgs::msg::PoseStamped current_pose_ik_base_frame_; + geometry_msgs::msg::PoseStamped current_pose_control_frame_; // This is the feedforward pose. Where should the end effector be with no wrench applied? geometry_msgs::msg::PoseStamped reference_pose_from_joint_deltas_ik_base_frame_; @@ -170,9 +462,15 @@ class AdmittanceRule geometry_msgs::msg::WrenchStamped reference_force_ik_base_frame_; geometry_msgs::msg::PoseStamped reference_pose_ik_base_frame_; + geometry_msgs::msg::PoseStamped reference_pose_control_frame_; geometry_msgs::msg::PoseStamped admittance_pose_ik_base_frame_; - geometry_msgs::msg::TransformStamped relative_admittance_pose_; + geometry_msgs::msg::PoseStamped sum_of_admittance_displacements_; + geometry_msgs::msg::PoseStamped sum_of_admittance_displacements_control_frame_; + geometry_msgs::msg::TransformStamped relative_admittance_pose_ik_base_frame_; + geometry_msgs::msg::TransformStamped relative_admittance_pose_control_frame_; + geometry_msgs::msg::TransformStamped admittance_velocity_ik_base_frame_; + geometry_msgs::msg::TransformStamped admittance_velocity_control_frame_; // Joint deltas calculation variables std::vector reference_joint_deltas_vec_; @@ -183,14 +481,14 @@ class AdmittanceRule // Pre-reserved update-loop variables std::array measured_wrench_ik_base_frame_arr_; - std::array reference_pose_ik_base_frame_arr_; - std::array current_pose_ik_base_frame_arr_; + std::array reference_pose_arr_; + std::array current_pose_arr_; std::array relative_admittance_pose_arr_; std::array admittance_pose_ik_base_frame_arr_; std::array admittance_velocity_arr_; // Keep a running tally of motion due to admittance, to calculate spring force in open-loop mode - std::array sum_of_admittance_displacements_; + std::array sum_of_admittance_displacements_arr_; std::vector relative_desired_joint_state_vec_; @@ -203,17 +501,30 @@ class AdmittanceRule private: template controller_interface::return_type - transform_message_to_ik_base_frame(const MsgType & message_in, MsgType & message_out) + transform_to_control_frame(const MsgType & message_in, MsgType & message_out) { - if (ik_base_frame_ != message_in.header.frame_id) { + return transform_to_frame(message_in, message_out, parameters_.control_frame_); + } + + template + controller_interface::return_type + transform_to_ik_base_frame(const MsgType & message_in, MsgType & message_out) + { + return transform_to_frame(message_in, message_out, parameters_.ik_base_frame_); + } + + template + controller_interface::return_type + transform_to_frame(const MsgType & message_in, MsgType & message_out, const std::string & frame) + { + if (frame != message_in.header.frame_id) { try { geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - ik_base_frame_, message_in.header.frame_id, tf2::TimePointZero); + frame, message_in.header.frame_id, tf2::TimePointZero); tf2::doTransform(message_in, message_out, transform); } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + - message_in.header.frame_id + "' to '" + ik_base_frame_ + "'."); + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); return controller_interface::return_type::ERROR; } } else { @@ -222,6 +533,73 @@ class AdmittanceRule return controller_interface::return_type::OK; } + template + controller_interface::return_type + transform_relative_to_control_frame(const MsgType & message_in, MsgType & message_out) + { + return transform_relative_to_frame(message_in, message_out, parameters_.control_frame_); + } + + template + controller_interface::return_type + transform_relative_to_ik_base_frame(const MsgType & message_in, MsgType & message_out) + { + return transform_relative_to_frame(message_in, message_out, parameters_.ik_base_frame_); + } + + /** + * Transforms relative movement/pose to a new frame. + * Consider following discussion/approaches in the future the: + * - https://answers.ros.org/question/192273/how-to-implement-velocity-transformation/?answer=192283#post-id-192283 + * - https://physics.stackexchange.com/questions/197009/transform-velocities-from-one-frame-to-an-other-within-a-rigid-body#244364 + */ + template + controller_interface::return_type + transform_relative_to_frame(const MsgType & message_in, MsgType & message_out, const std::string & frame) + { + controller_interface::return_type ret = controller_interface::return_type::OK; + + // Do calculation only if transformation needed + if (frame != message_in.header.frame_id) { + + MsgType message_transformed; + std::array message_transformed_arr; + + geometry_msgs::msg::PoseStamped zero_pose; + geometry_msgs::msg::PoseStamped zero_pose_transformed; + std::array zero_pose_transformed_arr; + + std::array relative_message_transformed_arr; + + ret = transform_to_frame(message_in, message_transformed, frame); + convert_message_to_array(message_transformed, message_transformed_arr); + + zero_pose.header.frame_id = message_in.header.frame_id; + ret = transform_to_frame(zero_pose, zero_pose_transformed, frame); + convert_message_to_array(zero_pose_transformed, zero_pose_transformed_arr); + + for (auto i = 0u; i < 6; ++i) { + relative_message_transformed_arr[i] = + message_transformed_arr[i] - zero_pose_transformed_arr[i]; + + if (i >= 3) { + relative_message_transformed_arr[i] = + angles::normalize_angle(relative_message_transformed_arr[i]); + } + + if (std::fabs(relative_message_transformed_arr[i]) < POSE_ERROR_EPSILON) { + relative_message_transformed_arr[i] = 0.0; + } + } + message_out.header = message_transformed.header; + convert_array_to_message(relative_message_transformed_arr, message_out); + } else { + message_out = message_in; + } + + return ret; + } + template void direct_transform(const Type & input, const tf2::Transform & transform, Type & output) @@ -233,17 +611,6 @@ class AdmittanceRule output_tf = input_tf * transform; tf2::toMsg(output_tf, output); } - - // TODO(destogl): As of C++17 use transform_reduce: - // https://stackoverflow.com/questions/58266717/accumulate-absolute-values-of-a-vector - template - double accumulate_absolute(const Type & container) { - double accumulator = 0.0; - for (auto i = 0ul; i < container.size(); i++) { - accumulator += std::fabs(container[i]); - } - return accumulator; - } }; } // 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 81afeedb90..73e5e98378 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -26,158 +26,39 @@ #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" #include "tf2/utils.h" -#include "tf2_eigen/tf2_eigen.h" - -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; - -template -void convert_message_to_array(const geometry_msgs::msg::Pose & msg, Type & vector_out) -{ - vector_out[0] = msg.position.x; - vector_out[1] = msg.position.y; - vector_out[2] = msg.position.z; - tf2::Quaternion q; - tf2::fromMsg(msg.orientation, q); - q.normalize(); - tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); - for (auto i = 3u; i < 6; ++i) { - vector_out[i] = angles::normalize_angle(vector_out[i]); - } -} - -template -void convert_message_to_array( - const geometry_msgs::msg::PoseStamped & msg, Type & vector_out) -{ - convert_message_to_array(msg.pose, vector_out); -} - -template -void convert_message_to_array(const geometry_msgs::msg::Transform & msg, Type & vector_out) -{ - vector_out[0] = msg.translation.x; - vector_out[1] = msg.translation.y; - vector_out[2] = msg.translation.z; - tf2::Quaternion q; - tf2::fromMsg(msg.rotation, q); - q.normalize(); - tf2::Matrix3x3(q).getRPY(vector_out[3], vector_out[4], vector_out[5]); - for (auto i = 3u; i < 6; ++i) { - vector_out[i] = angles::normalize_angle(vector_out[i]); - } -} - -template -void convert_message_to_array(const geometry_msgs::msg::TransformStamped & msg, Type & vector_out) -{ - convert_message_to_array(msg.transform, vector_out); -} - -template -void convert_message_to_array( - const geometry_msgs::msg::Wrench & msg, Type & vector_out) -{ - vector_out[0] = msg.force.x; - vector_out[1] = msg.force.y; - vector_out[2] = msg.force.z; - vector_out[3] = msg.torque.x; - vector_out[4] = msg.torque.y; - vector_out[5] = msg.torque.z; -} - -template -void convert_message_to_array( - const geometry_msgs::msg::WrenchStamped & msg, Type & vector_out) -{ - convert_message_to_array(msg.wrench, vector_out); -} - -template -void convert_array_to_message(const Type & vector, geometry_msgs::msg::Pose & msg_out) -{ - msg_out.position.x = vector[0]; - msg_out.position.y = vector[1]; - msg_out.position.z = vector[2]; - - tf2::Quaternion q; - q.setRPY(vector[3], vector[4], vector[5]); - - msg_out.orientation.x = q.x(); - msg_out.orientation.y = q.y(); - msg_out.orientation.z = q.z(); - msg_out.orientation.w = q.w(); -} - -template -void convert_array_to_message(const Type & vector, geometry_msgs::msg::PoseStamped & msg_out) -{ - convert_array_to_message(vector, msg_out.pose); -} - -// template -// void convert_array_to_message(const Type & vector, geometry_msgs::msg::Wrench & msg_out) -// { -// msg_out.force.x = vector[0]; -// msg_out.force.y = vector[1]; -// msg_out.force.z = vector[2]; -// msg_out.torque.x = vector[3]; -// msg_out.torque.y = vector[4]; -// msg_out.torque.z = vector[5]; -// } - -// template -// void convert_array_to_message(const Type & vector, geometry_msgs::msg::WrenchStamped & msg_out) -// { -// convert_array_to_message(vector, msg_out.wrench); -// } - -template -void convert_array_to_message(const Type & vector, geometry_msgs::msg::Transform & msg_out) -{ - msg_out.translation.x = vector[0]; - msg_out.translation.y = vector[1]; - msg_out.translation.z = vector[2]; - - tf2::Quaternion q; - q.setRPY(vector[3], vector[4], vector[5]); - - msg_out.rotation.x = q.x(); - msg_out.rotation.y = q.y(); - msg_out.rotation.z = q.z(); - msg_out.rotation.w = q.w(); -} - -template -void convert_array_to_message(const Type & vector, geometry_msgs::msg::TransformStamped & msg_out) -{ - convert_array_to_message(vector, msg_out.transform); -} - -} // utility namespace +#include "tf2_eigen/tf2_eigen.hpp" namespace admittance_controller { controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::SharedPtr node) { - tf_buffer_ = std::make_shared(node->get_clock()); + clock_ = node->get_clock(); + tf_buffer_ = std::make_shared(clock_); tf_listener_ = std::make_shared(*tf_buffer_); // Initialize variables used in the update loop - measured_wrench_.header.frame_id = sensor_frame_; + measured_wrench_.header.frame_id = parameters_.sensor_frame_; + + // The variables represent transformation within the same frame + relative_admittance_pose_ik_base_frame_.header.frame_id = parameters_.ik_base_frame_; + relative_admittance_pose_ik_base_frame_.child_frame_id = parameters_.ik_base_frame_; + relative_admittance_pose_control_frame_.header.frame_id = parameters_.control_frame_; + relative_admittance_pose_control_frame_.child_frame_id = parameters_.control_frame_; + + // The variables represent transformation within the same frame + admittance_velocity_ik_base_frame_.header.frame_id = parameters_.ik_base_frame_; + admittance_velocity_ik_base_frame_.child_frame_id = parameters_.ik_base_frame_; + admittance_velocity_control_frame_.header.frame_id = parameters_.control_frame_; + admittance_velocity_control_frame_.child_frame_id = parameters_.control_frame_; - relative_admittance_pose_.header.frame_id = control_frame_; - relative_admittance_pose_.child_frame_id = control_frame_; + sum_of_admittance_displacements_.header.frame_id = parameters_.ik_base_frame_; reference_joint_deltas_vec_.reserve(6); reference_deltas_vec_ik_base_.reserve(6); - reference_deltas_ik_base_.header.frame_id = ik_base_frame_; - reference_deltas_ik_base_.child_frame_id = ik_base_frame_; + // The variables represent transformation within the same frame + reference_deltas_ik_base_.header.frame_id = parameters_.ik_base_frame_; + reference_deltas_ik_base_.child_frame_id = parameters_.ik_base_frame_; identity_transform_.transform.rotation.w = 1; @@ -189,7 +70,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared admittance_rule_calculated_values_.effort.resize(6, 0.0); // Initialize IK - ik_ = std::make_shared(node, ik_group_name_); + ik_ = std::make_shared(node, parameters_.ik_group_name_); return controller_interface::return_type::OK; } @@ -197,35 +78,21 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared controller_interface::return_type AdmittanceRule::reset() { measured_wrench_ik_base_frame_arr_.fill(0.0); - reference_pose_ik_base_frame_arr_.fill(0.0); - current_pose_ik_base_frame_arr_.fill(0.0); + reference_pose_arr_.fill(0.0); + current_pose_arr_.fill(0.0); admittance_velocity_arr_.fill(0.0); - sum_of_admittance_displacements_.fill(0.0); + sum_of_admittance_displacements_arr_.fill(0.0); get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); reference_pose_from_joint_deltas_ik_base_frame_ = current_pose_ik_base_frame_; // "Open-loop" controller uses old desired pose as current pose: current_pose(K) = desired_pose(K-1) // Therefore desired pose has to be set before calling *update*-method - if (open_loop_control_) { + if (parameters_.open_loop_control_) { get_pose_of_control_frame_in_base_frame(admittance_pose_ik_base_frame_); convert_message_to_array(admittance_pose_ik_base_frame_, admittance_pose_ik_base_frame_arr_); } - // Initialize ik_tip and tool_frame transformations - those are fixed transformations - tf2::Stamped tf2_transform; - try { - auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, control_frame_, tf2::TimePointZero); - tf2::fromMsg(transform, tf2_transform); - ik_tip_to_control_frame_tf_ = tf2_transform; - control_frame_to_ik_tip_tf_ = tf2_transform.inverse(); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + - control_frame_ + "' to '" + ik_tip_frame_ + "'."); - return controller_interface::return_type::ERROR; - } - return controller_interface::return_type::OK; } @@ -239,54 +106,77 @@ controller_interface::return_type AdmittanceRule::update( ) { // Convert inputs to ik_base frame (assumed stationary) - transform_message_to_ik_base_frame(reference_pose, reference_pose_ik_base_frame_); + transform_to_ik_base_frame(reference_pose, reference_pose_ik_base_frame_); + + std::array pose_error; + geometry_msgs::msg::TransformStamped pose_error_pose; + pose_error_pose.header.frame_id = parameters_.ik_base_frame_; + pose_error_pose.child_frame_id = parameters_.control_frame_; - if (!open_loop_control_) { + if (!parameters_.open_loop_control_) { get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); + + // Convert all data to arrays for simpler calculation + transform_to_control_frame(reference_pose_ik_base_frame_, current_pose_control_frame_); + convert_message_to_array(current_pose_control_frame_, reference_pose_arr_); + transform_to_control_frame(current_pose_ik_base_frame_, reference_pose_control_frame_); + convert_message_to_array(reference_pose_control_frame_, current_pose_arr_); + + for (auto i = 0u; i < 6; ++i) { + pose_error[i] = current_pose_arr_[i] - reference_pose_arr_[i]; + + if (i >= 3) { + pose_error[i] = angles::normalize_angle(pose_error[i]); + } + if (std::fabs(pose_error[i]) < POSE_ERROR_EPSILON) { + pose_error[i] = 0.0; + } + } + } else { // In open-loop mode, assume the user's requested pose was exactly achieved // TODO(destogl): This will maybe now work when no feed-forward is used current_pose_ik_base_frame_ = reference_pose_ik_base_frame_; - } - - // Convert all data to arrays for simpler calculation - convert_message_to_array(reference_pose_ik_base_frame_, reference_pose_ik_base_frame_arr_); - convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); - - std::array pose_error; - for (auto i = 0u; i < 6; ++i) { - - if (!open_loop_control_) { - pose_error[i] = current_pose_ik_base_frame_arr_[i] - reference_pose_ik_base_frame_arr_[i]; - } else { - // Sum admittance displacement from the previous relative poses - sum_of_admittance_displacements_[i] += relative_admittance_pose_arr_[i]; - // In open-loop mode, spring force is related to the accumulated "admittance displacement" - pose_error[i] = sum_of_admittance_displacements_[i]; + // Sum admittance displacements (in ik_base_frame) from the previous relative poses + for (auto i = 0u; i < 6; ++i) { + sum_of_admittance_displacements_arr_[i] += relative_admittance_pose_arr_[i]; } - if (i >= 3) { - pose_error[i] = angles::normalize_angle(pose_error[i]); - } - if (std::fabs(pose_error[i]) < POSE_ERROR_EPSILON) { - pose_error[i] = 0.0; - } + // Transform sum of admittance displacements to control frame + convert_array_to_message(sum_of_admittance_displacements_arr_, sum_of_admittance_displacements_); + transform_relative_to_control_frame(sum_of_admittance_displacements_, sum_of_admittance_displacements_control_frame_); + convert_message_to_array(sum_of_admittance_displacements_control_frame_, pose_error); } process_wrench_measurements(measured_wrench); + // Transform internal state to updated control frame - could be changed since the last update + transform_relative_to_control_frame( + admittance_velocity_ik_base_frame_, admittance_velocity_control_frame_); + convert_message_to_array(admittance_velocity_control_frame_, admittance_velocity_arr_); + + // Calculate admittance rule in the control frame calculate_admittance_rule( measured_wrench_ik_base_frame_arr_, pose_error, period, relative_admittance_pose_arr_); - // This works in all cases because not current TF data are used + // Transform internal states from current "control" frame to "ik base" frame // Do clean conversion to the goal pose using transform and not messing with Euler angles - convert_array_to_message(relative_admittance_pose_arr_, relative_admittance_pose_); + convert_array_to_message(relative_admittance_pose_arr_, relative_admittance_pose_control_frame_); + transform_relative_to_ik_base_frame( + relative_admittance_pose_control_frame_, relative_admittance_pose_ik_base_frame_); + convert_message_to_array(relative_admittance_pose_ik_base_frame_, relative_admittance_pose_arr_); + + convert_array_to_message(admittance_velocity_arr_, admittance_velocity_control_frame_); + transform_relative_to_ik_base_frame( + admittance_velocity_control_frame_, admittance_velocity_ik_base_frame_); // Add deltas to previously-desired pose to get the next desired pose - tf2::doTransform(current_pose_ik_base_frame_, admittance_pose_ik_base_frame_, relative_admittance_pose_); + tf2::doTransform(current_pose_ik_base_frame_, admittance_pose_ik_base_frame_, + relative_admittance_pose_ik_base_frame_); - return calculate_desired_joint_state(current_joint_state, period, desired_joint_state); + return calculate_desired_joint_state(current_joint_state, relative_admittance_pose_arr_, + period, desired_joint_state); } // Update from target joint deltas @@ -301,7 +191,7 @@ controller_interface::return_type AdmittanceRule::update( // Get feed-forward cartesian deltas in the ik_base frame. // Since ik_base is MoveIt's working frame, the transform is identity. - identity_transform_.header.frame_id = ik_base_frame_; + identity_transform_.header.frame_id = parameters_.ik_base_frame_; ik_->update_robot_state(current_joint_state); if (!ik_->convert_joint_deltas_to_cartesian_deltas( reference_joint_deltas_vec_, identity_transform_, reference_deltas_vec_ik_base_)) @@ -357,22 +247,11 @@ controller_interface::return_type AdmittanceRule::get_controller_state( state_message.measured_wrench_filtered = measured_wrench_filtered_; state_message.measured_wrench_control_frame = measured_wrench_ik_base_frame_; - // FIXME(destogl): Something is wrong with this transformation - check frames... - try { - auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero); - tf2::doTransform(measured_wrench_ik_base_frame_, measured_wrench_endeffector_frame_, transform); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + - control_frame_ + "' to '" + endeffector_frame_ + "'."); - } - state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_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.relative_desired_pose = relative_admittance_pose_; + state_message.relative_desired_pose = relative_admittance_pose_ik_base_frame_; return controller_interface::return_type::OK; } @@ -380,7 +259,7 @@ controller_interface::return_type AdmittanceRule::get_controller_state( controller_interface::return_type AdmittanceRule::get_pose_of_control_frame_in_base_frame(geometry_msgs::msg::PoseStamped & pose) { try { - auto transform = tf_buffer_->lookupTransform(ik_base_frame_, control_frame_, tf2::TimePointZero); + auto transform = tf_buffer_->lookupTransform(parameters_.ik_base_frame_, parameters_.control_frame_, tf2::TimePointZero); pose.header = transform.header; pose.pose.position.x = transform.transform.translation.x; @@ -388,9 +267,8 @@ controller_interface::return_type AdmittanceRule::get_pose_of_control_frame_in_b pose.pose.position.z = transform.transform.translation.z; pose.pose.orientation= transform.transform.rotation; } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed from '" + - control_frame_ + "' to '" + ik_base_frame_ + "'."); + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what()); return controller_interface::return_type::ERROR; } return controller_interface::return_type::OK; @@ -403,8 +281,9 @@ void AdmittanceRule::process_wrench_measurements( measured_wrench_.wrench = measured_wrench; filter_chain_->update(measured_wrench_, measured_wrench_filtered_); - // TODO(andyz): This is not flexible to work in other control frames besides ik_base - transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_); + // TODO(destogl): rename this variables... + transform_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_); + transform_to_control_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_); convert_message_to_array(measured_wrench_ik_base_frame_, measured_wrench_ik_base_frame_arr_); // TODO(destogl): optimize this checks! @@ -433,11 +312,11 @@ void AdmittanceRule::calculate_admittance_rule( { // Compute admittance control law: F = M*a + D*v + S*(x - x_d) for (auto i = 0u; i < 6; ++i) { - if (selected_axes_[i]) { + if (parameters_.selected_axes_[i]) { // TODO(destogl): check if velocity is measured from hardware - const double admittance_acceleration = (1 / mass_[i]) * (measured_wrench[i] - - damping_[i] * admittance_velocity_arr_[i] - - stiffness_[i] * pose_error[i]); + const double admittance_acceleration = (1 / parameters_.mass_[i]) * (measured_wrench[i] - + parameters_.damping_[i] * admittance_velocity_arr_[i] - + parameters_.stiffness_[i] * pose_error[i]); admittance_velocity_arr_[i] += admittance_acceleration * period.seconds(); @@ -458,17 +337,16 @@ void AdmittanceRule::calculate_admittance_rule( controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const std::array & relative_pose, const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - convert_array_to_message(relative_admittance_pose_arr_, relative_admittance_pose_); - // Since ik_base is MoveIt's working frame, the transform is identity. - identity_transform_.header.frame_id = ik_base_frame_; + identity_transform_.header.frame_id = parameters_.ik_base_frame_; // Use Jacobian-based IK - std::vector relative_admittance_pose_vec(relative_admittance_pose_arr_.begin(), relative_admittance_pose_arr_.end()); + std::vector relative_admittance_pose_vec(relative_pose.begin(), relative_pose.end()); ik_->update_robot_state(current_joint_state); if (ik_->convert_cartesian_deltas_to_joint_deltas( relative_admittance_pose_vec, identity_transform_, relative_desired_joint_state_vec_)){ diff --git a/admittance_controller/include/admittance_controller/moveit_kinematics.hpp b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp index 86e37e5758..d31460ceb4 100644 --- a/admittance_controller/include/admittance_controller/moveit_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/moveit_kinematics.hpp @@ -69,17 +69,24 @@ class MoveItKinematics bool update_robot_state(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) { - if (current_joint_state.positions.size() != kinematic_state_->getVariableNames().size()) + if (current_joint_state.positions.size() != joint_model_group_->getVariableNames().size()) { RCLCPP_ERROR(node_->get_logger(), "Vector size mismatch in update_robot_state()"); return false; } - kinematic_state_->setVariablePositions(current_joint_state.positions); + kinematic_state_->setJointGroupPositions(joint_model_group_, current_joint_state.positions); return true; } private: + /** \brief Possibly calculate a velocity scaling factor, due to proximity of + * singularity and direction of motion + */ + double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse); + // MoveIt setup const moveit::core::JointModelGroup* joint_model_group_; moveit::core::RobotStatePtr kinematic_state_; @@ -87,6 +94,7 @@ class MoveItKinematics // Pre-allocate for speed Eigen::MatrixXd jacobian_; + Eigen::MatrixXd matrix_s_; Eigen::MatrixXd pseudo_inverse_; }; diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index e65c2fc7c9..06f8bd8fdc 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -3,7 +3,7 @@ admittance_controller 0.0.0 - Implemenation of admittance controllers for different input and output interface. + Implementation of admittance controllers for different input and output interface. Denis Štogl Andy Zelenak Apache License 2.0 @@ -15,6 +15,7 @@ filters geometry_msgs hardware_interface + joint_limits moveit_ros_move_group moveit_ros_planning_interface pluginlib diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 3a52c4eebd..b772dd14a5 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,21 +16,28 @@ #include "admittance_controller/admittance_controller.hpp" +#include #include #include #include -#include +#include #include #include +#include + #include "admittance_controller/admittance_rule_impl.hpp" #include "angles/angles.h" #include "controller_interface/helpers.hpp" #include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits_rosparam.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 { AdmittanceController::AdmittanceController() @@ -38,71 +45,31 @@ AdmittanceController::AdmittanceController() { } -controller_interface::return_type AdmittanceController::init(const std::string & controller_name) +CallbackReturn AdmittanceController::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { - return ret; - } + + admittance_ = std::make_unique(); try { // TODO: use variables as parameters - get_node()->declare_parameter>("joints", {}); - get_node()->declare_parameter>("command_interfaces", {}); - get_node()->declare_parameter>("state_interfaces", {}); + get_node()->declare_parameter>("joints", std::vector({})); + get_node()->declare_parameter>("command_interfaces", std::vector({})); + get_node()->declare_parameter>("state_interfaces", std::vector({})); get_node()->declare_parameter("ft_sensor_name", ""); get_node()->declare_parameter("use_joint_commands_as_input", false); - get_node()->declare_parameter("open_loop_control", false); + get_node()->declare_parameter("joint_limiter_type", "joint_limits/SimpleJointLimiter"); - get_node()->declare_parameter("IK.base", ""); - get_node()->declare_parameter("IK.tip", ""); // TODO(destogl): enable when IK-plugin support is added -// get_node()->declare_parameter("IK.plugin", ""); - get_node()->declare_parameter("IK.group_name", ""); - - get_node()->declare_parameter("control_frame", ""); - get_node()->declare_parameter("endeffector_frame", ""); - get_node()->declare_parameter("fixed_world_frame", ""); - get_node()->declare_parameter("sensor_frame", ""); - - // TODO(destogl): enable when force/position control is implemented -// get_node()->declare_parameter("admitance.unified_mode", false); - get_node()->declare_parameter("admittance.selected_axes.x", false); - get_node()->declare_parameter("admittance.selected_axes.y", false); - get_node()->declare_parameter("admittance.selected_axes.z", false); - get_node()->declare_parameter("admittance.selected_axes.rx", false); - get_node()->declare_parameter("admittance.selected_axes.ry", false); - get_node()->declare_parameter("admittance.selected_axes.rz", false); - - get_node()->declare_parameter("admittance.mass.x", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.mass.y", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.mass.z", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.mass.rx", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.mass.ry", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.mass.rz", std::numeric_limits::quiet_NaN()); - - get_node()->declare_parameter("admittance.damping.x", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.damping.y", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.damping.z", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.damping.rx", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.damping.ry", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.damping.rz", std::numeric_limits::quiet_NaN()); - - get_node()->declare_parameter("admittance.stiffness.x", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.stiffness.y", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.stiffness.z", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.stiffness.rx", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.stiffness.ry", std::numeric_limits::quiet_NaN()); - get_node()->declare_parameter("admittance.stiffness.rz", std::numeric_limits::quiet_NaN()); + // get_node()->declare_parameter("IK.plugin", ""); + + admittance_->parameters_.declare_parameters(get_node()); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - admittance_ = std::make_unique(); - - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn AdmittanceController::on_configure( @@ -135,61 +102,18 @@ CallbackReturn AdmittanceController::on_configure( return false; // TODO(destogl): how to check "if_empty" for bool? }; - auto get_double_param_and_error_if_empty = [&]( - double & parameter, const char * parameter_name) { - parameter = get_node()->get_parameter(parameter_name).get_value(); - if (std::isnan(parameter)) { - RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was not set", parameter_name); - return true; - } - return false; - }; - if ( get_string_array_param_and_error_if_empty(joint_names_, "joints") || get_string_array_param_and_error_if_empty(command_interface_types_, "command_interfaces") || get_string_array_param_and_error_if_empty(state_interface_types_, "state_interfaces") || get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") || get_bool_param_and_error_if_empty(use_joint_commands_as_input_, "use_joint_commands_as_input") || - get_bool_param_and_error_if_empty(admittance_->open_loop_control_, "open_loop_control") || - get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") || - get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") || - get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") || - get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") || - get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") || - get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") || - get_string_param_and_error_if_empty(admittance_->sensor_frame_, "sensor_frame") || - // TODO(destogl): add unified mode considering target force -// get_bool_param_and_error_if_empty(unified_mode_, "admittance.unified_mode") || - get_bool_param_and_error_if_empty(admittance_->selected_axes_[0], "admittance.selected_axes.x") || - get_bool_param_and_error_if_empty(admittance_->selected_axes_[1], "admittance.selected_axes.y") || - get_bool_param_and_error_if_empty(admittance_->selected_axes_[2], "admittance.selected_axes.z") || - get_bool_param_and_error_if_empty(admittance_->selected_axes_[3], "admittance.selected_axes.rx") || - get_bool_param_and_error_if_empty(admittance_->selected_axes_[4], "admittance.selected_axes.ry") || - get_bool_param_and_error_if_empty(admittance_->selected_axes_[5], "admittance.selected_axes.rz") || - - get_double_param_and_error_if_empty(admittance_->mass_[0], "admittance.mass.x") || - get_double_param_and_error_if_empty(admittance_->mass_[1], "admittance.mass.y") || - get_double_param_and_error_if_empty(admittance_->mass_[2], "admittance.mass.z") || - get_double_param_and_error_if_empty(admittance_->mass_[3], "admittance.mass.rx") || - get_double_param_and_error_if_empty(admittance_->mass_[4], "admittance.mass.ry") || - get_double_param_and_error_if_empty(admittance_->mass_[5], "admittance.mass.rz") || - - get_double_param_and_error_if_empty(admittance_->damping_[0], "admittance.damping.x") || - get_double_param_and_error_if_empty(admittance_->damping_[1], "admittance.damping.y") || - get_double_param_and_error_if_empty(admittance_->damping_[2], "admittance.damping.z") || - get_double_param_and_error_if_empty(admittance_->damping_[3], "admittance.damping.rx") || - get_double_param_and_error_if_empty(admittance_->damping_[4], "admittance.damping.ry") || - get_double_param_and_error_if_empty(admittance_->damping_[5], "admittance.damping.rz") || - - get_double_param_and_error_if_empty(admittance_->stiffness_[0], "admittance.stiffness.x") || - get_double_param_and_error_if_empty(admittance_->stiffness_[1], "admittance.stiffness.y") || - get_double_param_and_error_if_empty(admittance_->stiffness_[2], "admittance.stiffness.z") || - get_double_param_and_error_if_empty(admittance_->stiffness_[3], "admittance.stiffness.rx") || - get_double_param_and_error_if_empty(admittance_->stiffness_[4], "admittance.stiffness.ry") || - get_double_param_and_error_if_empty(admittance_->stiffness_[5], "admittance.stiffness.rz") + get_string_param_and_error_if_empty(joint_limiter_type_, "joint_limiter_type") || + + !admittance_->parameters_.get_parameters(get_node()) ) { + RCLCPP_ERROR(get_node()->get_logger(), "Error happend during reading parameters"); return CallbackReturn::ERROR; } @@ -220,7 +144,7 @@ CallbackReturn AdmittanceController::on_configure( 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 '" + interface + "' not allowed!"); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Command interface type '" << interface << "' not allowed!"); return CallbackReturn::ERROR; } } @@ -241,14 +165,14 @@ CallbackReturn AdmittanceController::on_configure( 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 '" + interface + "' not allowed!"); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "State interface type '" << interface << "' not allowed!"); return CallbackReturn::ERROR; } } if (!controller_interface::interface_list_contains_interface_type( state_interface_types_, hardware_interface::HW_IF_POSITION)) { - RCLCPP_ERROR(get_node()->get_logger(), "State interface type '" + std::string(hardware_interface::HW_IF_POSITION) + "' has to be always present allowed!"); + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "State interface type '" << std::string(hardware_interface::HW_IF_POSITION) << "' has to be always present allowed!"); return CallbackReturn::ERROR; } @@ -274,6 +198,25 @@ CallbackReturn AdmittanceController::on_configure( get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); + auto num_joints = joint_names_.size(); + + // Initialize joint limits + if (!joint_limiter_type_.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), "Using joint limiter plugin: '%s'", joint_limiter_type_.c_str()); + joint_limiter_loader_ = std::make_shared>( + "joint_limits", "joint_limits::JointLimiterInterface"); + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_->createUnmanagedInstance(joint_limiter_type_)); + joint_limiter_->init(joint_names_, get_node()); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), "Not using joint limiter plugin as none defined."); + } + // Initialize FTS semantic semantic_component force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor(ft_sensor_name_)); @@ -312,8 +255,6 @@ CallbackReturn AdmittanceController::on_configure( "~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(s_publisher_); - auto num_joints = joint_names_.size(); - // Initialize state message state_publisher_->lock(); state_publisher_->msg_.joint_names = joint_names_; @@ -322,35 +263,36 @@ CallbackReturn AdmittanceController::on_configure( state_publisher_->msg_.error_joint_state.positions.resize(num_joints, 0.0); state_publisher_->unlock(); - // Configure AdmittanceRule - admittance_->configure(get_node()); - - // wait for TF to become available. Important to do there because it is blocking, i.e. non real-time safe code - std::shared_ptr msg_pose = std::make_shared(); - // TODO(destogl): This will break tests because there is no TF inside them - auto iterations = 0u; - const auto max_iterations = 20u; - while (admittance_->get_pose_of_control_frame_in_base_frame(*msg_pose) != controller_interface::return_type::OK) - { - RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *(get_node()->get_clock()), 5000, "Waiting for base to control frame transform to become available."); - rclcpp::sleep_for(std::chrono::seconds(1)); - if (++iterations > max_iterations) { - RCLCPP_ERROR(get_node()->get_logger(), "After waiting for TF for %zu seconds, still no transformation available. Admittance Controller can not be configured.", max_iterations); - return CallbackReturn::ERROR; - } - } - - // TODO(destogl): Use reserve instead of resize? last_commanded_state_.positions.resize(num_joints); last_commanded_state_.velocities.resize(num_joints, 0.0); last_commanded_state_.accelerations.resize(num_joints, 0.0); if (use_joint_commands_as_input_) { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Using Joint input mode."); + RCLCPP_INFO(get_node()->get_logger(), "Using Joint input mode."); + } else { + RCLCPP_INFO(get_node()->get_logger(), "Using Cartesian input mode."); + } + + // Configure AdmittanceRule + admittance_->configure(get_node()); + + // Add callback to dynamically update parameters + on_set_callback_handle_ = get_node()->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + + return admittance_->parameters_.set_parameter_callback(parameters); + }); + + // HACK: This is workaround because it seems that updating parameters only in `on_activate` does + // not work properly + if (!admittance_->parameters_.check_if_parameters_are_valid()) { + RCLCPP_WARN(get_node()->get_logger(), + "Parameters are not valid and therefore will not be udpated"); } else { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Using Cartesian input mode."); + admittance_->parameters_.update(); } - RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -393,6 +335,14 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & { const auto num_joints = joint_names_.size(); + // Update dynamic parameters before controller is started + if (!admittance_->parameters_.check_if_parameters_are_valid()) { + RCLCPP_WARN(get_node()->get_logger(), + "Parameters are not valid and therefore will not be udpated"); + } else { + admittance_->parameters_.update(); + } + // order all joints in the storage for (const auto & interface : command_interface_types_) { auto it = std::find( @@ -402,7 +352,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & command_interfaces_, joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %u '%s' command interfaces, got %u.", + node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints, interface.c_str(), joint_command_interface_[index].size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -415,7 +365,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & state_interfaces_, joint_names_, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - node_->get_logger(), "Expected %u '%s' state interfaces, got %u.", + node_->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints, interface.c_str(), joint_state_interface_[index].size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -429,12 +379,16 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & previous_time_ = get_node()->now(); read_state_from_hardware(last_commanded_state_); + if (joint_limiter_) + { + joint_limiter_->configure(last_commanded_state_); + } // Handle restart of controller by reading last_commanded_state_ from commands if not nan read_state_from_command_interfaces(last_commanded_state_); // Set initial command values - initialize all to simplify update std::shared_ptr msg_wrench = std::make_shared(); - msg_wrench->header.frame_id = admittance_->control_frame_; + msg_wrench->header.frame_id = admittance_->parameters_.control_frame_; input_wrench_command_.writeFromNonRT(msg_wrench); std::shared_ptr msg_joint = std::make_shared(); @@ -453,8 +407,15 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & input_joint_command_.writeFromNonRT(msg_joint); std::shared_ptr msg_pose = std::make_shared(); - msg_pose->header.frame_id = admittance_->control_frame_; - admittance_->get_pose_of_control_frame_in_base_frame(*msg_pose); + msg_pose->header.frame_id = admittance_->parameters_.control_frame_; + if (admittance_->get_pose_of_control_frame_in_base_frame(*msg_pose) != + controller_interface::return_type::OK) + { + RCLCPP_ERROR(node_->get_logger(), + "Can not find transform from '%s' to '%s' needed in the update loop", + admittance_->parameters_.ik_base_frame_.c_str(), admittance_->parameters_.control_frame_.c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } input_pose_command_.writeFromNonRT(msg_pose); return CallbackReturn::SUCCESS; @@ -497,13 +458,14 @@ controller_interface::return_type AdmittanceController::update() read_state_from_hardware(current_joint_states); - if (admittance_->open_loop_control_) { + if (admittance_->parameters_.open_loop_control_) { // TODO(destogl): This may not work in every case. // Please add checking which states are available and which not! current_joint_states = last_commanded_state_; } - auto ft_values = force_torque_sensor_->get_values_as_message(); + geometry_msgs::msg::Wrench ft_values; + force_torque_sensor_->get_values_as_message(ft_values); auto duration_since_last_call = get_node()->now() - previous_time_; @@ -536,6 +498,11 @@ controller_interface::return_type AdmittanceController::update() // } previous_time_ = get_node()->now(); + if (joint_limiter_) + { + joint_limiter_->enforce(current_joint_states, desired_joint_states, duration_since_last_call); + } + // Write new joint angles to the robot for (auto index = 0u; index < num_joints; ++index) { if (has_position_command_interface_) { @@ -669,8 +636,6 @@ bool AdmittanceController::read_state_from_command_interfaces( } // namespace admittance_controller #include "pluginlib/class_list_macros.hpp" -#include -#include PLUGINLIB_EXPORT_CLASS( admittance_controller::AdmittanceController, diff --git a/admittance_controller/src/moveit_kinematics.cpp b/admittance_controller/src/moveit_kinematics.cpp index 2b72ff75b6..6b3b055773 100644 --- a/admittance_controller/src/moveit_kinematics.cpp +++ b/admittance_controller/src/moveit_kinematics.cpp @@ -16,7 +16,13 @@ #include "admittance_controller/moveit_kinematics.hpp" -#include "tf2_eigen/tf2_eigen.h" +#include "tf2_eigen/tf2_eigen.hpp" + +constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); +// TODO: Parameterize singularity thresholds +constexpr double LOWER_SINGULARITY_THRESHOLD = 20.; +constexpr double APPROACHING_STOP_SINGULARITY_THRESHOLD = 60.; +constexpr double HARD_STOP_SINGULARITY_THRESHOLD = 120.; namespace admittance_controller { @@ -74,10 +80,13 @@ bool MoveItKinematics::convert_cartesian_deltas_to_joint_deltas( // Multiply with the pseudoinverse to get delta_theta jacobian_ = kinematic_state_->getJacobian(joint_model_group_); - // TODO(andyz): the SVD method would be more stable near singularities - // (or do what Olivier suggested: https://github.com/ros-controls/ros2_controllers/pull/173#discussion_r627936628) - pseudo_inverse_ = jacobian_.transpose() * (jacobian_ * jacobian_.transpose()).inverse(); + // TODO(andyz): consider what Olivier suggested: https://github.com/ros-controls/ros2_controllers/pull/173#discussion_r627936628 + Eigen::JacobiSVD svd = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); + matrix_s_ = svd.singularValues().asDiagonal(); + pseudo_inverse_ = svd.matrixV() * matrix_s_.inverse() * svd.matrixU().transpose(); + Eigen::VectorXd delta_theta = pseudo_inverse_ * delta_x; + delta_theta *= velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse_); std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); delta_theta_vec = delta_theta_v; @@ -143,4 +152,71 @@ Eigen::Isometry3d MoveItKinematics::get_link_transform( return kinematic_state_->getGlobalLinkTransform(link_name); } +// Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion +double MoveItKinematics::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse) +{ + double velocity_scale = 1; + std::size_t num_dimensions = commanded_velocity.size(); + + // Find the direction away from nearest singularity. + // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. + // The sign can flip at any time, so we have to do some extra checking. + // Look ahead to see if the Jacobian's condition will decrease. + Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); + + double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + + // TODO: Remove or switch to DEBUG + RCLCPP_INFO_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 50000., "Singularity condition number is " << ini_condition); + + // This singular vector tends to flip direction unpredictably. See R. Bro, + // "Resolving the Sign Ambiguity in the Singular Value Decomposition". + // Look ahead to see if the Jacobian's condition will decrease in this + // direction. Start with a scaled version of the singular vector + Eigen::VectorXd delta_x(num_dimensions); + double scale = 100; + delta_x = vector_toward_singularity / scale; + + // Calculate a small change in joints + Eigen::VectorXd new_theta; + kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); + new_theta += pseudo_inverse * delta_x; + kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); + Eigen::MatrixXd new_jacobian = kinematic_state_->getJacobian(joint_model_group_); + + Eigen::JacobiSVD new_svd(new_jacobian); + double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); + // If new_condition < ini_condition, the singular vector does point towards a + // singularity. Otherwise, flip its direction. + if (ini_condition >= new_condition) + { + vector_toward_singularity *= -1; + } + + // If this dot product is positive, we're moving toward singularity ==> decelerate + double dot = vector_toward_singularity.dot(commanded_velocity); + double upper_threshold = dot > 0 ? APPROACHING_STOP_SINGULARITY_THRESHOLD : HARD_STOP_SINGULARITY_THRESHOLD; + // Ramp velocity down linearly when the Jacobian condition is between lower_singularity_threshold and + // hard_stop_singularity_threshold, and we're moving towards the singularity + if ((ini_condition > LOWER_SINGULARITY_THRESHOLD) && + (ini_condition < upper_threshold)) + { + velocity_scale = + 1. - (ini_condition - LOWER_SINGULARITY_THRESHOLD) / + (upper_threshold - LOWER_SINGULARITY_THRESHOLD); + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, "Close to a singularity, decelerating"); + } + + // Very close to singularity, so halt. + else if (ini_condition > upper_threshold) + { + velocity_scale = 0; + RCLCPP_WARN_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, "Very close to a singularity, emergency stop"); + } + + return velocity_scale; +} + } // namespace admittance_controller diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 15e3e0b2d4..c5ba7a549a 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -31,7 +31,7 @@ TEST_P(AdmittanceControllerTestParameterizedParameters, one_parameter_is_missing } // TODO(anyone): the new gtest version afer 1.8.0 uses INSTANTIATE_TEST_SUITE_P -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringConfiguration, AdmittanceControllerTestParameterizedParameters, ::testing::Values( @@ -63,10 +63,6 @@ INSTANTIATE_TEST_CASE_P( std::string("IK.base"), rclcpp::ParameterValue("") ), - std::make_tuple( - std::string("IK.tip"), - rclcpp::ParameterValue("") - ), std::make_tuple( std::string("IK.group_name"), rclcpp::ParameterValue("") @@ -75,10 +71,6 @@ INSTANTIATE_TEST_CASE_P( std::string("control_frame"), rclcpp::ParameterValue("") ), - std::make_tuple( - std::string("fixed_world_frame"), - rclcpp::ParameterValue("") - ), std::make_tuple( std::string("sensor_frame"), rclcpp::ParameterValue("") @@ -210,34 +202,32 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) state_interface_types_.begin(), state_interface_types_.end())); ASSERT_EQ(controller_->ft_sensor_name_, ft_sensor_name_); - ASSERT_EQ(controller_->admittance_->ik_base_frame_, ik_base_frame_); - ASSERT_EQ(controller_->admittance_->ik_tip_frame_, ik_tip_frame_); - // ASSERT_EQ(controller_->admittance_->ik_group_name_, ik_group_name_); - ASSERT_EQ(controller_->admittance_->fixed_world_frame_, fixed_world_frame_); - ASSERT_EQ(controller_->admittance_->sensor_frame_, sensor_frame_); - - ASSERT_TRUE(!controller_->admittance_->selected_axes_.empty()); - ASSERT_TRUE(controller_->admittance_->selected_axes_.size() == admittance_selected_axes_.size()); + 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_->selected_axes_.begin(), controller_->admittance_->selected_axes_.end(), + controller_->admittance_->parameters_.selected_axes_.begin(), controller_->admittance_->parameters_.selected_axes_.end(), admittance_selected_axes_.begin(), admittance_selected_axes_.end())); - ASSERT_TRUE(!controller_->admittance_->mass_.empty()); - ASSERT_TRUE(controller_->admittance_->mass_.size() == admittance_mass_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.mass_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.mass_.size() == admittance_mass_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->mass_.begin(), controller_->admittance_->mass_.end(), + controller_->admittance_->parameters_.mass_.begin(), controller_->admittance_->parameters_.mass_.end(), admittance_mass_.begin(), admittance_mass_.end())); - ASSERT_TRUE(!controller_->admittance_->damping_.empty()); - ASSERT_TRUE(controller_->admittance_->damping_.size() == admittance_damping_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.damping_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.damping_.size() == admittance_damping_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->damping_.begin(), controller_->admittance_->damping_.end(), + controller_->admittance_->parameters_.damping_.begin(), controller_->admittance_->parameters_.damping_.end(), admittance_damping_.begin(), admittance_damping_.end())); - ASSERT_TRUE(!controller_->admittance_->stiffness_.empty()); - ASSERT_TRUE(controller_->admittance_->stiffness_.size() == admittance_stiffness_.size()); + ASSERT_TRUE(!controller_->admittance_->parameters_.stiffness_.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.stiffness_.size() == admittance_stiffness_.size()); ASSERT_TRUE(std::equal( - controller_->admittance_->stiffness_.begin(), controller_->admittance_->stiffness_.end(), + controller_->admittance_->parameters_.stiffness_.begin(), controller_->admittance_->parameters_.stiffness_.end(), admittance_stiffness_.begin(), admittance_stiffness_.end())); } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index dd84624e3d..918714dd56 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -62,7 +62,7 @@ rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription } // namespace -// subclassing and friending so we can access member varibles +// subclassing and friending so we can access member variables class TestableAdmittanceController : public admittance_controller::AdmittanceController { From e1eebd92a4c77a90ec77126d0ed5f92046f57cd0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 21 Oct 2021 16:19:37 +0200 Subject: [PATCH 96/99] Compilable with changed controllers' update() api -> against destogl:admittance-controller-development branch. --- .../admittance_controller.hpp | 3 ++- .../src/admittance_controller.cpp | 19 ++++++++----------- .../test/test_admittance_controller.cpp | 12 +++++++----- .../test/test_admittance_controller.hpp | 2 +- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 1be4829e64..64e659f63f 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -69,7 +69,8 @@ class AdmittanceController : public controller_interface::ControllerInterface CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ADMITTANCE_CONTROLLER_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::vector joint_names_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b772dd14a5..8b2b20cabf 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -113,7 +113,7 @@ CallbackReturn AdmittanceController::on_configure( !admittance_->parameters_.get_parameters(get_node()) ) { - RCLCPP_ERROR(get_node()->get_logger(), "Error happend during reading parameters"); + RCLCPP_ERROR(get_node()->get_logger(), "Error happened during reading parameters"); return CallbackReturn::ERROR; } @@ -376,7 +376,6 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & // Initialize Admittance Rule from current states admittance_->reset(); - previous_time_ = get_node()->now(); read_state_from_hardware(last_commanded_state_); if (joint_limiter_) @@ -441,7 +440,8 @@ CallbackReturn AdmittanceController::on_deactivate( return CallbackReturn::SUCCESS; } -controller_interface::return_type AdmittanceController::update() +controller_interface::return_type AdmittanceController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // get input commands auto input_wrench_cmd = input_wrench_command_.readFromRT(); @@ -467,11 +467,9 @@ controller_interface::return_type AdmittanceController::update() geometry_msgs::msg::Wrench ft_values; force_torque_sensor_->get_values_as_message(ft_values); - auto duration_since_last_call = get_node()->now() - previous_time_; - // TODO(destogl): Enable this when unified mode is used // if (admittance_->unified_mode_) { - // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_wrench_cmd, duration_since_last_call, desired_joint_states); + // admittance_->update(current_joint_states, ft_values, **input_pose_cmd, **input_wrench_cmd, period, desired_joint_states); // } else { // TODO(destogl): refactor this into different admittance controllers: 1. Pose input, Joint State input and Unified mode (is there need for switching between unified and non-unified mode?) @@ -481,7 +479,7 @@ controller_interface::return_type AdmittanceController::update() // TODO(destogl): add error handling if ((*input_joint_cmd)->points[0].positions.empty()) { for (auto index = 0u; index < num_joints; ++index) { - joint_deltas[index] = (*input_joint_cmd)->points[0].velocities[index] * duration_since_last_call.seconds(); + joint_deltas[index] = (*input_joint_cmd)->points[0].velocities[index] * period.seconds(); } } else { for (auto index = 0u; index < num_joints; ++index) { @@ -491,16 +489,15 @@ controller_interface::return_type AdmittanceController::update() } } - admittance_->update(current_joint_states, ft_values, joint_deltas, duration_since_last_call, desired_joint_states); + admittance_->update(current_joint_states, ft_values, joint_deltas, period, desired_joint_states); } else { - admittance_->update(current_joint_states, ft_values, **input_pose_cmd, duration_since_last_call, desired_joint_states); + admittance_->update(current_joint_states, ft_values, **input_pose_cmd, period, desired_joint_states); } // } - previous_time_ = get_node()->now(); if (joint_limiter_) { - joint_limiter_->enforce(current_joint_states, desired_joint_states, duration_since_last_call); + joint_limiter_->enforce(current_joint_states, desired_joint_states, period); } // Write new joint angles to the robot diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index c5ba7a549a..36999d764e 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -286,7 +286,8 @@ 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(), 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) @@ -308,7 +309,8 @@ TEST_F(AdmittanceControllerTest, reactivate_success) assign_interfaces(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); broadcast_tfs(); - ASSERT_EQ(controller_->update(), 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) @@ -320,7 +322,7 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); broadcast_tfs(); - ASSERT_EQ(controller_->update(), 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); @@ -418,7 +420,7 @@ 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(), 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 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())); @@ -432,7 +434,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) ASSERT_TRUE(controller_->wait_for_commands(executor)); broadcast_tfs(); - ASSERT_EQ(controller_->update(), 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], 0.0, COMMON_THRESHOLD); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 918714dd56..6476956d91 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -301,7 +301,7 @@ class AdmittanceControllerTest : public ::testing::Test "/test_admittance_controller/state", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(controller_->update(), 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); From 712081734450e180e09f11242919deaac4c50a28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Oct 2021 20:18:06 +0200 Subject: [PATCH 97/99] Add debuging output to the AdmittanceController to know when SRDF is not read properly. --- admittance_controller/src/admittance_controller.cpp | 7 ++++++- admittance_controller/src/moveit_kinematics.cpp | 10 ++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 8b2b20cabf..fff94e1558 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -371,6 +371,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & } } + // Initialize interface of the FTS semantic semantic component force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); @@ -405,6 +406,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & input_joint_command_.writeFromNonRT(msg_joint); + // TODO(destogl): Move this to the Cartesian admittance controller std::shared_ptr msg_pose = std::make_shared(); msg_pose->header.frame_id = admittance_->parameters_.control_frame_; if (admittance_->get_pose_of_control_frame_in_base_frame(*msg_pose) != @@ -451,10 +453,13 @@ controller_interface::return_type AdmittanceController::update( // Position has to always be there auto num_joints = joint_state_interface_[0].size(); trajectory_msgs::msg::JointTrajectoryPoint current_joint_states; - current_joint_states.positions.resize(num_joints, 0.0); + current_joint_states.positions.resize(num_joints); + current_joint_states.velocities.resize(num_joints, 0.0); + current_joint_states.accelerations.resize(num_joints, 0.0); trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states; desired_joint_states.positions.resize(num_joints); desired_joint_states.velocities.resize(num_joints); + desired_joint_states.accelerations.resize(num_joints); read_state_from_hardware(current_joint_states); diff --git a/admittance_controller/src/moveit_kinematics.cpp b/admittance_controller/src/moveit_kinematics.cpp index 6b3b055773..3b6551aabf 100644 --- a/admittance_controller/src/moveit_kinematics.cpp +++ b/admittance_controller/src/moveit_kinematics.cpp @@ -37,6 +37,16 @@ MoveItKinematics::MoveItKinematics(const std::shared_ptr & node, c kinematic_state_ = std::make_shared(kinematic_model); // By default, the MoveIt Jacobian frame is the last link + + // TODO(destogl): manage also is this parameter is set! + auto robot_description_semantic = + node_->get_parameter("robot_description_semantic").as_string(); + if (robot_description_semantic.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Vector size mismatch in update_robot_state()"); + // TODO(destogl): This should be cached in the AdmittanceController... + throw std::runtime_error(std::string("No 'robot_description_semantic' parameter found!")); + } } bool MoveItKinematics::convert_cartesian_deltas_to_joint_deltas( From d29d6c2f6337f68e544466b236357e9e10da9c09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 28 Oct 2021 19:34:54 +0200 Subject: [PATCH 98/99] Update to new version of ControlParameters and add enable_paramter_update_without_reactivation parameter. --- .../admittance_controller/admittance_rule.hpp | 14 +++++++++++--- .../src/admittance_controller.cpp | 19 +++++++------------ 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a0c48a1bad..297afd4874 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -172,7 +172,7 @@ namespace admittance_controller class AdmittanceParameters : public controller_interface::ControllerParameters { public: - AdmittanceParameters() : controller_interface::ControllerParameters(7, 24, 4) + AdmittanceParameters() : controller_interface::ControllerParameters("", 7, 24, 4) { add_string_parameter("IK.base", false); add_string_parameter("IK.group_name", false); @@ -180,6 +180,7 @@ class AdmittanceParameters : public controller_interface::ControllerParameters add_string_parameter("sensor_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); @@ -285,7 +286,7 @@ class AdmittanceParameters : public controller_interface::ControllerParameters } } - void update() override + void update_storage() override { ik_base_frame_ = string_parameters_[0].second; RCUTILS_LOG_INFO_NAMED( @@ -308,10 +309,16 @@ class AdmittanceParameters : public controller_interface::ControllerParameters 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 = 1; // 2 because there is already one parameter for (auto i = 0ul; i < 6; ++i) { - selected_axes_[i] = bool_parameters_[i+1].second; // +1 because there is already one parameter + 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 ")); @@ -347,6 +354,7 @@ class AdmittanceParameters : public controller_interface::ControllerParameters std::string control_frame_; bool open_loop_control_; + bool enable_parameter_update_without_reactivation_; std::array damping_; std::array damping_ratio_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index fff94e1558..19aae8ac7b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -285,12 +285,7 @@ CallbackReturn AdmittanceController::on_configure( // HACK: This is workaround because it seems that updating parameters only in `on_activate` does // not work properly - if (!admittance_->parameters_.check_if_parameters_are_valid()) { - RCLCPP_WARN(get_node()->get_logger(), - "Parameters are not valid and therefore will not be udpated"); - } else { - admittance_->parameters_.update(); - } + admittance_->parameters_.update(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -336,12 +331,7 @@ CallbackReturn AdmittanceController::on_activate(const rclcpp_lifecycle::State & const auto num_joints = joint_names_.size(); // Update dynamic parameters before controller is started - if (!admittance_->parameters_.check_if_parameters_are_valid()) { - RCLCPP_WARN(get_node()->get_logger(), - "Parameters are not valid and therefore will not be udpated"); - } else { - admittance_->parameters_.update(); - } + admittance_->parameters_.update(); // order all joints in the storage for (const auto & interface : command_interface_types_) { @@ -445,6 +435,11 @@ CallbackReturn AdmittanceController::on_deactivate( controller_interface::return_type AdmittanceController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + if (admittance_->parameters_.enable_parameter_update_without_reactivation_) + { + admittance_->parameters_.update(); + } + // get input commands auto input_wrench_cmd = input_wrench_command_.readFromRT(); auto input_joint_cmd = input_joint_command_.readFromRT(); From ed51262c15d37ec00377b061b8f1b69ff7dc7586 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 31 Oct 2021 19:35:20 +0100 Subject: [PATCH 99/99] Use updated ParameterHandler from control_toolbox. --- admittance_controller/CMakeLists.txt | 3 +++ .../include/admittance_controller/admittance_rule.hpp | 6 +++--- admittance_controller/package.xml | 1 + admittance_controller/src/admittance_controller.cpp | 7 ++++--- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 4c10e4b0d1..d9fd9f9947 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -12,6 +12,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) +find_package(control_toolbox REQUIRED) find_package(controller_interface REQUIRED) find_package(Eigen3 REQUIRED) find_package(filters REQUIRED) @@ -43,6 +44,7 @@ target_include_directories( ament_target_dependencies( admittance_controller control_msgs + control_toolbox controller_interface ${Eigen_LIBRARIES} filters @@ -113,6 +115,7 @@ ament_export_libraries( ) ament_export_dependencies( control_msgs + control_toolbox controller_interface filters geometry_msgs diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 297afd4874..e5f1b80f9e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -22,8 +22,8 @@ #include "angles/angles.h" #include "admittance_controller/moveit_kinematics.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" +#include "control_toolbox/parameter_handler.hpp" #include "controller_interface/controller_interface.hpp" -#include "controller_interface/controller_parameters.hpp" #include "filters/filter_chain.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -169,10 +169,10 @@ void convert_array_to_message(const Type & vector, geometry_msgs::msg::Transform namespace admittance_controller { -class AdmittanceParameters : public controller_interface::ControllerParameters +class AdmittanceParameters : public control_toolbox::ParameterHandler { public: - AdmittanceParameters() : controller_interface::ControllerParameters("", 7, 24, 4) + AdmittanceParameters() : control_toolbox::ParameterHandler("", 7, 0, 24, 4) { add_string_parameter("IK.base", false); add_string_parameter("IK.group_name", false); diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 06f8bd8fdc..7c6aea0065 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake control_msgs + control_toolbox controller_interface filters geometry_msgs diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 19aae8ac7b..65f0a0527a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -50,6 +50,8 @@ CallbackReturn AdmittanceController::on_init() admittance_ = std::make_unique(); + admittance_->parameters_.initialize(get_node()); + try { // TODO: use variables as parameters get_node()->declare_parameter>("joints", std::vector({})); @@ -62,8 +64,7 @@ CallbackReturn AdmittanceController::on_init() // TODO(destogl): enable when IK-plugin support is added // get_node()->declare_parameter("IK.plugin", ""); - admittance_->parameters_.declare_parameters(get_node()); - + 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; @@ -110,7 +111,7 @@ CallbackReturn AdmittanceController::on_configure( get_bool_param_and_error_if_empty(use_joint_commands_as_input_, "use_joint_commands_as_input") || get_string_param_and_error_if_empty(joint_limiter_type_, "joint_limiter_type") || - !admittance_->parameters_.get_parameters(get_node()) + !admittance_->parameters_.get_parameters() ) { RCLCPP_ERROR(get_node()->get_logger(), "Error happened during reading parameters");