diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp index 5dbd4f15..adfa8f00 100644 --- a/include/control_filters/gravity_compensation.hpp +++ b/include/control_filters/gravity_compensation.hpp @@ -29,7 +29,6 @@ namespace control_filters { - template class GravityCompensation : public filters::FilterBase { diff --git a/src/control_filters/README.md b/src/control_filters/README.md index fa304cb6..02cb7c6e 100644 --- a/src/control_filters/README.md +++ b/src/control_filters/README.md @@ -1,37 +1,35 @@ -# Control Filters +# Control filters ## Available filters * Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench). -* +## Gravity compensation filter -## 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. - 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. + The filter relies on tf2, 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 +### 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 +### 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: +* `data_in`, a wrench ℱi = {fi, τi} represented in the `data_in` frame ℛi +* access to tf2 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: +Compute `data_out` compensated wrench ℱco = {fco, τco} represented in the `data_out` frame ℛo if given, or the `data_in` frame ℛi otherwise, with equations: ℱco = Tos.ℱcs, @@ -40,20 +38,20 @@ with ℱcs = {fcs, τcs} the compensated and, -fcs = fs - Tsw.gw +fcs = fs - Tswgw its force and, -τcs = τs - ps x (Tsw.gw) +τcs = τs - ps x (Tswgw) its torque, and -ℱs = Tsi.ℱi +ℱs = Tsi.ℱi = {fs, τs} 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. +* `data_in` frame is usually equal to `sensor_frame`, but could be different since measurement of wrench might occur in another frame. E.g.: 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 (thickness 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. E.g.: wrench expressed in a `control_frame` like the 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_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml index 95cfe8b0..ebfef4a3 100644 --- a/src/control_filters/gravity_compensation_filter_parameters.yaml +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -1,8 +1,7 @@ gravity_compensation_filter: world_frame: { type: string, - # default_value: world, - description: "fixed world frame in which the constant field-induced force is given", + description: "Fixed world frame in which the constant field-induced force is given.", read_only: true, validation: { not_empty<>: [] @@ -10,8 +9,7 @@ gravity_compensation_filter: } sensor_frame: { type: string, - # default_value: ft_sensor, - description: "Sensor frame in which center of gravity (CoG) is measured and computation occur", + description: "Sensor frame in which center of gravity (CoG) is measured and computation occur.", read_only: true, validation: { not_empty<>: []