From d1a2f1322778dd84ab3ff302890cd6d89e437bec Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Fri, 5 Apr 2024 18:32:57 -0400 Subject: [PATCH 1/3] 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 c6a8168736..1aa55ada66 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -283,6 +283,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 2851e980fcaf3113e4c7cb6d8ecc6d013b1079c1 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 14:02:01 -0400 Subject: [PATCH 2/3] 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 36c027491c..2a6d755104 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -109,7 +109,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 9b03924882..bf72b3a408 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 "rclcpp/duration.hpp" @@ -33,7 +34,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; @@ -51,7 +53,7 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_ = std::unique_ptr( 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 1aa55ada66..048b410cc7 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -283,14 +283,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 e33b223e9bf34d85703b7371cb20bbd31e287c78 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 15 Apr 2024 19:18:26 -0400 Subject: [PATCH 3/3] 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 048b410cc7..9db5121bf6 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -396,6 +396,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_);