From b74a1eeb71ffc6da3501a31219e56598218c7186 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 20 Mar 2023 10:44:24 +0100 Subject: [PATCH] Improved understandability of the filter --- .../control_filters/gravity_compensation.hpp | 2 +- src/control_filters/README.md | 59 +++++++++++++++++++ src/control_filters/gravity_compensation.cpp | 15 ++--- 3 files changed, 68 insertions(+), 8 deletions(-) create mode 100644 src/control_filters/README.md diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index d1db2367..5dbd4f15 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -77,7 +77,7 @@ class GravityCompensation : public filters::FilterBase std::unique_ptr p_tf_Buffer_; std::unique_ptr p_tf_Listener_; geometry_msgs::msg::TransformStamped transform_sensor_datain_, transform_world_dataout_, - transform_data_out_sensor_, rot_sensor_world_; + transform_data_out_sensor_, transform_sensor_world_; }; template diff --git a/src/control_filters/README.md b/src/control_filters/README.md new file mode 100644 index 00000000..fa304cb6 --- /dev/null +++ b/src/control_filters/README.md @@ -0,0 +1,59 @@ +# Control Filters + +## Available filters + +* Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench). +* + + +## Gravity Compensation filter + + This filter implements a gravity compensation algorithm, applied to an `data_in wrench`, computed at a `sensor frame` in which the center of gravity (CoG) of the to-be-compensated mass is known. + + The filter relies on ROS TF, and might fail if transforms are missing. + + Note that, for convenience, the filter can perform additional frame changes if data_out frame id is given. + +### required parameters + +* `world_frame` (ℛw): frame in which the `CoG.force` is represented. +* `sensor_frame` (ℛs): frame in which the `CoG.pos` is defined +* `CoG.pos` (ps): position of the CoG of the mass the filter should compensate for +* `CoG.force` (gw): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the `world_frame` + +### algorithm + +Given +* above-required parameters, ℛw, ℛs, ps, gw +* `data_in`, a wrench ℱi = {fi, τi} represented in `data_in.frame` ℛi +* access to TF homogeneous transforms: + * Tsi from ℛi to ℛs + * Tsw from ℛw to ℛs + * Tos from ℛs to ℛo + +Compute `data_out` compensated wrench ℱco = {fco, τco} represented in `data_out.frame` ℛo if given, or `data_in.frame` ℛi otherwise, with equations: + +ℱco = Tos.ℱcs, + + +with ℱcs = {fcs, τcs} the compensated wrench in `sensor_frame` (common frame for computation) + +and, + +fcs = fs - Tsw.gw + +its force and, + +τcs = τs - ps x (Tsw.gw) + +its torque, and + +ℱs = Tsi.ℱi + +the full transform of the input wrench ℱi to sensor frame ℛs + +Remarks : +* a full vector is used for gravity force, to not impose gravity to be only along z of `world_frame`. +* `data_in.frame` is usually equal to `sensor_frame`, but could be different since measurement of wrech might occur in another frame. Ex: measurements are at the **FT sensor flange** = `data_in.frame`, but CoG is given in **FT sensor base** = `sensor_frame` (=frame to which it is mounted on the robot), introducing an offset (thichkess of the sensor) to be accounted for. +* `data_out.frame` is usually `data_in.frame`, but for convenience, can be set to any other useful frame. Ex: Wrench expressed in a `control_frame` for instance center of a gripper. +* Tsw will only rotate the gw vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from ℛw to ℛs). diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp index 6b03061a..b525cdad 100644 --- a/src/control_filters/gravity_compensation.cpp +++ b/src/control_filters/gravity_compensation.cpp @@ -40,7 +40,7 @@ bool GravityCompensation::update( try { - // transform from data_in frame to sensor_frame (most likely to be identity) + // transform from data_in frame to sensor_frame transform_sensor_datain_ = p_tf_Buffer_->lookupTransform(parameters_.sensor_frame, data_in.header.frame_id, rclcpp::Time()); @@ -55,8 +55,8 @@ bool GravityCompensation::update( } transform_data_out_sensor_ = p_tf_Buffer_->lookupTransform( data_out.header.frame_id, parameters_.sensor_frame, rclcpp::Time()); - // rotation only (because gravity is a field) from world (gravity frame) to sensor frame - rot_sensor_world_ = p_tf_Buffer_->lookupTransform( + // transform from world (gravity) frame to sensor frame + transform_sensor_world_ = p_tf_Buffer_->lookupTransform( parameters_.sensor_frame, parameters_.world_frame, rclcpp::Time()); } catch (const tf2::TransformException & ex) @@ -76,15 +76,16 @@ bool GravityCompensation::update( // CoG is already in sensor_frame - // Transform field force vector to sensor_frame frame + // Rotate (no wrench, just a force) the gravity force to sensor frame geometry_msgs::msg::Vector3Stamped cst_ext_force_transformed; - tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, rot_sensor_world_); + tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, transform_sensor_world_); - // Compensate for gravity force + // Compensate for gravity force in sensor frame wrench_sensor.force.x -= cst_ext_force_transformed.vector.x; wrench_sensor.force.y -= cst_ext_force_transformed.vector.y; wrench_sensor.force.z -= cst_ext_force_transformed.vector.z; - // Compensation values for torque result from cross-product of cog Vector and force + // Compensate for torque produced by offset CoG in sensor frame + // result from cross-product of cog Vector and force tf2::Vector3 cog_vector = {cog_.vector.x, cog_.vector.y, cog_.vector.z}; auto added_torque = cog_vector.cross({cst_ext_force_transformed.vector.x, cst_ext_force_transformed.vector.y,