From a1154d89ba82a81a7bf49466e16198af383e0a14 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 10:44:51 +0000 Subject: [PATCH] added a reset --- lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp | 2 +- lbr_fri_ros2/src/ft_estimator.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 68f433c0..ce171ab6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -34,9 +34,9 @@ class FTEstimator { FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", const std::string &chain_tip = "link_ee"); - void compute(const_jnt_pos_array_t_ref measured_joint_position, const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext); + void reset(); protected: KDL::Tree tree_; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index a175d881..e36e5ccd 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -18,9 +18,7 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - q_.data.setZero(); - tau_ext_.setZero(); - f_ext_.setZero(); + reset(); } void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, @@ -40,4 +38,10 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, Eigen::Map>(f_ext.data()) = f_ext_; } + +void FTEstimator::reset() { + q_.data.setZero(); + tau_ext_.setZero(); + f_ext_.setZero(); +} } // end of namespace lbr_fri_ros2