diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 77f1277b35..cab8b4cf45 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -46,11 +46,17 @@ controller_interface::return_type AdmittanceRule::configure( { try { + // Make sure we destroy the interface first. Otherwise we might run into a segfault + if (kinematics_loader_) + { + kinematics_.reset(); + } kinematics_loader_ = std::make_shared>( parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( node->get_node_parameters_interface(), parameters_.kinematics.tip)) {