From 43463696d70c8c4094a982a00e18c62c6afb2562 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Thu, 18 Jan 2024 16:42:21 +0100 Subject: [PATCH 01/14] to different folder --- .../{payload => force_estimation}/ForceEstimation.h | 0 src/{payload => force_estimation}/ForceEstimation.cpp | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename include/estimation_utils/{payload => force_estimation}/ForceEstimation.h (100%) rename src/{payload => force_estimation}/ForceEstimation.cpp (100%) diff --git a/include/estimation_utils/payload/ForceEstimation.h b/include/estimation_utils/force_estimation/ForceEstimation.h similarity index 100% rename from include/estimation_utils/payload/ForceEstimation.h rename to include/estimation_utils/force_estimation/ForceEstimation.h diff --git a/src/payload/ForceEstimation.cpp b/src/force_estimation/ForceEstimation.cpp similarity index 100% rename from src/payload/ForceEstimation.cpp rename to src/force_estimation/ForceEstimation.cpp From 729249200e663a017afe092bfef7d8e02a134d36 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Thu, 18 Jan 2024 16:48:22 +0100 Subject: [PATCH 02/14] gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..35d6a52 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.vscode +*.kdev \ No newline at end of file From ece4d439eb94a719569fc540cb3798f06b7ce151 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Thu, 18 Jan 2024 16:49:49 +0100 Subject: [PATCH 03/14] offset computation --- CMakeLists.txt | 2 + .../force_estimation/ForceEstimation.h | 19 +++++--- .../payload/payload_estimation.h | 2 +- src/force_estimation/CMakeLists.txt | 24 ++++++++++ src/force_estimation/ForceEstimation.cpp | 45 ++++++++++++++++--- src/payload/CMakeLists.txt | 4 +- 6 files changed, 81 insertions(+), 15 deletions(-) create mode 100644 src/force_estimation/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 867ae5d..4b89b92 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,8 @@ set(ESTIMATION_UTILS_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(src/kalman) +add_subdirectory(src/force_estimation) + add_subdirectory(src/payload) add_subdirectory(src/nodes) diff --git a/include/estimation_utils/force_estimation/ForceEstimation.h b/include/estimation_utils/force_estimation/ForceEstimation.h index 049d218..09ff1f0 100644 --- a/include/estimation_utils/force_estimation/ForceEstimation.h +++ b/include/estimation_utils/force_estimation/ForceEstimation.h @@ -16,7 +16,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 +25,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 +57,12 @@ class ForceEstimation bool getResiduals(Eigen::VectorXd &res) const; + void resetOffset(double sec=3); + protected: XBot::ModelInterface::ConstPtr _model; + double _rate; private: @@ -70,7 +74,8 @@ class ForceEstimation XBot::ForceTorqueSensor::Ptr sensor; std::vector dofs; std::string link_name; - + //probably is better to store this into the forceTorque class + Eigen::Vector6d wrench_offset; }; std::set _ignore_idx; @@ -88,6 +93,11 @@ class ForceEstimation Eigen::JacobiSVD _svd; Eigen::ColPivHouseholderQR _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; }; class ForceEstimationMomentumBased : public ForceEstimation @@ -98,10 +108,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 +121,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; diff --git a/include/estimation_utils/payload/payload_estimation.h b/include/estimation_utils/payload/payload_estimation.h index f1bfc6e..8b5ea17 100644 --- a/include/estimation_utils/payload/payload_estimation.h +++ b/include/estimation_utils/payload/payload_estimation.h @@ -3,8 +3,8 @@ #include #include -#include +#include "../force_estimation/ForceEstimation.h" #include "../kalman/kalman.h" namespace estimation_utils diff --git a/src/force_estimation/CMakeLists.txt b/src/force_estimation/CMakeLists.txt new file mode 100644 index 0000000..786624c --- /dev/null +++ b/src/force_estimation/CMakeLists.txt @@ -0,0 +1,24 @@ +find_package(XBotInterface REQUIRED) +find_package(matlogger2 REQUIRED) + +add_library(force_estimation SHARED + ForceEstimation.cpp) + +set_target_properties(force_estimation PROPERTIES PREFIX "${PROJECT_NAME}_") + +target_link_libraries(force_estimation + PUBLIC + Eigen3::Eigen + XBotInterface::XBotInterface + matlogger2::matlogger2 +) + +target_include_directories(force_estimation + PUBLIC + $ + $ +) + +install(TARGETS force_estimation + EXPORT ${PROJECT_NAME}Targets + DESTINATION lib) diff --git a/src/force_estimation/ForceEstimation.cpp b/src/force_estimation/ForceEstimation.cpp index a090614..cf0c1ef 100644 --- a/src/force_estimation/ForceEstimation.cpp +++ b/src/force_estimation/ForceEstimation.cpp @@ -1,12 +1,13 @@ -#include +#include using namespace estimation_utils; -const double ForceEstimation::DEFAULT_SVD_THRESHOLD = 0.05; ForceEstimation::ForceEstimation(XBot::ModelInterface::ConstPtr model, + double rate, double svd_threshold): _model(model), + _rate(rate), _ndofs(0) { _svd.setThreshold(svd_threshold); @@ -75,6 +76,7 @@ XBot::ForceTorqueSensor::ConstPtr ForceEstimation::add_link(std::string name, static int id = -1; t.sensor = std::make_shared(urdf_link, id--); t.dofs = dofs; + t.wrench_offset.setZero(); _tasks.push_back(t); @@ -173,8 +175,23 @@ void ForceEstimation::update() wrench.head<3>() = sensor_R_w * wrench.head<3>(); wrench.tail<3>() = sensor_R_w * wrench.tail<3>(); - - t.sensor->setWrench(wrench, 0.0); + + if(_reset_offset_running) { + _reset_offset_i++; + std::cout << _reset_offset_i << " / " << _reset_offset_N << std::endl; + + //moving average + t.wrench_offset = t.wrench_offset + (wrench - t.wrench_offset) / _reset_offset_i; + + if (_reset_offset_i >= _reset_offset_N) { + + _reset_offset_running = false; + } + + } + std::cout << wrench.transpose() << std::endl; + std::cout << t.wrench_offset.transpose() << std::endl; + t.sensor->setWrench(wrench - t.wrench_offset, 0.0); } } @@ -185,6 +202,21 @@ bool ForceEstimation::getResiduals(Eigen::VectorXd& res) const return true; } +void ForceEstimation::resetOffset(double sec) { + + if (sec > 0) { + _reset_offset_i = 0; + _reset_offset_running = true; + _reset_offset_N = sec * _rate; + } + + for(TaskInfo& t : _tasks) + { + t.wrench_offset.setZero(); + } +} + + void estimation_utils::ForceEstimation::log(XBot::MatLogger2::Ptr logger) const { for(const TaskInfo& t : _tasks) @@ -212,9 +244,8 @@ ForceEstimationMomentumBased::ForceEstimationMomentumBased(XBot::ModelInterface: double rate, double svd_threshold, double obs_bw): - ForceEstimation(model, svd_threshold), - _k_obs(2.0 * M_PI * obs_bw), - _rate(rate) + ForceEstimation(model, rate, svd_threshold), + _k_obs(2.0 * M_PI * obs_bw) { init_momentum_obs(); } diff --git a/src/payload/CMakeLists.txt b/src/payload/CMakeLists.txt index 48f99d6..ef205c8 100644 --- a/src/payload/CMakeLists.txt +++ b/src/payload/CMakeLists.txt @@ -2,14 +2,14 @@ find_package(XBotInterface REQUIRED) find_package(matlogger2 REQUIRED) add_library(payload_estimation SHARED - payload_estimation.cpp - ForceEstimation.cpp) + payload_estimation.cpp) set_target_properties(payload_estimation PROPERTIES PREFIX "${PROJECT_NAME}_") target_link_libraries(payload_estimation PUBLIC kalman + force_estimation Eigen3::Eigen XBotInterface::XBotInterface matlogger2::matlogger2 From b318b389c0f03162ebb48a80829b021bbb38d690 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Thu, 18 Jan 2024 16:53:51 +0100 Subject: [PATCH 04/14] old prints --- src/force_estimation/ForceEstimation.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/force_estimation/ForceEstimation.cpp b/src/force_estimation/ForceEstimation.cpp index cf0c1ef..e509fea 100644 --- a/src/force_estimation/ForceEstimation.cpp +++ b/src/force_estimation/ForceEstimation.cpp @@ -178,7 +178,6 @@ void ForceEstimation::update() if(_reset_offset_running) { _reset_offset_i++; - std::cout << _reset_offset_i << " / " << _reset_offset_N << std::endl; //moving average t.wrench_offset = t.wrench_offset + (wrench - t.wrench_offset) / _reset_offset_i; @@ -189,8 +188,6 @@ void ForceEstimation::update() } } - std::cout << wrench.transpose() << std::endl; - std::cout << t.wrench_offset.transpose() << std::endl; t.sensor->setWrench(wrench - t.wrench_offset, 0.0); } From b9bed00c0f119379e338529b515e6d4b7c35f0bb Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Thu, 18 Jan 2024 18:15:47 +0100 Subject: [PATCH 05/14] todo copy pasted files --- .../force_estimation/ForceSensorHandler.h | 78 ++++++++ .../ForceEstimationHandler.cpp | 0 src/nodes/force_estimation_node.cpp | 187 ++++++++++++++++++ 3 files changed, 265 insertions(+) create mode 100644 include/estimation_utils/force_estimation/ForceSensorHandler.h create mode 100644 src/force_estimation/ForceEstimationHandler.cpp create mode 100644 src/nodes/force_estimation_node.cpp diff --git a/include/estimation_utils/force_estimation/ForceSensorHandler.h b/include/estimation_utils/force_estimation/ForceSensorHandler.h new file mode 100644 index 0000000..1ebfdc2 --- /dev/null +++ b/include/estimation_utils/force_estimation/ForceSensorHandler.h @@ -0,0 +1,78 @@ +#ifndef FORCE_ESTIMATION_HANDLER_H +#define FORCE_ESTIMATION_HANDLER_H + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +namespace tpo { + +class ForceSensorHandler{ + +public: + ForceSensorHandler(); + + bool init(const std::unique_ptr& nh, std::string group_name, std::string sensor_link, + std::string ref_link, bool filter, double force_dead_zone_limit, + double period_sec, bool pub_arrows = true); + + bool reset(); + + void getSensedWrench(Eigen::Matrix& w) const; + void getFilteredSensedWrench(Eigen::Matrix & w) const; + + void publishArrows(); + + void update(); + + std::string getSensorLink() const; + + +private: + + std::string group_name; + std::string sensor_link; + std::string ref_link; + + ros::Subscriber sensed_wrench_sub; + geometry_msgs::WrenchStamped sensed_wrench_raw_msg; + bool sensed_wrench_new_data; + void sensedWrenchSubCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg); + std::mutex sensed_wrench_mutex; + + Eigen::Matrix sensed_wrench; + Eigen::Matrix sensed_wrench_filtered; + + bool filter; + void filterSensedForce(); + double force_dead_zone_limit; + tpo::utils::SecondOrderFilter> filter_sensed_force; + bool initSensedForceFilter(); + double period_sec; + + ros::Publisher sensed_force_filtered_pub; + + ros::Publisher sensed_force_marker_pub; + bool pub_arrows; + + tf2_ros::Buffer tf_buffer; + std::unique_ptr tf_listener; +}; + +} //namespace + +#endif // FORCE_ESTIMATION_HANDLER_H + diff --git a/src/force_estimation/ForceEstimationHandler.cpp b/src/force_estimation/ForceEstimationHandler.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/nodes/force_estimation_node.cpp b/src/nodes/force_estimation_node.cpp new file mode 100644 index 0000000..c8ccd37 --- /dev/null +++ b/src/nodes/force_estimation_node.cpp @@ -0,0 +1,187 @@ +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +//https://github.com/ADVRHumanoids/CartesianInterface/blob/2.0-devel/src/ros/ForceEstimationNode.cpp + +bool reset_requested = false; + +bool wrenchZeroOffsetClbk(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + reset_requested = true; + + return true; +} + +int main(int argc, char ** argv) +{ + + // init ros, get handles + ros::init(argc, argv, "force_estimation_node"); + ros::NodeHandle nh_priv("~"); + + //lets wait for xbot + if (!tpo::utils::waitForXbotCore(&nh_priv)) { + + ROS_ERROR_STREAM("WaitForXbotCore failed, exiting.."); + return -1; + } + + + // get robot, model, and one imu + auto robot = XBot::RobotInterface::getRobot(XBot::ConfigOptionsFromParamServer(nh_priv)); + auto model = XBot::ModelInterface::getModel(XBot::ConfigOptionsFromParamServer(nh_priv)); + XBot::ImuSensor::ConstPtr imu; + + if(robot->getImu().size() > 0) + { + imu = robot->getImu().begin()->second; + } + else + { + ROS_INFO("IMU not found: map is empty\n"); + } + + // configure node + double rate = nh_priv.param("rate", 100.0); + auto links = nh_priv.param("links", std::vector()); + + if(links.size() == 0) + { + ROS_INFO("Private parameter ~/links is empty, exiting.."); + return 1; + } + + auto chains = nh_priv.param("chains", std::vector()); + if(links.size() == 0) + { + ROS_INFO("Private parameter ~/chains is empty, will use all torque sensors"); + } + + double svd_th = nh_priv.param("svd_threshold", (double)estimation_utils::ForceEstimation::DEFAULT_SVD_THRESHOLD); + + // get torque offset map + auto tau_off_map = nh_priv.param("torque_offset", std::map()); + XBot::JointNameMap tau_off_map_xbot(tau_off_map.begin(), tau_off_map.end()); + Eigen::VectorXd tau_offset; + tau_offset.setZero(model->getJointNum()); + model->mapToEigen(tau_off_map_xbot, tau_offset); + + ///// ros service to request for zeroing the force estimation offset + ros::ServiceServer request_wrench_zero_offset; + request_wrench_zero_offset = nh_priv.advertiseService("wrench_zero_offset", wrenchZeroOffsetClbk); + double reset_time_sec = nh_priv.param("sec_to_reset", 3);; + + // construct force estimation class + std::shared_ptr f_est_ptr; + + if(nh_priv.param("use_momentum_based", false)) + { + ROS_INFO("using momentum based estimation"); + f_est_ptr = std::make_shared(model, rate, svd_th); + } + else + { + f_est_ptr = std::make_shared(model, rate, svd_th); + } + + estimation_utils::ForceEstimation& f_est = *f_est_ptr; + + // set ignored joints + auto ignored_joints = nh_priv.param("ignored_joints", std::vector()); + for(auto jname : ignored_joints) + { + //f_est.setIgnoredJoint(jname); + } + + // generate virtual fts + std::map ft_map; + + for(auto l : links) + { + auto dofs = nh_priv.param(l + "/dofs", std::vector()); + + auto pub = nh_priv.advertise("force_estimation/" + l, 1); + + ft_map[f_est.add_link(l, dofs, chains)] = pub; + } + + // log + XBot::MatLogger2::Ptr logger; + if(nh_priv.param("enable_log", false)) + { + logger = XBot::MatLogger2::MakeLogger("/tmp/force_estimation_log"); + } + + + // start looping + Eigen::VectorXd tau; + ros::Rate loop_rate(rate); + Eigen::Vector6d wrench; + + while(ros::ok()) + { + // update model from robot, set imu + robot->sense(false); + model->syncFrom(*robot, XBot::Sync::All, XBot::Sync::MotorSide); + if(model->isFloatingBase() && imu) + { + model->setFloatingBaseState(imu); + model->update(); + } + model->update(); + model->getJointEffort(tau); + tau += tau_offset; + model->setJointEffort(tau); + + //reset offset stuff + if (reset_requested) { + + f_est.resetOffset(reset_time_sec); + reset_requested = false; + } + + // update force estimation + f_est.update(); + + // publish to topics + + for(const auto& ft_pub : ft_map) + { + geometry_msgs::WrenchStamped msg; + + ft_pub.first->getWrench(wrench); + + tf::wrenchEigenToMsg(wrench, msg.wrench); + + msg.header.frame_id = ft_pub.first->getSensorName(); + msg.header.stamp = ros::Time::now(); + + ft_pub.second.publish(msg); + } + + // log + if(logger) + { + f_est.log(logger); + } + + + ros::spinOnce(); + // sync with desired loop freq + loop_rate.sleep(); + } + + return 0; +} From 3f60580510cc29f143e23f785cf4592ef3a00869 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Thu, 25 Jan 2024 17:57:02 +0100 Subject: [PATCH 06/14] launches, packages to run the force estimation node (it works) --- CMakeLists.txt | 4 + config/force_estimation_node_centauro.yaml | 11 + config/force_estimation_node_relax.yaml | 9 + launch/force_estimation_centauro.launch | 29 +++ launch/force_estimation_relax.launch | 30 +++ package.xml | 60 +++++ .../ForceEstimationHandler.cpp | 232 ++++++++++++++++++ src/nodes/CMakeLists.txt | 24 +- src/nodes/force_estimation_node.cpp | 47 +++- 9 files changed, 439 insertions(+), 7 deletions(-) create mode 100644 config/force_estimation_node_centauro.yaml create mode 100644 config/force_estimation_node_relax.yaml create mode 100644 launch/force_estimation_centauro.launch create mode 100644 launch/force_estimation_relax.launch create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b89b92..2eb3981 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,10 @@ 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}) add_subdirectory(src/kalman) diff --git a/config/force_estimation_node_centauro.yaml b/config/force_estimation_node_centauro.yaml new file mode 100644 index 0000000..2deaeb2 --- /dev/null +++ b/config/force_estimation_node_centauro.yaml @@ -0,0 +1,11 @@ +force_estimator: + rate: 100.0 + use_momentum_based: false + is_model_floating_base: true + model_type: RBDL + links: [arm1_8, arm2_8] + chains: [left_arm, right_arm] + arm1_8: + dofs: [0, 1, 2] + arm2_8: + dofs: [0, 1, 2] \ No newline at end of file diff --git a/config/force_estimation_node_relax.yaml b/config/force_estimation_node_relax.yaml new file mode 100644 index 0000000..91f22f3 --- /dev/null +++ b/config/force_estimation_node_relax.yaml @@ -0,0 +1,9 @@ +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] \ No newline at end of file diff --git a/launch/force_estimation_centauro.launch b/launch/force_estimation_centauro.launch new file mode 100644 index 0000000..a7ccafc --- /dev/null +++ b/launch/force_estimation_centauro.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/force_estimation_relax.launch b/launch/force_estimation_relax.launch new file mode 100644 index 0000000..4546d53 --- /dev/null +++ b/launch/force_estimation_relax.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..462839c --- /dev/null +++ b/package.xml @@ -0,0 +1,60 @@ + + + estimation_utils + 0.0.1 + The estimation_utils main package + + + + + hhcm + + + + + + GPLv3 + + + + + + + + + + + + + + + + + + + roscpp + rospy + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/src/force_estimation/ForceEstimationHandler.cpp b/src/force_estimation/ForceEstimationHandler.cpp index e69de29..18f2327 100644 --- a/src/force_estimation/ForceEstimationHandler.cpp +++ b/src/force_estimation/ForceEstimationHandler.cpp @@ -0,0 +1,232 @@ +#include + +using tpo::ForceSensorHandler; + +ForceSensorHandler::ForceSensorHandler() {} + + +bool tpo::ForceSensorHandler::init(const std::unique_ptr& nh, std::string group_name, std::string sensor_link, + std::string ref_link, bool filter, double force_dead_zone_limit, + double period_sec, bool pub_arrows) +{ + this->group_name = group_name; + this->sensor_link = sensor_link; + this->ref_link = ref_link; + + this->period_sec = period_sec; + this->filter = filter; + this->pub_arrows = pub_arrows; + + this-> force_dead_zone_limit = force_dead_zone_limit; //lower than this sensed forces are ignored + + std::string topic_name = "/force_estimator/force_estimation/" + sensor_link; + + sensed_wrench_sub = nh->subscribe(topic_name, 1, &ForceSensorHandler::sensedWrenchSubCallback, this); + sensed_wrench_raw_msg = geometry_msgs::WrenchStamped(); + sensed_wrench_new_data= false; + sensed_wrench= Eigen::Matrix::Zero(); + + tf_listener = std::make_unique(tf_buffer); + + if (filter) { + initSensedForceFilter(); + sensed_force_filtered_pub = nh->advertise("/force_estimation/force_filtered/" + sensor_link, 1); + } + + if (pub_arrows) { + sensed_force_marker_pub = nh->advertise("/force_estimation/force_marker/" + sensor_link, 1); + } + + return true; +} + +bool tpo::ForceSensorHandler::reset() +{ + + Eigen::VectorXd resetVect = Eigen::Matrix::Zero(); + + // reset filter + filter_sensed_force.reset(resetVect); + + sensed_wrench_raw_msg = geometry_msgs::WrenchStamped(); + sensed_wrench = Eigen::Matrix::Zero(); + sensed_wrench_filtered = Eigen::Matrix::Zero(); + sensed_wrench_new_data = false; + + return true; + +} + +void tpo::ForceSensorHandler::publishArrows() { + + visualization_msgs::MarkerArray markers; + + geometry_msgs::TransformStamped transformStamped; + try{ + transformStamped = tf_buffer.lookupTransform(sensor_link, ref_link, //first is target, second is source + ros::Time(0)); //latest available transform + } + catch (tf2::TransformException &ex) { + + ROS_WARN("FORCE VISUALIZER: %s",ex.what()); + //ros::Duration(1.0).sleep(); + return; + } + + Eigen::Quaterniond quat( + transformStamped.transform.rotation.w, + transformStamped.transform.rotation.x, + transformStamped.transform.rotation.y, + transformStamped.transform.rotation.z); + + visualization_msgs::Marker marker; + marker.header.frame_id = sensor_link; + marker.id = 0; + marker.header.stamp = ros::Time::now(); + marker.scale.x = 0.02; //shaft diameter + marker.scale.y = 0.04; //head diameter + marker.scale.z = 0.05; //head length auto computed + marker.pose.orientation.w = 1; + marker.frame_locked = true; + marker.points.push_back(geometry_msgs::Point()); + marker.points.push_back(geometry_msgs::Point()); + + if (!filter) { + + const auto & it = sensed_wrench; + + marker.ns = "raw"; + + marker.color.a = 0.8; + marker.color.r = 0.5; + marker.color.g = 0.5; + marker.color.b = 0.5; + + Eigen::Vector3d vect; + geometry_msgs::Point point1; + + vect = quat * it.head<3>(); + if (vect.norm() > 0.5) { + vect *= 0.5 / vect.norm() ; + } + point1.x = vect(0); + point1.y = vect(1); + point1.z = vect(2); + marker.points.at(1) = point1; + + } else { + + const auto & it = sensed_wrench_filtered; + + marker.ns = "filtered"; + + marker.color.a = 0.8; + marker.color.r = 0.2; + marker.color.g = 1.0; + marker.color.b = 0.9; + + Eigen::Vector3d vect; + geometry_msgs::Point point1; + + vect = quat * it.head<3>(); + if (vect.norm() > 0.5) { + vect *= 0.5 / vect.norm() ; + } + point1.x = vect(0); + point1.y = vect(1); + point1.z = vect(2); + marker.points.at(1) = point1; + } + + markers.markers.push_back(marker); + + sensed_force_marker_pub.publish(markers); +} + + +void tpo::ForceSensorHandler::sensedWrenchSubCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg) { + + const std::lock_guard lock(sensed_wrench_mutex); + + sensed_wrench_raw_msg = *msg; + sensed_wrench_new_data = true; +} + +void tpo::ForceSensorHandler::update() { + + const auto &it = sensed_wrench_raw_msg; + + if (sensed_wrench_new_data) { + + const std::lock_guard lock(sensed_wrench_mutex); + + geometry_msgs::WrenchStamped msg_transf; + + try { + tf_buffer.transform(it, msg_transf, ref_link); + } + catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + return; + } + + //DEBUG PRINTS + //std::cout << sensor_link << " : " << it.wrench.force.x << " " << it.wrench.force.y << " " << it.wrench.force.z << std::endl; + //std::cout << "after tf trasf: : " << msg_transf.wrench.force.x << " " << msg_transf.wrench.force.y << " " << msg_transf.wrench.force.z << std::endl; + + tf::wrenchMsgToEigen(msg_transf.wrench, sensed_wrench); + //std::cout << "after eigen trasf: : " << sensed_wrench.transpose() << std::endl; + + sensed_wrench_new_data = false; + } + + //even if not data, we must filtered because filter should be run at each loop rate given...? + if (filter) { + filterSensedForce(); + geometry_msgs::WrenchStamped msg; + tf::wrenchEigenToMsg(sensed_wrench_filtered, msg.wrench); + + msg.header.frame_id = ref_link; + msg.header.stamp = it.header.stamp; + + sensed_force_filtered_pub.publish(msg); + } + if (pub_arrows) { + publishArrows(); + } +} + +void tpo::ForceSensorHandler::filterSensedForce() { + + sensed_wrench_filtered = sensed_wrench; + + tpo::utils::cutOff(sensed_wrench_filtered, force_dead_zone_limit); + + sensed_wrench_filtered= filter_sensed_force.process(sensed_wrench_filtered); + +} + +bool tpo::ForceSensorHandler::initSensedForceFilter() { + + const double DAMPING_FACT = 0.8; + const double BW_MEDIUM = 3; + double omega = 2.0 * M_PI * BW_MEDIUM; + + Eigen::VectorXd resetVect = Eigen::Matrix::Zero(); + + filter_sensed_force = tpo::utils::SecondOrderFilter>(); + filter_sensed_force.setDamping ( DAMPING_FACT ); + filter_sensed_force.setTimeStep ( period_sec ); + filter_sensed_force.setOmega ( omega ); + + // reset filter + filter_sensed_force.reset(resetVect); + + sensed_wrench_filtered = Eigen::Matrix::Zero(); + + return true; +} + +void tpo::ForceSensorHandler::getSensedWrench(Eigen::Matrix & w) const { w = sensed_wrench; } +void tpo::ForceSensorHandler::getFilteredSensedWrench(Eigen::Matrix & w) const { w = sensed_wrench_filtered; } +std::string tpo::ForceSensorHandler::getSensorLink() const { return sensor_link; } diff --git a/src/nodes/CMakeLists.txt b/src/nodes/CMakeLists.txt index 0eadb4a..3b80aeb 100644 --- a/src/nodes/CMakeLists.txt +++ b/src/nodes/CMakeLists.txt @@ -1,16 +1,34 @@ find_package(roscpp REQUIRED) find_package(eigen_conversions REQUIRED) -add_executable(payload_estimation_node - payload_estimation_node.cpp) - include_directories( ${roscpp_INCLUDE_DIRECTORIES} ${eigen_conversions_INCLUDE_DIRECTORIES} ) +add_executable(payload_estimation_node + payload_estimation_node.cpp) + target_link_libraries(payload_estimation_node payload_estimation ${roscpp_LIBRARIES} ${eigen_conversions_LIBRARIES} ) + +add_executable(force_estimation_node + force_estimation_node.cpp) + + +target_link_libraries(force_estimation_node + force_estimation + ${roscpp_LIBRARIES} +) + +install(TARGETS + force_estimation_node + payload_estimation_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + diff --git a/src/nodes/force_estimation_node.cpp b/src/nodes/force_estimation_node.cpp index c8ccd37..a8f74f1 100644 --- a/src/nodes/force_estimation_node.cpp +++ b/src/nodes/force_estimation_node.cpp @@ -5,13 +5,15 @@ #include #include -#include +#include #include -#include - #include +//for waitForXbotCore method +#include + + //https://github.com/ADVRHumanoids/CartesianInterface/blob/2.0-devel/src/ros/ForceEstimationNode.cpp bool reset_requested = false; @@ -24,6 +26,43 @@ bool wrenchZeroOffsetClbk(std_srvs::Empty::Request &req, return true; } +bool waitForXbotCore(ros::NodeHandle* nh, ros::Duration timeout = ros::Duration(-1)) { + + //ros::Time startTime = ros::Time::now(); + ros::ServiceClient client = nh->serviceClient("/xbotcore/ros_io/state"); + + auto wait_lambda = [&](){ + + ROS_INFO_STREAM("Waiting for XbotCore..."); + if (!client.waitForExistence(timeout)) { + return false; + } + + return true; + }; + + if (! wait_lambda()) { + return false; + } + + xbot_msgs::PluginStatus status; + ros::Rate r(10); + ros::Duration(1).sleep(); //necessary because idk but service is not really ready and the client.call hands 4ever + while(status.response.status.compare("Running") != 0) { + + if (!client.call(status)) { +// ROS_INFO_STREAM("Call for XbotCore fail, I will retry..."); +// if (! wait_lambda()) { +// return false; +// } + } + r.sleep(); + } + + return true; + +} + int main(int argc, char ** argv) { @@ -32,7 +71,7 @@ int main(int argc, char ** argv) ros::NodeHandle nh_priv("~"); //lets wait for xbot - if (!tpo::utils::waitForXbotCore(&nh_priv)) { + if (!waitForXbotCore(&nh_priv)) { ROS_ERROR_STREAM("WaitForXbotCore failed, exiting.."); return -1; From c70cea0bf9b634e5ac2223a9298ab700224cf4ff Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Fri, 26 Jan 2024 18:01:47 +0100 Subject: [PATCH 07/14] filter and cutoff in library && node using these and publishing markers, plus launches and configs --- CMakeLists.txt | 2 + config/force_estimation_node_centauro.yaml | 20 +- .../force_estimation/ForceEstimation.h | 12 + .../force_estimation/ForceSensorHandler.h | 78 ----- .../nodes/ForceEstimationNode.h | 71 +++++ .../utils/SecondOrderFilter.h | 202 ++++++++++++ launch/force_estimation_centauro.launch | 6 +- launch/force_estimation_relax.launch | 2 +- src/force_estimation/ForceEstimation.cpp | 35 ++- .../ForceEstimationHandler.cpp | 232 -------------- src/nodes/CMakeLists.txt | 19 +- src/nodes/ForceEstimationNode.cpp | 294 ++++++++++++++++++ src/nodes/force_estimation_node.cpp | 204 +----------- 13 files changed, 657 insertions(+), 520 deletions(-) delete mode 100644 include/estimation_utils/force_estimation/ForceSensorHandler.h create mode 100644 include/estimation_utils/nodes/ForceEstimationNode.h create mode 100644 include/estimation_utils/utils/SecondOrderFilter.h delete mode 100644 src/force_estimation/ForceEstimationHandler.cpp create mode 100644 src/nodes/ForceEstimationNode.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2eb3981..3dd264e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,8 @@ catkin_package() set(ESTIMATION_UTILS_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}) +include_directories(include/utils) + add_subdirectory(src/kalman) add_subdirectory(src/force_estimation) diff --git a/config/force_estimation_node_centauro.yaml b/config/force_estimation_node_centauro.yaml index 2deaeb2..27beaa5 100644 --- a/config/force_estimation_node_centauro.yaml +++ b/config/force_estimation_node_centauro.yaml @@ -3,9 +3,25 @@ force_estimator: use_momentum_based: false is_model_floating_base: true model_type: RBDL - links: [arm1_8, arm2_8] + links: [arm1_6, arm2_6] chains: [left_arm, right_arm] arm1_8: dofs: [0, 1, 2] arm2_8: - dofs: [0, 1, 2] \ No newline at end of file + 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 + \ No newline at end of file diff --git a/include/estimation_utils/force_estimation/ForceEstimation.h b/include/estimation_utils/force_estimation/ForceEstimation.h index 09ff1f0..6caf1bd 100644 --- a/include/estimation_utils/force_estimation/ForceEstimation.h +++ b/include/estimation_utils/force_estimation/ForceEstimation.h @@ -6,6 +6,8 @@ // #include #include +#include + namespace estimation_utils { @@ -59,6 +61,8 @@ class ForceEstimation 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; @@ -76,6 +80,7 @@ class ForceEstimation std::string link_name; //probably is better to store this into the forceTorque class Eigen::Vector6d wrench_offset; + estimation_utils::utils::FilterWrap::Ptr filter; }; std::set _ignore_idx; @@ -98,6 +103,13 @@ class ForceEstimation 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 diff --git a/include/estimation_utils/force_estimation/ForceSensorHandler.h b/include/estimation_utils/force_estimation/ForceSensorHandler.h deleted file mode 100644 index 1ebfdc2..0000000 --- a/include/estimation_utils/force_estimation/ForceSensorHandler.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef FORCE_ESTIMATION_HANDLER_H -#define FORCE_ESTIMATION_HANDLER_H - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include - -#include - -namespace tpo { - -class ForceSensorHandler{ - -public: - ForceSensorHandler(); - - bool init(const std::unique_ptr& nh, std::string group_name, std::string sensor_link, - std::string ref_link, bool filter, double force_dead_zone_limit, - double period_sec, bool pub_arrows = true); - - bool reset(); - - void getSensedWrench(Eigen::Matrix& w) const; - void getFilteredSensedWrench(Eigen::Matrix & w) const; - - void publishArrows(); - - void update(); - - std::string getSensorLink() const; - - -private: - - std::string group_name; - std::string sensor_link; - std::string ref_link; - - ros::Subscriber sensed_wrench_sub; - geometry_msgs::WrenchStamped sensed_wrench_raw_msg; - bool sensed_wrench_new_data; - void sensedWrenchSubCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg); - std::mutex sensed_wrench_mutex; - - Eigen::Matrix sensed_wrench; - Eigen::Matrix sensed_wrench_filtered; - - bool filter; - void filterSensedForce(); - double force_dead_zone_limit; - tpo::utils::SecondOrderFilter> filter_sensed_force; - bool initSensedForceFilter(); - double period_sec; - - ros::Publisher sensed_force_filtered_pub; - - ros::Publisher sensed_force_marker_pub; - bool pub_arrows; - - tf2_ros::Buffer tf_buffer; - std::unique_ptr tf_listener; -}; - -} //namespace - -#endif // FORCE_ESTIMATION_HANDLER_H - diff --git a/include/estimation_utils/nodes/ForceEstimationNode.h b/include/estimation_utils/nodes/ForceEstimationNode.h new file mode 100644 index 0000000..fdd6f31 --- /dev/null +++ b/include/estimation_utils/nodes/ForceEstimationNode.h @@ -0,0 +1,71 @@ +#ifndef FORCE_ESTIMATION_NODE_H +#define FORCE_ESTIMATION_NODE_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +//for waitForXbotCore method +#include + +#include + +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 _f_est_ptr; + Eigen::VectorXd _tau, _tau_offset; + geometry_msgs::WrenchStamped _wrench_msg; + + std::map _ft_map; + std::map _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; + 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 + diff --git a/include/estimation_utils/utils/SecondOrderFilter.h b/include/estimation_utils/utils/SecondOrderFilter.h new file mode 100644 index 0000000..3680a14 --- /dev/null +++ b/include/estimation_utils/utils/SecondOrderFilter.h @@ -0,0 +1,202 @@ +/* + * Copyright (C) 2016 IIT-ADVR + * Author: Arturo Laurenzi, Luca Muratore + * email: arturo.laurenzi@iit.it, luca.muratore@iit.it + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see +*/ + +#ifndef __ESTIMATION_UTILS_UTILS__ +#define __ESTIMATION_UTILS_UTILS__ + +#include + +namespace estimation_utils { namespace utils { + +template +/** + * @brief SecondOrderFilter implements a canonical continuous-time + * second order filter with transfer function + * 1 + * P(s) = -----------------------, w = natural frequency, eps = damping ratio + * (s/w)^2 + 2*eps/w*s + 1 + * + * and discretized according to a trapezoidal (aka Tustin) scheme. This yields + * a difference equation of the following form: + * + * a0*y + a1*yd + a2*ydd = u + b1*ud + b2*udd + * + * where yd = y(k-1), ydd = y(k-2) and so on (d = delayed). + */ +class SecondOrderFilter { + +public: + + typedef std::shared_ptr> Ptr; + + SecondOrderFilter(): + _omega(1.0), + _eps(0.8), + _ts(0.01), + _reset_has_been_called(false) + { + computeCoeff(); + } + + SecondOrderFilter(double omega, double eps, double ts, const SignalType& initial_state): + _omega(omega), + _eps(eps), + _ts(ts), + _reset_has_been_called(false) + { + computeCoeff(); + reset(initial_state); + } + + void reset(const SignalType& initial_state){ + _reset_has_been_called = true; + _u = initial_state; + _y = initial_state; + _yd = initial_state; + _ydd = initial_state; + _udd = initial_state; + _ud = initial_state; + } + + const SignalType& process(const SignalType& input){ + + if(!_reset_has_been_called) reset(input*0); + + + _ydd = _yd; + _yd = _y; + _udd = _ud; + _ud = _u; + + + _u = input; + _y = 1.0/_a0 * ( _u + _b1*_ud + _b2*_udd - _a1*_yd - _a2*_ydd ); + + return _y; + } + + const SignalType& getOutput() const { + return _y; + } + + void setOmega(double omega){ + _omega = omega; + computeCoeff(); + } + + double getOmega() + { + return _omega; + } + + void setDamping(double eps){ + _eps = eps; + computeCoeff(); + } + + double getDamping() + { + return _eps; + } + + void setTimeStep(double ts){ + _ts = ts; + computeCoeff(); + } + + double getTimeStep() + { + return _ts; + } + +private: + + void computeCoeff() + { + _b1 = 2.0; + _b2 = 1.0; + + _a0 = 1.0 + 4.0*_eps/(_omega*_ts) + 4.0/std::pow(_omega*_ts, 2.0); + _a1 = 2 - 8.0/std::pow(_omega*_ts, 2.0); + _a2 = 1.0 + 4.0/std::pow(_omega*_ts, 2.0) - 4.0*_eps/(_omega*_ts); + + } + + double _omega; + double _eps; + double _ts; + + double _b1, _b2; + double _a0, _a1, _a2; + + bool _reset_has_been_called; + + SignalType _y, _yd, _ydd, _u, _ud, _udd; + +}; + +template +class FilterWrap { + +public: + + typedef std::shared_ptr> Ptr; + typedef std::unique_ptr> UniquePtr; + + FilterWrap(const double& DAMPING_FACT, const double& BW_MEDIUM, const double& periodSec, const unsigned& dim) : + _dim(dim) + { + + double omega = 2.0 * M_PI * BW_MEDIUM; + + _filter.setDamping ( DAMPING_FACT ); + _filter.setTimeStep ( periodSec ); + _filter.setOmega ( omega ); + + reset(); + + }; + + void reset(const double& DAMPING_FACT, const double& BW_MEDIUM) { + + double omega = 2.0 * M_PI * BW_MEDIUM; + + _filter.setDamping ( DAMPING_FACT ); + _filter.setOmega ( omega ); + + reset(); + } + + void reset() { + Eigen::VectorXd resetVect = Eigen::VectorXd::Zero(_dim); + _filter.reset (resetVect); + } + + const SignalType& process(const SignalType& input) { + return (_filter.process(input)); + } + +private: + SecondOrderFilter _filter; + const unsigned int _dim; +}; + +}} + +#endif // __ESTIMATION_UTILS_UTILS__ diff --git a/launch/force_estimation_centauro.launch b/launch/force_estimation_centauro.launch index a7ccafc..b9a847d 100644 --- a/launch/force_estimation_centauro.launch +++ b/launch/force_estimation_centauro.launch @@ -16,12 +16,12 @@ - - + + diff --git a/launch/force_estimation_relax.launch b/launch/force_estimation_relax.launch index 4546d53..d5524b6 100644 --- a/launch/force_estimation_relax.launch +++ b/launch/force_estimation_relax.launch @@ -17,7 +17,7 @@ diff --git a/src/force_estimation/ForceEstimation.cpp b/src/force_estimation/ForceEstimation.cpp index e509fea..c59d3a9 100644 --- a/src/force_estimation/ForceEstimation.cpp +++ b/src/force_estimation/ForceEstimation.cpp @@ -185,10 +185,28 @@ void ForceEstimation::update() if (_reset_offset_i >= _reset_offset_N) { _reset_offset_running = false; + if (_filter_in_use) { + t.filter->reset(); + } } } - t.sensor->setWrench(wrench - t.wrench_offset, 0.0); + + wrench = wrench - t.wrench_offset; + + if (_filter_in_use) { + if (_filter_dead_zone > 0){ + for (int i=0; iprocess(wrench); + } + + t.sensor->setWrench(wrench, 0.0); } } @@ -213,6 +231,21 @@ void ForceEstimation::resetOffset(double sec) { } } +bool ForceEstimation::initFilter(const double& damping, const double& bw, const double& dead_zone) { + + _filter_damping = damping; + _filter_bw = bw; + _filter_dead_zone = dead_zone; + _filter_in_use = true; + + for(TaskInfo& t : _tasks) + { + t.filter = std::make_shared>( + _filter_damping, _filter_bw, 1.0/_rate, 6); + } + + return true; +} void estimation_utils::ForceEstimation::log(XBot::MatLogger2::Ptr logger) const { diff --git a/src/force_estimation/ForceEstimationHandler.cpp b/src/force_estimation/ForceEstimationHandler.cpp deleted file mode 100644 index 18f2327..0000000 --- a/src/force_estimation/ForceEstimationHandler.cpp +++ /dev/null @@ -1,232 +0,0 @@ -#include - -using tpo::ForceSensorHandler; - -ForceSensorHandler::ForceSensorHandler() {} - - -bool tpo::ForceSensorHandler::init(const std::unique_ptr& nh, std::string group_name, std::string sensor_link, - std::string ref_link, bool filter, double force_dead_zone_limit, - double period_sec, bool pub_arrows) -{ - this->group_name = group_name; - this->sensor_link = sensor_link; - this->ref_link = ref_link; - - this->period_sec = period_sec; - this->filter = filter; - this->pub_arrows = pub_arrows; - - this-> force_dead_zone_limit = force_dead_zone_limit; //lower than this sensed forces are ignored - - std::string topic_name = "/force_estimator/force_estimation/" + sensor_link; - - sensed_wrench_sub = nh->subscribe(topic_name, 1, &ForceSensorHandler::sensedWrenchSubCallback, this); - sensed_wrench_raw_msg = geometry_msgs::WrenchStamped(); - sensed_wrench_new_data= false; - sensed_wrench= Eigen::Matrix::Zero(); - - tf_listener = std::make_unique(tf_buffer); - - if (filter) { - initSensedForceFilter(); - sensed_force_filtered_pub = nh->advertise("/force_estimation/force_filtered/" + sensor_link, 1); - } - - if (pub_arrows) { - sensed_force_marker_pub = nh->advertise("/force_estimation/force_marker/" + sensor_link, 1); - } - - return true; -} - -bool tpo::ForceSensorHandler::reset() -{ - - Eigen::VectorXd resetVect = Eigen::Matrix::Zero(); - - // reset filter - filter_sensed_force.reset(resetVect); - - sensed_wrench_raw_msg = geometry_msgs::WrenchStamped(); - sensed_wrench = Eigen::Matrix::Zero(); - sensed_wrench_filtered = Eigen::Matrix::Zero(); - sensed_wrench_new_data = false; - - return true; - -} - -void tpo::ForceSensorHandler::publishArrows() { - - visualization_msgs::MarkerArray markers; - - geometry_msgs::TransformStamped transformStamped; - try{ - transformStamped = tf_buffer.lookupTransform(sensor_link, ref_link, //first is target, second is source - ros::Time(0)); //latest available transform - } - catch (tf2::TransformException &ex) { - - ROS_WARN("FORCE VISUALIZER: %s",ex.what()); - //ros::Duration(1.0).sleep(); - return; - } - - Eigen::Quaterniond quat( - transformStamped.transform.rotation.w, - transformStamped.transform.rotation.x, - transformStamped.transform.rotation.y, - transformStamped.transform.rotation.z); - - visualization_msgs::Marker marker; - marker.header.frame_id = sensor_link; - marker.id = 0; - marker.header.stamp = ros::Time::now(); - marker.scale.x = 0.02; //shaft diameter - marker.scale.y = 0.04; //head diameter - marker.scale.z = 0.05; //head length auto computed - marker.pose.orientation.w = 1; - marker.frame_locked = true; - marker.points.push_back(geometry_msgs::Point()); - marker.points.push_back(geometry_msgs::Point()); - - if (!filter) { - - const auto & it = sensed_wrench; - - marker.ns = "raw"; - - marker.color.a = 0.8; - marker.color.r = 0.5; - marker.color.g = 0.5; - marker.color.b = 0.5; - - Eigen::Vector3d vect; - geometry_msgs::Point point1; - - vect = quat * it.head<3>(); - if (vect.norm() > 0.5) { - vect *= 0.5 / vect.norm() ; - } - point1.x = vect(0); - point1.y = vect(1); - point1.z = vect(2); - marker.points.at(1) = point1; - - } else { - - const auto & it = sensed_wrench_filtered; - - marker.ns = "filtered"; - - marker.color.a = 0.8; - marker.color.r = 0.2; - marker.color.g = 1.0; - marker.color.b = 0.9; - - Eigen::Vector3d vect; - geometry_msgs::Point point1; - - vect = quat * it.head<3>(); - if (vect.norm() > 0.5) { - vect *= 0.5 / vect.norm() ; - } - point1.x = vect(0); - point1.y = vect(1); - point1.z = vect(2); - marker.points.at(1) = point1; - } - - markers.markers.push_back(marker); - - sensed_force_marker_pub.publish(markers); -} - - -void tpo::ForceSensorHandler::sensedWrenchSubCallback(const geometry_msgs::WrenchStamped::ConstPtr& msg) { - - const std::lock_guard lock(sensed_wrench_mutex); - - sensed_wrench_raw_msg = *msg; - sensed_wrench_new_data = true; -} - -void tpo::ForceSensorHandler::update() { - - const auto &it = sensed_wrench_raw_msg; - - if (sensed_wrench_new_data) { - - const std::lock_guard lock(sensed_wrench_mutex); - - geometry_msgs::WrenchStamped msg_transf; - - try { - tf_buffer.transform(it, msg_transf, ref_link); - } - catch (tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); - return; - } - - //DEBUG PRINTS - //std::cout << sensor_link << " : " << it.wrench.force.x << " " << it.wrench.force.y << " " << it.wrench.force.z << std::endl; - //std::cout << "after tf trasf: : " << msg_transf.wrench.force.x << " " << msg_transf.wrench.force.y << " " << msg_transf.wrench.force.z << std::endl; - - tf::wrenchMsgToEigen(msg_transf.wrench, sensed_wrench); - //std::cout << "after eigen trasf: : " << sensed_wrench.transpose() << std::endl; - - sensed_wrench_new_data = false; - } - - //even if not data, we must filtered because filter should be run at each loop rate given...? - if (filter) { - filterSensedForce(); - geometry_msgs::WrenchStamped msg; - tf::wrenchEigenToMsg(sensed_wrench_filtered, msg.wrench); - - msg.header.frame_id = ref_link; - msg.header.stamp = it.header.stamp; - - sensed_force_filtered_pub.publish(msg); - } - if (pub_arrows) { - publishArrows(); - } -} - -void tpo::ForceSensorHandler::filterSensedForce() { - - sensed_wrench_filtered = sensed_wrench; - - tpo::utils::cutOff(sensed_wrench_filtered, force_dead_zone_limit); - - sensed_wrench_filtered= filter_sensed_force.process(sensed_wrench_filtered); - -} - -bool tpo::ForceSensorHandler::initSensedForceFilter() { - - const double DAMPING_FACT = 0.8; - const double BW_MEDIUM = 3; - double omega = 2.0 * M_PI * BW_MEDIUM; - - Eigen::VectorXd resetVect = Eigen::Matrix::Zero(); - - filter_sensed_force = tpo::utils::SecondOrderFilter>(); - filter_sensed_force.setDamping ( DAMPING_FACT ); - filter_sensed_force.setTimeStep ( period_sec ); - filter_sensed_force.setOmega ( omega ); - - // reset filter - filter_sensed_force.reset(resetVect); - - sensed_wrench_filtered = Eigen::Matrix::Zero(); - - return true; -} - -void tpo::ForceSensorHandler::getSensedWrench(Eigen::Matrix & w) const { w = sensed_wrench; } -void tpo::ForceSensorHandler::getFilteredSensedWrench(Eigen::Matrix & w) const { w = sensed_wrench_filtered; } -std::string tpo::ForceSensorHandler::getSensorLink() const { return sensor_link; } diff --git a/src/nodes/CMakeLists.txt b/src/nodes/CMakeLists.txt index 3b80aeb..6a496b3 100644 --- a/src/nodes/CMakeLists.txt +++ b/src/nodes/CMakeLists.txt @@ -15,17 +15,26 @@ target_link_libraries(payload_estimation_node ${eigen_conversions_LIBRARIES} ) -add_executable(force_estimation_node - force_estimation_node.cpp) - - +add_library(force_estimation_node + ForceEstimationNode.cpp +) target_link_libraries(force_estimation_node force_estimation ${roscpp_LIBRARIES} + ${eigen_conversions_LIBRARIES} +) + +add_executable(force_estimation_node_executable + force_estimation_node.cpp) + +target_link_libraries(force_estimation_node_executable + force_estimation_node + ${roscpp_LIBRARIES} ) install(TARGETS - force_estimation_node + force_estimation_node_executable + force_estimation_node payload_estimation_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/src/nodes/ForceEstimationNode.cpp b/src/nodes/ForceEstimationNode.cpp new file mode 100644 index 0000000..9fd601f --- /dev/null +++ b/src/nodes/ForceEstimationNode.cpp @@ -0,0 +1,294 @@ +#include + +using estimation_utils::ForceEstimationNode; + +ForceEstimationNode::ForceEstimationNode(ros::NodeHandle* nh, const double& rate): _nh(nh), _rate(rate) {} + + +bool ForceEstimationNode::init() +{ + + //lets wait for xbot + if (!waitForXbotCore(_nh)) { + + ROS_ERROR_STREAM("WaitForXbotCore failed, exiting.."); + return -1; + } + + // get robot, model, and one imu + _robot = XBot::RobotInterface::getRobot(XBot::ConfigOptionsFromParamServer(*_nh)); + _model = XBot::ModelInterface::getModel(XBot::ConfigOptionsFromParamServer(*_nh)); + + + if(_robot->getImu().size() > 0) + { + _imu = _robot->getImu().begin()->second; + } + else + { + ROS_INFO("IMU not found: map is empty\n"); + } + + // configure node + auto links = _nh->param("links", std::vector()); + + if(links.size() == 0) + { + ROS_INFO("Private parameter ~/links is empty, exiting.."); + return 1; + } + + auto chains = _nh->param("chains", std::vector()); + if(chains.size() == 0) + { + ROS_INFO("Private parameter ~/chains is empty, will use all torque sensors"); + } + + double svd_th = _nh->param("svd_threshold", (double)ForceEstimation::DEFAULT_SVD_THRESHOLD); + + // get torque offset map + auto tau_off_map = _nh->param("torque_offset", std::map()); + XBot::JointNameMap tau_off_map_xbot(tau_off_map.begin(), tau_off_map.end()); + _tau_offset.setZero(_model->getJointNum()); + _model->mapToEigen(tau_off_map_xbot, _tau_offset); + + // ros service to request for zeroing the force estimation offset + _request_wrench_zero_offset = _nh->advertiseService("wrench_zero_offset", &ForceEstimationNode::wrenchZeroOffsetClbk, this); + _reset_time_sec = _nh->param("sec_to_reset", 3); + + // construct force estimation class + if(_nh->param("use_momentum_based", false)) + { + ROS_INFO("using momentum based estimation"); + _f_est_ptr = std::make_shared(_model, _rate, svd_th); + } + else + { + _f_est_ptr = std::make_shared(_model, _rate, svd_th); + } + + + // set ignored joints + auto ignored_joints = _nh->param("ignored_joints", std::vector()); + for(auto jname : ignored_joints) + { + _f_est_ptr->setIgnoredJoint(jname); + } + + // generate virtual fts + for(const auto& l : links) + { + auto dofs = _nh->param(l + "/dofs", std::vector()); + + auto pub = _nh->advertise("force_estimation/" + l, 1); + + _ft_map[l] = _f_est_ptr->add_link(l, dofs, chains); + _ft_pub_map[l] = pub; + } + + // log + if(_nh->param("enable_log", false)) + { + _logger = XBot::MatLogger2::MakeLogger("/tmp/force_estimation_log"); + } + + _ref_frame = _nh->param("ref_frame", std::string()); + + //filter + if(_nh->param("enable_filter", false)) { + double filter_damp = _nh->param("filter_damp", 0.8); + double filter_bw = _nh->param("filter_bw", 3); + double filter_dead_zone = _nh->param("filter_dead_zone", 0); //lower than this forces are ignored + _f_est_ptr->initFilter(filter_damp, filter_bw, filter_dead_zone); + } + + //markers + _pub_markers = _nh->param("pub_arrows", false); + if (_pub_markers) { + + _arrows_pub = _nh->advertise("force_estimation/force_markers/", 1); + + _marker.id = 0; + _marker.scale.x = 0.02; //shaft diameter + _marker.scale.y = 0.04; //head diameter + _marker.scale.z = 0.05; //head length auto computed + _marker.color.a = 0.8; + _marker.color.r = 0.5; + _marker.color.g = 0.5; + _marker.color.b = 0.5; + _marker.pose.orientation.w = 1; + _marker.frame_locked = true; + _marker.points.resize(2); + _marker.points.at(0) = geometry_msgs::Point(); + + } + + return true; +} + + +bool estimation_utils::ForceEstimationNode::run() { + + // update model from robot, set imu + _robot->sense(false); + _model->syncFrom(*_robot, XBot::Sync::All, XBot::Sync::MotorSide); + if(_model->isFloatingBase() && _imu) + { + _model->setFloatingBaseState(_imu); + _model->update(); + } + _model->update(); + _model->getJointEffort(_tau); + _tau += _tau_offset; + _model->setJointEffort(_tau); + + //reset offset stuff + if (_reset_requested) { + + _f_est_ptr->resetOffset(_reset_time_sec); + _reset_requested = false; + } + + // update force estimation + _f_est_ptr->update(); + + // publish to topics + + for(const auto& ft : _ft_map) + { + Eigen::Vector6d wrench; + ft.second->getWrench(wrench); + + if (_ref_frame.size()>0) { + + Eigen::Matrix3d sensor_R_ref; + //source target + if (! _model->getOrientation(ft.second->getSensorName(), _ref_frame, sensor_R_ref)) { + return false; + } + + wrench.head<3>() = sensor_R_ref * wrench.head<3>(); + wrench.tail<3>() = sensor_R_ref * wrench.tail<3>(); + + _wrench_msg.header.frame_id = _ref_frame; + + } else { + _wrench_msg.header.frame_id = ft.second->getSensorName(); + + } + + tf::wrenchEigenToMsg(wrench, _wrench_msg.wrench); + _wrench_msg.header.stamp = ros::Time::now(); + _ft_pub_map.at(ft.first).publish(_wrench_msg); + } + + if (_pub_markers) { + publishArrows(); + } + + // log + if(_logger) + { + _f_est_ptr->log(_logger); + } + + return true; + +} + +bool ForceEstimationNode::wrenchZeroOffsetClbk(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + _reset_requested = true; + return true; +} + +bool ForceEstimationNode::waitForXbotCore(ros::NodeHandle* nh, ros::Duration timeout) { + + //ros::Time startTime = ros::Time::now(); + ros::ServiceClient client = nh->serviceClient("/xbotcore/ros_io/state"); + + auto wait_lambda = [&](){ + + ROS_INFO_STREAM("Waiting for XbotCore..."); + if (!client.waitForExistence(timeout)) { + return false; + } + + return true; + }; + + if (! wait_lambda()) { + return false; + } + + xbot_msgs::PluginStatus status; + ros::Rate r(10); + ros::Duration(1).sleep(); //necessary because idk but service is not really ready and the client.call hands 4ever + while(status.response.status.compare("Running") != 0) { + + if (!client.call(status)) { +// ROS_INFO_STREAM("Call for XbotCore fail, I will retry..."); +// if (! wait_lambda()) { +// return false; +// } + } + r.sleep(); + } + + return true; + +} + + +void ForceEstimationNode::publishArrows() { + + visualization_msgs::MarkerArray markers; + + for (const auto& ft : _ft_map) { + + // geometry_msgs::TransformStamped transformStamped; + // try{ + // transformStamped = tf_buffer.lookupTransform(sensor_link, ref_frame, //first is target, second is source + // ros::Time(0)); //latest available transform + // } + // catch (tf2::TransformException &ex) { + + // ROS_WARN("FORCE VISUALIZER: %s",ex.what()); + // //ros::Duration(1.0).sleep(); + // return; + // } + + // Eigen::Quaterniond quat( + // transformStamped.transform.rotation.w, + // transformStamped.transform.rotation.x, + // transformStamped.transform.rotation.y, + // transformStamped.transform.rotation.z); + + + _marker.header.frame_id = ft.second->getSensorName(); + _marker.header.stamp = ros::Time::now(); + _marker.ns = _marker.header.frame_id; + + Eigen::Vector6d wrench; + ft.second->getWrench(wrench); + + //Eigen::Vector3d vect = quat * wrench.head<3>(); + Eigen::Vector3d vect = wrench.head<3>(); + + //saturate arrow + if (vect.norm() > 0.5) { + vect *= 0.5 / vect.norm() ; + } + + geometry_msgs::Point point1; + point1.x = vect(0); + point1.y = vect(1); + point1.z = vect(2); + _marker.points.at(1) = point1; + + markers.markers.push_back(_marker); + } + + _arrows_pub.publish(markers); +} \ No newline at end of file diff --git a/src/nodes/force_estimation_node.cpp b/src/nodes/force_estimation_node.cpp index a8f74f1..7d2183c 100644 --- a/src/nodes/force_estimation_node.cpp +++ b/src/nodes/force_estimation_node.cpp @@ -1,67 +1,7 @@ -#include -#include -#include - -#include -#include - -#include -#include - -#include - -//for waitForXbotCore method -#include - +#include //https://github.com/ADVRHumanoids/CartesianInterface/blob/2.0-devel/src/ros/ForceEstimationNode.cpp -bool reset_requested = false; - -bool wrenchZeroOffsetClbk(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res) -{ - reset_requested = true; - - return true; -} - -bool waitForXbotCore(ros::NodeHandle* nh, ros::Duration timeout = ros::Duration(-1)) { - - //ros::Time startTime = ros::Time::now(); - ros::ServiceClient client = nh->serviceClient("/xbotcore/ros_io/state"); - - auto wait_lambda = [&](){ - - ROS_INFO_STREAM("Waiting for XbotCore..."); - if (!client.waitForExistence(timeout)) { - return false; - } - - return true; - }; - - if (! wait_lambda()) { - return false; - } - - xbot_msgs::PluginStatus status; - ros::Rate r(10); - ros::Duration(1).sleep(); //necessary because idk but service is not really ready and the client.call hands 4ever - while(status.response.status.compare("Running") != 0) { - - if (!client.call(status)) { -// ROS_INFO_STREAM("Call for XbotCore fail, I will retry..."); -// if (! wait_lambda()) { -// return false; -// } - } - r.sleep(); - } - - return true; - -} int main(int argc, char ** argv) { @@ -69,153 +9,21 @@ int main(int argc, char ** argv) // init ros, get handles ros::init(argc, argv, "force_estimation_node"); ros::NodeHandle nh_priv("~"); - - //lets wait for xbot - if (!waitForXbotCore(&nh_priv)) { - - ROS_ERROR_STREAM("WaitForXbotCore failed, exiting.."); - return -1; - } - - - // get robot, model, and one imu - auto robot = XBot::RobotInterface::getRobot(XBot::ConfigOptionsFromParamServer(nh_priv)); - auto model = XBot::ModelInterface::getModel(XBot::ConfigOptionsFromParamServer(nh_priv)); - XBot::ImuSensor::ConstPtr imu; - if(robot->getImu().size() > 0) - { - imu = robot->getImu().begin()->second; - } - else - { - ROS_INFO("IMU not found: map is empty\n"); - } - - // configure node double rate = nh_priv.param("rate", 100.0); - auto links = nh_priv.param("links", std::vector()); - - if(links.size() == 0) - { - ROS_INFO("Private parameter ~/links is empty, exiting.."); - return 1; - } - - auto chains = nh_priv.param("chains", std::vector()); - if(links.size() == 0) - { - ROS_INFO("Private parameter ~/chains is empty, will use all torque sensors"); - } - - double svd_th = nh_priv.param("svd_threshold", (double)estimation_utils::ForceEstimation::DEFAULT_SVD_THRESHOLD); - - // get torque offset map - auto tau_off_map = nh_priv.param("torque_offset", std::map()); - XBot::JointNameMap tau_off_map_xbot(tau_off_map.begin(), tau_off_map.end()); - Eigen::VectorXd tau_offset; - tau_offset.setZero(model->getJointNum()); - model->mapToEigen(tau_off_map_xbot, tau_offset); - - ///// ros service to request for zeroing the force estimation offset - ros::ServiceServer request_wrench_zero_offset; - request_wrench_zero_offset = nh_priv.advertiseService("wrench_zero_offset", wrenchZeroOffsetClbk); - double reset_time_sec = nh_priv.param("sec_to_reset", 3);; - - // construct force estimation class - std::shared_ptr f_est_ptr; + estimation_utils::ForceEstimationNode force_estimation_node(&nh_priv, rate); + if (! force_estimation_node.init()) { - if(nh_priv.param("use_momentum_based", false)) - { - ROS_INFO("using momentum based estimation"); - f_est_ptr = std::make_shared(model, rate, svd_th); - } - else - { - f_est_ptr = std::make_shared(model, rate, svd_th); - } - - estimation_utils::ForceEstimation& f_est = *f_est_ptr; - - // set ignored joints - auto ignored_joints = nh_priv.param("ignored_joints", std::vector()); - for(auto jname : ignored_joints) - { - //f_est.setIgnoredJoint(jname); - } - - // generate virtual fts - std::map ft_map; - - for(auto l : links) - { - auto dofs = nh_priv.param(l + "/dofs", std::vector()); - - auto pub = nh_priv.advertise("force_estimation/" + l, 1); - - ft_map[f_est.add_link(l, dofs, chains)] = pub; - } - - // log - XBot::MatLogger2::Ptr logger; - if(nh_priv.param("enable_log", false)) - { - logger = XBot::MatLogger2::MakeLogger("/tmp/force_estimation_log"); + ROS_ERROR_STREAM("Init failed"); + return -1; } - - // start looping - Eigen::VectorXd tau; ros::Rate loop_rate(rate); - Eigen::Vector6d wrench; while(ros::ok()) { - // update model from robot, set imu - robot->sense(false); - model->syncFrom(*robot, XBot::Sync::All, XBot::Sync::MotorSide); - if(model->isFloatingBase() && imu) - { - model->setFloatingBaseState(imu); - model->update(); - } - model->update(); - model->getJointEffort(tau); - tau += tau_offset; - model->setJointEffort(tau); - - //reset offset stuff - if (reset_requested) { - - f_est.resetOffset(reset_time_sec); - reset_requested = false; - } - - // update force estimation - f_est.update(); - - // publish to topics - - for(const auto& ft_pub : ft_map) - { - geometry_msgs::WrenchStamped msg; - ft_pub.first->getWrench(wrench); - - tf::wrenchEigenToMsg(wrench, msg.wrench); - - msg.header.frame_id = ft_pub.first->getSensorName(); - msg.header.stamp = ros::Time::now(); - - ft_pub.second.publish(msg); - } - - // log - if(logger) - { - f_est.log(logger); - } - + force_estimation_node.run(); ros::spinOnce(); // sync with desired loop freq From 53210ce954aff73f64396a97e405995678f4560a Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Wed, 31 Jan 2024 12:23:06 +0100 Subject: [PATCH 08/14] more conf for relax --- config/force_estimation_node_relax.yaml | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/config/force_estimation_node_relax.yaml b/config/force_estimation_node_relax.yaml index 91f22f3..b9ccfc2 100644 --- a/config/force_estimation_node_relax.yaml +++ b/config/force_estimation_node_relax.yaml @@ -6,4 +6,19 @@ force_estimator: links: [relax_arm1_linkEndEffector] chains: [arm] relax_arm1_linkEndEffector: - dofs: [0, 1, 2] \ No newline at end of file + 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 \ No newline at end of file From 44a7b7f595a87e857b3c852c322f37d6ef046602 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Mon, 26 Feb 2024 17:06:01 +0100 Subject: [PATCH 09/14] fix --- config/force_estimation_node_centauro.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/force_estimation_node_centauro.yaml b/config/force_estimation_node_centauro.yaml index 27beaa5..3c68024 100644 --- a/config/force_estimation_node_centauro.yaml +++ b/config/force_estimation_node_centauro.yaml @@ -5,9 +5,9 @@ force_estimator: model_type: RBDL links: [arm1_6, arm2_6] chains: [left_arm, right_arm] - arm1_8: + arm1_6: dofs: [0, 1, 2] - arm2_8: + arm2_6: dofs: [0, 1, 2] ignored_joints: [] svd_threshold: 0.05 From f8296efe669a0dc5c99de13ee46ba58b8cb65d11 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Fri, 1 Mar 2024 12:34:51 +0100 Subject: [PATCH 10/14] arrows scaling --- config/force_estimation_node_centauro.yaml | 5 +++++ config/force_estimation_node_relax.yaml | 4 +++- include/estimation_utils/nodes/ForceEstimationNode.h | 1 + src/nodes/ForceEstimationNode.cpp | 9 +++++++-- 4 files changed, 16 insertions(+), 3 deletions(-) diff --git a/config/force_estimation_node_centauro.yaml b/config/force_estimation_node_centauro.yaml index 27beaa5..5ff1364 100644 --- a/config/force_estimation_node_centauro.yaml +++ b/config/force_estimation_node_centauro.yaml @@ -24,4 +24,9 @@ force_estimator: 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 \ No newline at end of file diff --git a/config/force_estimation_node_relax.yaml b/config/force_estimation_node_relax.yaml index b9ccfc2..6e91943 100644 --- a/config/force_estimation_node_relax.yaml +++ b/config/force_estimation_node_relax.yaml @@ -21,4 +21,6 @@ force_estimator: filter_bw: 3 filter_dead_zone: 0 #markers - pub_arrows: true \ No newline at end of file + pub_arrows: true + arrow_scale_factor: 30 # The number will DIVIDE each component, so bigger == shorter arrows + arrow_max_norm: 3 \ No newline at end of file diff --git a/include/estimation_utils/nodes/ForceEstimationNode.h b/include/estimation_utils/nodes/ForceEstimationNode.h index fdd6f31..702047e 100644 --- a/include/estimation_utils/nodes/ForceEstimationNode.h +++ b/include/estimation_utils/nodes/ForceEstimationNode.h @@ -54,6 +54,7 @@ class ForceEstimationNode { //markers pub bool _pub_markers; + double _arrow_scale_factor, _arrow_max_norm; ros::Publisher _arrows_pub; visualization_msgs::Marker _marker; diff --git a/src/nodes/ForceEstimationNode.cpp b/src/nodes/ForceEstimationNode.cpp index 9fd601f..1388ebe 100644 --- a/src/nodes/ForceEstimationNode.cpp +++ b/src/nodes/ForceEstimationNode.cpp @@ -121,6 +121,9 @@ bool ForceEstimationNode::init() _marker.points.resize(2); _marker.points.at(0) = geometry_msgs::Point(); + _arrow_scale_factor = _nh->param("arrow_scale_factor", 1); + _arrow_max_norm = _nh->param("arrow_max_norm", 5); + } return true; @@ -276,9 +279,11 @@ void ForceEstimationNode::publishArrows() { //Eigen::Vector3d vect = quat * wrench.head<3>(); Eigen::Vector3d vect = wrench.head<3>(); + vect /= _arrow_scale_factor; + //saturate arrow - if (vect.norm() > 0.5) { - vect *= 0.5 / vect.norm() ; + if (vect.norm() > _arrow_max_norm) { + vect *= _arrow_max_norm / vect.norm() ; } geometry_msgs::Point point1; From de993cf4a1d13733973c8c1fcdb3143276e12588 Mon Sep 17 00:00:00 2001 From: Davide Torielli Date: Tue, 19 Mar 2024 16:24:22 +0100 Subject: [PATCH 11/14] gdb option --- launch/force_estimation_centauro.launch | 9 ++++++++- launch/force_estimation_relax.launch | 9 ++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/launch/force_estimation_centauro.launch b/launch/force_estimation_centauro.launch index b9a847d..8024148 100644 --- a/launch/force_estimation_centauro.launch +++ b/launch/force_estimation_centauro.launch @@ -3,6 +3,11 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/force_estimation_inail2arm.launch b/launch/force_estimation_inail2arm.launch new file mode 100644 index 0000000..f585176 --- /dev/null +++ b/launch/force_estimation_inail2arm.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + From 917b3dcf6148924afa7fe315b0569b9a88c33fb8 Mon Sep 17 00:00:00 2001 From: damigas Date: Wed, 8 May 2024 14:19:57 +0200 Subject: [PATCH 14/14] Fixing config inail --- config/force_estimation_node_inail2arm.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/force_estimation_node_inail2arm.yaml b/config/force_estimation_node_inail2arm.yaml index f249c3f..c486500 100644 --- a/config/force_estimation_node_inail2arm.yaml +++ b/config/force_estimation_node_inail2arm.yaml @@ -7,7 +7,7 @@ force_estimator: chains: [arm] dagana_1_tcp: dofs: [0, 1, 2, 3, 4, 5] - ignored_joints: [dagana_1_tcp] + ignored_joints: [dagana_1_claw_joint] svd_threshold: 0.05 #reset offset sec_to_reset: 3