Add gravity compensation filter #153

guihomework · 2023-03-07T21:16:14Z

This is a rework of PR #115 for the gravity-compensation filter part, now using generate_parameter_library.

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

…ptimize code.

fixed gravity compensation calculated values fixed guard condition issues (rclcpp init must be called before) fixed configure issues (low-level must be configured to avoid undefined logger)

guihomework · 2023-03-18T17:30:56Z

The new changes to this PR stem from using the filter in admittance control and realizing mistakes.

One major mistake was the switch to wrench transforms (as suggested in a TODO in previous version of this code), because it (correctly) applies an additional torque, which should not happen due to gravity being a field and not a force exerted at a certain point than generates a torque when moved. So one should not transform the wrench to base or what ever other computation frame, just rotate it, and remove the gravity force on the correct axes.

Additionally, since there are lookupTransforms done, I offered the possibility to have a different output frame than the input frame. A transform back was pre-existing anyway, so might as well be one that is useful if desired.

I refactored variable names and comments to better explain what happens, and simplified the frames (no need to compute in world frame, sensor_frame=measurement frame is completely fine in my opinion).
I generalized the force field to a vector, to permit having a tilted world in which gravity is not along -z.

I would still like to add a test using a param.yaml + maybe some transforms to fully evaluate the filter in real conditions. The current tests were not sufficient to spot the wrench "transform" errors, because they set up identity transforms which are a special case.

destogl

The calculations look too complex to me – at least to the older version as I can remember. Is everything necessary?

Also, tests should not declare parameters if possible, but generate_parameter_library should take care of it

destogl · 2023-03-19T20:55:45Z

README.md

Those changes should be removed.

destogl · 2023-03-19T20:56:10Z

include/control_filters/gravity_compensation.hpp

+
+
+


Suggested change

destogl · 2023-03-19T20:58:11Z

include/control_filters/gravity_compensation.hpp

+bool GravityCompensation<T>::configure()
+{
+  clock_ = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_));


would it be possible to get somehow clock from the node? This could lead to wrong definition of clocks and thus not getting correct transformations.

Unless the filter_base is changed, I don't think the filter has any access to the node (only its logger and parameter interfaces). Maybe the filter should get a time and dt passed when updated (as controllers do)

maybe for a follow-up PR, you could augment the filterbase to also store a clock interface

destogl · 2023-03-19T21:01:31Z

src/control_filters/gravity_compensation.cpp

+    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(


Are you sure this is correct?

for the rotation, well, if I use the full transform, the -mg force along z world gets transformed from the world to sensor frame, generating a force matching -mg in the sensor frame, but also the cross product of the relative distance between sensor and world with the -mg, creating a torque. Gravity is not creating such a torque at the CoG. The only cross product is the distance of CoG to center of measurement with the simple gravity force at CoG (but oriented correctly).
The same calculation is done there https://github.com/ros-controls/ros2_controllers/blob/master/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp#L318

destogl · 2023-03-19T21:02:49Z

test/control_filters/test_gravity_compensation.cpp

+  node_->declare_parameter("world_frame", "world");
+  node_->declare_parameter("sensor_frame", "sensor");
+  node_->declare_parameter("CoG.force", std::vector<double>({0.0, 0.0, -gravity_acc * mass}));


We shouldn't do this. We have generate_parameter_library to take care of this things. So no declaration of setting of parameters, but using generate_parameter_library that sets the params when node is started.

For differnet cases simply use different *.yaml and test files

hum, ok. I did not know this change to generate_parameter_library had to also be applied to the tests which are quite flexible with node parameter declaration.
Indeed, in other PRs where I had to create tests with a YAML, the values in the YAML have to match the values in the test (to verify the values were loaded correctly by code). This means 2 files have to be edited when setting the parameters, to ensure everything matches. It is easier when params and verified in one place.
Maybe I could move to "set_parameter" only instead of declaring. But I think this might be complex or impossible because param declaration happens in the filter_chain, with the node passed to it, and should already have the params overriden there. To override the params in the node, they must be declared isn't it ? this is a chicken-egg problem.

I did the change. I am not sure this is was really needed, but at least there is no "visible" declaration.

destogl · 2023-03-19T21:05:11Z

test/control_filters/test_gravity_compensation.hpp

+// TODO(destogl): do this
+// subclassing and friending so we can access member variables
+


Suggested change

// TODO(destogl): do this

// subclassing and friending so we can access member variables

christophfroehlich

Could we add a short documentation here please? Some (pseudo-) formulas to describe the mathematics would be great, otherwise it is very hard to review.
Maybe in a separate readme.[md,rst] together with some high-level description of what the filter is good for.

guihomework · 2023-03-20T00:29:44Z

Could we add a short documentation here please? Some (pseudo-) formulas to describe the mathematics would be great, otherwise it is very hard to review. Maybe in a separate readme.[md,rst] together with some high-level description of what the filter is good for.

Sure, this could permit to avoid others not understanding immediately what was going on either, redoing all the maths.

guihomework · 2023-03-20T00:37:47Z

The calculations look too complex to me – at least to the older version as I can remember. Is everything necessary?

Also, tests should not declare parameters if possible, but generate_parameter_library should take care of it

I think I will clarify with a tiny drawing in a readme. I would say the code I started from was a special case, and did not work in all cases, especially if the force torque measurement does not happen in a frame that the kinematics knows about (not talking about lookup here).
One example was in case the force torque sensor is mounted at the robot flange, but the measurement happens 5 cm further (thickness of the sensor). So one needs to consider this transform, which adds one transform. But I simplified (removed one transform from Cog to world) the original code to not have to move the computation in world frame and back. World frame is just where gravity is defined. Computation do not have to be there I believe.

guihomework · 2023-03-20T13:43:14Z

Could we add a short documentation here please? Some (pseudo-) formulas to describe the mathematics would be great, otherwise it is very hard to review. Maybe in a separate readme.[md,rst] together with some high-level description of what the filter is good for.

I added a README.md inside the source folder, as the general README does not even describe the PID etc...

For differenet cases simply use different *.yaml and test files

I will try address this in some near future (also for #152)

christophfroehlich · 2023-03-20T13:57:21Z

I added a README.md inside the source folder, as the general README does not even describe the PID etc...

Great thx, I already realized the lack of any docs here. -> #154 ;)

I'll have a look the next days.

christophfroehlich

LGTM

Thanks for the docs, sounds reasonable and according to the actual implementation.
I only added some typo and format-related suggestions.

christophfroehlich · 2023-04-02T09:22:37Z

src/control_filters/README.md

+## Available filters
+
+* Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench).
+*


Suggested change

*

christophfroehlich · 2023-04-02T09:22:55Z

src/control_filters/README.md

@@ -0,0 +1,59 @@
+# Control Filters


Suggested change

# Control Filters

# Control filters

christophfroehlich · 2023-04-02T09:23:03Z

src/control_filters/README.md

+*
+
+
+## Gravity Compensation filter


Suggested change

## Gravity Compensation filter

## Gravity compensation filter

christophfroehlich · 2023-04-02T09:23:35Z

src/control_filters/README.md

+
+ 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.


Suggested change

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.

christophfroehlich · 2023-04-02T09:24:47Z

src/control_filters/README.md

+
+## 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.


Suggested change

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.

christophfroehlich · 2023-04-02T09:32:25Z

src/control_filters/README.md

+
+and,
+
+fc<sub>s</sub> = f<sub>s</sub> - T<sub>sw</sub>.g<sub>w</sub>


Suggested change

fcs = fs - Tsw.gw

fcs = fs - Tswgw

christophfroehlich · 2023-04-02T09:32:33Z

src/control_filters/README.md

+
+its force  and,
+
+&tau;c<sub>s</sub> = &tau;<sub>s</sub> - p<sub>s</sub> x (T<sub>sw</sub>.g<sub>w</sub>)


Suggested change

τcs = τs - ps x (Tsw.gw)

τcs = τs - ps x (Tswgw)

christophfroehlich · 2023-04-02T09:32:43Z

src/control_filters/README.md

+
+its torque, and
+
+&Fscr;<sub>s</sub>  = T<sub>si</sub>.&Fscr;<sub>i</sub>


Suggested change

&Fscr;s = Tsi.&Fscr;i

&Fscr;s = Tsi&Fscr;i = {fs, τs}

christophfroehlich · 2023-04-02T09:34:32Z

src/control_filters/README.md

+
+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.


Suggested change

* `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_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.

christophfroehlich · 2023-04-02T09:35:11Z

src/control_filters/README.md

+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.


Suggested change

* `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_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.

guihomework · 2023-04-03T09:35:37Z

Thanks a lot @christophfroehlich for the review and sorry for fixing spell-checking, I had forgotten to install a spell checker in VS Code, my former editor had one integrated.

Now, only the issue about removing clock access is open I believe and is not straight forward to solve. @destogl can we live with this ?

bmagyar · 2023-05-06T20:24:19Z

include/control_filters/gravity_compensation.hpp

+bool GravityCompensation<T>::configure()
+{
+  clock_ = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_));


maybe for a follow-up PR, you could augment the filterbase to also store a clock interface

test/control_filters/test_gravity_compensation.cpp

guihomework · 2023-05-10T08:26:10Z

I just added one export that solves an issue when building in symlink-install

  ament_cmake_symlink_install_targets()
  'gravity_compensation_filter_parameters' is an interface library - there's
  nothing to symlink install.  Perhaps you forgot to add EXPORT or INCLUDES
  DESTINATION?

…compensation-filter-rolling

destogl and others added 10 commits March 7, 2023 17:15

Added initial files of control filters

5471301

Apply suggestions from code review

7532c93

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>

Added initial files of control filters

d61437a

Update version of gravity compensation to use parameter_handler and o…

e63a7a1

…ptimize code.

Apply formatting to filters.

d5a55a7

Update filters to use full impl of parameter hanlder.

3817947

Fixed PR115 especially for humble

c046598

fixed gravity compensation calculated values fixed guard condition issues (rclcpp init must be called before) fixed configure issues (low-level must be configured to avoid undefined logger)

Switched to generate_parameter_library

e6d3e27

Reworked and activated control filter loading test

9cee59e

Improved parameter_handler usage when re-configure

5715e8f

guihomework force-pushed the add-gravity-compensation-filter-rolling branch from 528d4ea to 5715e8f Compare March 9, 2023 15:57

Applied code-review suggestions and fixed includes

fcb9cca

destogl mentioned this pull request Mar 13, 2023

Ported LowPass and GravityCompensation filters iirob_filters repository #115

Closed

Fixed missing namespace forwarding

b18ffbd

guihomework force-pushed the add-gravity-compensation-filter-rolling branch from ab3838d to b18ffbd Compare March 13, 2023 19:31

guihomework added 7 commits March 18, 2023 18:11

Switched to wrench transforms

ebd7481

Fail update at failed lookupTransform

b7ffc66

Allow data_out frame of reference selection

94ed150

Renaming test nodes for consistency

fc47ee0

Made computation clearer & for generic const force

c910033

Clarified error msg of GravityCompensation update

8fec166

Fixed, simplified & clarified Gravity computations

73072fb

guihomework mentioned this pull request Mar 19, 2023

Admittance with filters destogl/ros2_controllers#45

Closed

destogl requested review from bmagyar, christophfroehlich and destogl March 19, 2023 20:53

destogl marked this pull request as ready for review March 19, 2023 20:53

destogl requested changes Mar 19, 2023

View reviewed changes

christophfroehlich reviewed Mar 19, 2023

View reviewed changes

guihomework added 2 commits March 20, 2023 10:44

Cleaned-up

fdf43db

Improved understandability of the filter

b74a1ee

Switched to yaml-based param in filter test

88d6b8a

christophfroehlich previously approved these changes Apr 2, 2023

View reviewed changes

Applied suggested formatting and typo fixes

b22a754

guihomework dismissed christophfroehlich’s stale review via b22a754 April 3, 2023 09:30

This was referenced Apr 3, 2023

Add filtering capability and publishers for additional frames to fts broadcaster ros-controls/ros2_controllers#559

Draft

Admittance with filters ros-controls/ros2_controllers#560

Open

christophfroehlich previously approved these changes Apr 4, 2023

View reviewed changes

Merge branch 'ros2-master' into add-gravity-compensation-filter-rolling

f181fe7

bmagyar previously approved these changes May 6, 2023

View reviewed changes

Cleaned up TODO

819941c

guihomework dismissed stale reviews from bmagyar and christophfroehlich via 819941c May 9, 2023 09:59

Fixed build fail in symlink mode

1c960dd

guihomework and others added 2 commits August 4, 2023 15:35

Merge remote-tracking branch 'upstream/ros2-master' into add-gravity-…

7b59f3c

…compensation-filter-rolling

Merge branch 'ros2-master' into add-gravity-compensation-filter-rolling

5915fe1

destogl mentioned this pull request Aug 17, 2023

How to add clock interface in the filters? #162

Open

Merge branch 'ros2-master' into add-gravity-compensation-filter-rolling

1e7d9dc

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add gravity compensation filter #153

Add gravity compensation filter #153

guihomework commented Mar 7, 2023

guihomework commented Mar 18, 2023

destogl left a comment •

edited

Loading

destogl Mar 19, 2023

destogl Mar 19, 2023

destogl Mar 19, 2023

guihomework Mar 20, 2023

bmagyar May 6, 2023

destogl Mar 19, 2023

guihomework Mar 20, 2023

destogl Mar 19, 2023

destogl Mar 19, 2023

guihomework Mar 20, 2023

guihomework Mar 25, 2023

destogl Mar 19, 2023

christophfroehlich left a comment

guihomework commented Mar 20, 2023

guihomework commented Mar 20, 2023

guihomework commented Mar 20, 2023

christophfroehlich commented Mar 20, 2023

christophfroehlich left a comment •

edited

Loading

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

christophfroehlich Apr 2, 2023

guihomework commented Apr 3, 2023

bmagyar May 6, 2023

guihomework commented May 10, 2023

		// TODO(destogl): do this
		// subclassing and friending so we can access member variables

	## 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.

		The filter relies on ROS TF, and might fail if transforms are missing.

	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.


		and,

		fc<sub>s</sub> = f<sub>s</sub> - T<sub>sw</sub>.g<sub>w</sub>


		its force and,

		τc<sub>s</sub> = τ<sub>s</sub> - p<sub>s</sub> x (T<sub>sw</sub>.g<sub>w</sub>)


		its torque, and

		&Fscr;<sub>s</sub> = T<sub>si</sub>.&Fscr;<sub>i</sub>

	&Fscr;<sub>s</sub> = T<sub>si</sub>.&Fscr;<sub>i</sub>
	&Fscr;<sub>s</sub> = T<sub>si</sub>&Fscr;<sub>i</sub> = {f<sub>s</sub>, τ<sub>s</sub>}

	* `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_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. Ex: Wrench expressed in a `control_frame` for instance center of a gripper.
	* `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.

Add gravity compensation filter #153

Are you sure you want to change the base?

Add gravity compensation filter #153

Conversation

guihomework commented Mar 7, 2023

guihomework commented Mar 18, 2023

destogl left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

christophfroehlich left a comment

Choose a reason for hiding this comment

guihomework commented Mar 20, 2023

guihomework commented Mar 20, 2023

guihomework commented Mar 20, 2023

christophfroehlich commented Mar 20, 2023

christophfroehlich left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

guihomework commented Apr 3, 2023

Choose a reason for hiding this comment

guihomework commented May 10, 2023

destogl left a comment •

edited

Loading

christophfroehlich left a comment •

edited

Loading