added a reset

lbr-stack · Dec 8, 2023 · a1154d8 · a1154d8
1 parent 5af8e51
commit a1154d8
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 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
@@ -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
@@ -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<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
 }
+
+void FTEstimator::reset() {
+  q_.data.setZero();
+  tau_ext_.setZero();
+  f_ext_.setZero();
+}
 } // end of namespace lbr_fri_ros2