From 609d98b8a1bc9d3b41a8864951eb420ebd004723 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Fri, 5 Apr 2024 18:32:57 -0400 Subject: [PATCH 1/7] Use the urdf_ to set the robot_description in admittance controller --- admittance_controller/src/admittance_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a4b56d739c..2cc57a6a2e 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -279,6 +279,12 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); + // The AdmittanceRule / kinematics_interface::KinematicsInterfaceKDL requires + // the robot_description as a parameter. The controller manager stores the + // robot description in the urdf_ variable. + get_node()->declare_parameter("robot_description", rclcpp::PARAMETER_STRING); + get_node()->set_parameter(rclcpp::Parameter("robot_description", urdf_)); + // configure admittance rule if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) { From a4b90dbeb9961324c8e6276e9239158d4ad7ec76 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 14:02:01 -0400 Subject: [PATCH 2/7] Pass robot_description to kinematics_interface using input arg --- .../include/admittance_controller/admittance_rule.hpp | 3 ++- .../admittance_controller/admittance_rule_impl.hpp | 6 ++++-- admittance_controller/src/admittance_controller.cpp | 10 +++------- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7223dbe9d1..a326b663d0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -102,7 +102,8 @@ class AdmittanceRule /// Configure admittance rule memory using number of joints. controller_interface::return_type configure( - const std::shared_ptr & node, const size_t num_joint); + const std::shared_ptr & node, const size_t num_joint, + const std::string & robot_description); /// Reset all values back to default controller_interface::return_type reset(const size_t num_joints); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index cab8b4cf45..4e63416a46 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -20,6 +20,7 @@ #include "admittance_controller/admittance_rule.hpp" #include +#include #include #include @@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( - const std::shared_ptr & node, const size_t num_joints) + const std::shared_ptr & node, const size_t num_joints, + const std::string & robot_description) { num_joints_ = num_joints; @@ -58,7 +60,7 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - node->get_node_parameters_interface(), parameters_.kinematics.tip)) + node->get_node_parameters_interface(), parameters_.kinematics.tip, robot_description)) { return controller_interface::return_type::ERROR; } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 2cc57a6a2e..50a6b10e1b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -279,14 +279,10 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); - // The AdmittanceRule / kinematics_interface::KinematicsInterfaceKDL requires - // the robot_description as a parameter. The controller manager stores the - // robot description in the urdf_ variable. - get_node()->declare_parameter("robot_description", rclcpp::PARAMETER_STRING); - get_node()->set_parameter(rclcpp::Parameter("robot_description", urdf_)); - // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + if ( + admittance_->configure(get_node(), num_joints_, urdf_) == + controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; } From 30c149aa164da4036310bd96fe8d46f1149c4920 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 15 Apr 2024 19:18:26 -0400 Subject: [PATCH 3/7] admittance_controller: skip large dt periods --- admittance_controller/src/admittance_controller.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 50a6b10e1b..2edc1066d9 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -392,6 +392,12 @@ controller_interface::return_type AdmittanceController::update_and_write_command return controller_interface::return_type::ERROR; } + if (period.seconds() > 5.0) { + RCLCPP_WARN( + get_node()->get_logger(), "Large dt, skipping!"); + return controller_interface::return_type::OK; + } + // update input reference from chainable interfaces read_state_reference_interfaces(reference_); From 0a944d10ffb5949fde7a6a63636d3ed9ede6c5b6 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Thu, 1 Aug 2024 11:23:59 +0200 Subject: [PATCH 4/7] get robot_description correctly --- admittance_controller/src/admittance_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 2edc1066d9..1718ded8e3 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -281,7 +281,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( // configure admittance rule if ( - admittance_->configure(get_node(), num_joints_, urdf_) == + admittance_->configure(get_node(), num_joints_, this->get_robot_description()) == controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; From 79f6ebfef58ebce3c105b62dd19cf811f6d06709 Mon Sep 17 00:00:00 2001 From: Nikola Banovic Date: Thu, 1 Aug 2024 11:24:41 +0200 Subject: [PATCH 5/7] correct for kinematics_interface initialize() API change --- .../include/admittance_controller/admittance_rule_impl.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 4e63416a46..3480225ea4 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -60,7 +60,9 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - node->get_node_parameters_interface(), parameters_.kinematics.tip, robot_description)) + robot_description, + node->get_node_parameters_interface(), + "kinematics")) { return controller_interface::return_type::ERROR; } From 48131cdde792c6d22765108cd7fac9c9f6b3d50d Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 16:22:46 +0200 Subject: [PATCH 6/7] Remove skipping of large dts because why to add that? --- admittance_controller/src/admittance_controller.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1718ded8e3..6e0e38140a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -392,12 +392,6 @@ controller_interface::return_type AdmittanceController::update_and_write_command return controller_interface::return_type::ERROR; } - if (period.seconds() > 5.0) { - RCLCPP_WARN( - get_node()->get_logger(), "Large dt, skipping!"); - return controller_interface::return_type::OK; - } - // update input reference from chainable interfaces read_state_reference_interfaces(reference_); From 45e37e992577dee706c75ccb83c7a1dae2460fda Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 6 Nov 2024 19:50:04 +0000 Subject: [PATCH 7/7] pass robot_description at controller init --- .../admittance_controller/admittance_rule_impl.hpp | 4 +--- .../test/test_admittance_controller.hpp | 9 +++------ 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 3480225ea4..13d4e67fbc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -60,9 +60,7 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - robot_description, - node->get_node_parameters_interface(), - "kinematics")) + robot_description, node->get_node_parameters_interface(), "kinematics")) { return controller_interface::return_type::ERROR; } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b2a95c12fa..4f4635d861 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -102,7 +102,6 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon } } -private: 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; }; @@ -110,10 +109,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon class AdmittanceControllerTest : public ::testing::Test { public: - static void SetUpTestCase() - { - // rclcpp::init(0, nullptr); - } + static void SetUpTestCase() {} void SetUp() { @@ -163,7 +159,8 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = controller_->init(controller_name, "", 0, "", options); + auto result = + controller_->init(controller_name, controller_->robot_description_, 0, "", options); controller_->export_reference_interfaces(); assign_interfaces();