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 c6a8168736..9db5121bf6 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -284,7 +284,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + if ( + admittance_->configure(get_node(), num_joints_, urdf_) == + controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; } @@ -394,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_);