Additional tools for force estimation

.gitignore

@@ -0,0 +1,2 @@
+.vscode
+*.kdev

CMakeLists.txt

@@ -3,10 +3,18 @@ project(estimation_utils VERSION 1.0.0)
 
 find_package(Eigen3 REQUIRED)
 
+find_package(catkin REQUIRED)
+
+catkin_package()
+
 set(ESTIMATION_UTILS_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR})
 
+include_directories(include/utils)
+
 add_subdirectory(src/kalman)
 
+add_subdirectory(src/force_estimation)
+
 add_subdirectory(src/payload)
 
 add_subdirectory(src/nodes)

config/force_estimation_node_centauro.yaml

@@ -0,0 +1,32 @@
+force_estimator:
+  rate: 100.0
+  use_momentum_based: false
+  is_model_floating_base: true
+  model_type: RBDL
+  links: [arm1_6, arm2_6]
+  chains: [left_arm, right_arm]
+  arm1_6:
+    dofs: [0, 1, 2]
+  arm2_6:
+    dofs: [0, 1, 2]
+  ignored_joints: []
+  svd_threshold: 0.05
+  #reset offset
+  sec_to_reset: 3
+  #log
+  enable_log: false
+  #ref frame wrt forces are published
+  ref_frame: pelvis
+  #filter
+  enable_filter: true
+  filter_damp: 0.8
+  filter_bw: 3
+  filter_dead_zone: 0
+  #markers
+  pub_arrows: true
+  #The scale factor will scale the incoming forces vector to show the arrow with a scaled magnitude.
+  #Then, after the scaling, if the arrow has a norm > arrow_max_norm, the arrow norm will be saturated.
+  #Consider that arrow_max_norm are meters, so set these values according to robot size for a useful visualization
+  arrow_scale_factor: 20  # The number will DIVIDE each component, so bigger == shorter arrows
+  arrow_max_norm: 3
+

config/force_estimation_node_inail2arm.yaml

@@ -0,0 +1,25 @@
+force_estimator:
+  rate: 100.0
+  use_momentum_based: false
+  is_model_floating_base: false
+  model_type: RBDL
+  links: [dagana_1_tcp]
+  chains: [arm]
+  dagana_1_tcp:
+    dofs: [0, 1, 2, 3, 4, 5]
+  ignored_joints: [dagana_1_claw_joint]
+  svd_threshold: 0.05
+  #reset offset
+  sec_to_reset: 3
+  #log
+  enable_log: false
+  #ref frame wrt forces are published
+  ref_frame: base_link
+  #filter
+  enable_filter: false
+  filter_damp: 0.8
+  filter_bw: 3
+  filter_dead_zone: 0
+  #markers
+  pub_arrows: false
+

config/force_estimation_node_relax.yaml

@@ -0,0 +1,26 @@
+force_estimator:
+  rate: 100.0
+  use_momentum_based: false
+  is_model_floating_base: false
+  model_type: RBDL
+  links: [relax_arm1_linkEndEffector]
+  chains: [arm]
+  relax_arm1_linkEndEffector:
+    dofs: [0, 1, 2]
+  ignored_joints: []
+  svd_threshold: 0.05
+  #reset offset
+  sec_to_reset: 3
+  #log
+  enable_log: false
+  #ref frame wrt forces are published
+  ref_frame: ""
+  #filter
+  enable_filter: true
+  filter_damp: 0.8
+  filter_bw: 3
+  filter_dead_zone: 0
+  #markers
+  pub_arrows: true
+  arrow_scale_factor: 30  # The number will DIVIDE each component, so bigger == shorter arrows
+  arrow_max_norm: 3

include/estimation_utils/payload/ForceEstimation.h → include/estimation_utils/force_estimation/ForceEstimation.h

@@ -6,6 +6,8 @@
 // #include <cartesian_interface/Macro.h>
 #include <matlogger2/matlogger2.h>
 
+#include <estimation_utils/utils/SecondOrderFilter.h>
+
 namespace estimation_utils
 {
 
@@ -16,7 +18,7 @@ class ForceEstimation
 
     // CARTESIO_DECLARE_SMART_PTR(ForceEstimation)
 
-    static const double DEFAULT_SVD_THRESHOLD;
+    static constexpr double DEFAULT_SVD_THRESHOLD = 0.05;
 
     /**
      * @brief ForceEstimation constructor.
@@ -25,6 +27,7 @@ class ForceEstimation
      * @param svd_threshold: threshold for solution regularization (close to singularities)
      */
     ForceEstimation(XBot::ModelInterface::ConstPtr model,
+                    double rate,
                     double svd_threshold = DEFAULT_SVD_THRESHOLD);
 
     /**
@@ -56,9 +59,14 @@ class ForceEstimation
 
     bool getResiduals(Eigen::VectorXd &res) const;
 
+    void resetOffset(double sec=3);
+
+    bool initFilter(const double& damping = 0.8, const double& bw = 3, const double& dead_zone = 0);
+
 protected:
 
     XBot::ModelInterface::ConstPtr _model;
+    double _rate;
 
 private:
 
@@ -70,7 +78,9 @@ class ForceEstimation
         XBot::ForceTorqueSensor::Ptr sensor;
         std::vector<int> dofs;
         std::string link_name;
-
+        //probably is better to store this into the forceTorque class
+        Eigen::Vector6d wrench_offset;
+        estimation_utils::utils::FilterWrap<Eigen::Vector6d>::Ptr filter;
     };
 
     std::set<int> _ignore_idx;
@@ -88,6 +98,18 @@ class ForceEstimation
     Eigen::JacobiSVD<Eigen::MatrixXd> _svd;
     Eigen::ColPivHouseholderQR<Eigen::MatrixXd> _qr;
 
+    //reset the offset things
+    bool _reset_offset_requested = false;
+    bool _reset_offset_running  = false;
+    unsigned int _reset_offset_i = 0;
+    double _reset_offset_N = 0;
+
+    //filter things. For now all ft have the same filter params
+    double _filter_damping;
+    double _filter_bw;
+    double _filter_dead_zone;
+    bool _filter_in_use = false;
+
 };
 
 class ForceEstimationMomentumBased : public ForceEstimation
@@ -98,10 +120,9 @@ class ForceEstimationMomentumBased : public ForceEstimation
     // CARTESIO_DECLARE_SMART_PTR(ForceEstimationMomentumBased);
 
     static constexpr double DEFAULT_OBS_BW = 4.0;
-    static constexpr double DEFAULT_RATE = 200.0;
 
     ForceEstimationMomentumBased(XBot::ModelInterface::ConstPtr model,
-                                 double rate = DEFAULT_RATE,
+                                 double rate,
                                  double svd_threshold = DEFAULT_SVD_THRESHOLD,
                                  double obs_bw = DEFAULT_OBS_BW);
 
@@ -112,7 +133,7 @@ class ForceEstimationMomentumBased : public ForceEstimation
 
     void init_momentum_obs();
 
-    double _rate, _k_obs;
+    double _k_obs;
 
     Eigen::VectorXd _y, _tau, _g, _b, _sol;
     Eigen::VectorXd _p0, _p1, _p2, _q, _qdot, _q_old, _h, _coriolis, _y_static;

include/estimation_utils/nodes/ForceEstimationNode.h

@@ -0,0 +1,72 @@
+#ifndef FORCE_ESTIMATION_NODE_H
+#define FORCE_ESTIMATION_NODE_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <tf_conversions/tf_eigen.h>
+#include <std_srvs/Empty.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <XBotInterface/RobotInterface.h>
+#include <RobotInterfaceROS/ConfigFromParam.h>
+#include <matlogger2/matlogger2.h>
+//for waitForXbotCore method
+#include <xbot_msgs/PluginStatus.h>
+
+#include <estimation_utils/force_estimation/ForceEstimation.h>
+
+namespace estimation_utils {
+
+class ForceEstimationNode {
+
+public:
+    ForceEstimationNode(ros::NodeHandle* nh, const double& rate);
+
+    bool init();
+
+    void publishArrows();  
+
+    bool run();
+
+private:
+
+    ros::NodeHandle* _nh;
+
+    const double _rate;
+
+    XBot::RobotInterface::Ptr _robot;
+    XBot::ModelInterface::Ptr _model;
+    XBot::ImuSensor::ConstPtr _imu;
+
+    std::shared_ptr<estimation_utils::ForceEstimation> _f_est_ptr;
+    Eigen::VectorXd _tau, _tau_offset;
+    geometry_msgs::WrenchStamped _wrench_msg;
+
+    std::map<std::string, XBot::ForceTorqueSensor::ConstPtr> _ft_map;
+    std::map<std::string, ros::Publisher> _ft_pub_map;
+
+    //offset reset
+    ros::ServiceServer _request_wrench_zero_offset;
+    double _reset_time_sec;
+    bool _reset_requested = false;
+    bool wrenchZeroOffsetClbk(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+
+    //markers pub
+    bool _pub_markers;
+    double _arrow_scale_factor, _arrow_max_norm;
+    ros::Publisher _arrows_pub;
+    visualization_msgs::Marker _marker;
+
+    std::string _ref_frame;
+
+    //log
+    XBot::MatLogger2::Ptr _logger;
+
+    bool waitForXbotCore(ros::NodeHandle* nh, ros::Duration timeout = ros::Duration(-1));
+};
+
+} //namespace
+
+#endif // FORCE_ESTIMATION_NODE_H
+

include/estimation_utils/payload/payload_estimation.h

@@ -3,8 +3,8 @@
 
 #include <Eigen/Dense>
 #include <XBotInterface/ModelInterface.h>
-#include <estimation_utils/payload/ForceEstimation.h>
 
+#include "../force_estimation/ForceEstimation.h"
 #include "../kalman/kalman.h"
 
 namespace estimation_utils