Split mass and gravity_field

Author: christophfroehlich

control_toolbox/include/control_filters/gravity_compensation.hpp

@@ -53,12 +53,12 @@ class GravityCompensation : public filters::FilterBase<T>
 protected:
   void compute_internal_params()
   {
-    cog_.vector.x = parameters_.CoG.pos[0];
-    cog_.vector.y = parameters_.CoG.pos[1];
-    cog_.vector.z = parameters_.CoG.pos[2];
-    cst_ext_force_.vector.x = parameters_.CoG.force[0];
-    cst_ext_force_.vector.y = parameters_.CoG.force[1];
-    cst_ext_force_.vector.z = parameters_.CoG.force[2];
+    cog_.vector.x = parameters_.tool.pos[0];
+    cog_.vector.y = parameters_.tool.pos[1];
+    cog_.vector.z = parameters_.tool.pos[2];
+    cst_ext_force_.vector.x = parameters_.tool.gravity_field[0]*parameters_.tool.mass;
+    cst_ext_force_.vector.y = parameters_.tool.gravity_field[1]*parameters_.tool.mass;
+    cst_ext_force_.vector.z = parameters_.tool.gravity_field[2]*parameters_.tool.mass;
   };
 
 private:

control_toolbox/src/control_filters/gravity_compensation_filter_parameters.yaml

@@ -15,18 +15,23 @@ gravity_compensation_filter:
       not_empty<>: []
     },
   }
-  CoG:
+  tool:
     pos: {
       type: double_array,
       description: "Position of the CoG of tool attached to the FT sensor in the sensor frame.",
       validation: {
         fixed_size<>: 3
       },
     }
-    force: {
+    gravity_field: {
         type: double_array,
-        description: "Specifies the constant field-induced force applied at the CoG of tool defined in fixed world frame, e.g [0, 0, -mass * 9.81].",
+        description: "Specifies the gravity field defined in fixed world frame in m/s^2.",
+        default_value: [0, 0, -9.81],
         validation: {
           fixed_size<>: 3
         },
       }
+    mass: {
+        type: double,
+        description: "The mass of the tool in kg.",
+      }

control_toolbox/test/control_filters/test_gravity_compensation.cpp

@@ -47,13 +47,13 @@ TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter
     node_->get_node_logging_interface(), node_->get_node_parameters_interface()), std::exception);
 
   // fixed wrong vector size
-  node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector<double>({0.0, 0.0, 0.0})));
+  node_->set_parameter(rclcpp::Parameter("tool.pos", std::vector<double>({0.0, 0.0, 0.0})));
   // all parameters correctly set AND second call to yet unconfigured filter
   ASSERT_TRUE(filter_->configure("", "TestGravityCompensation",
     node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
 
   // change a parameter
-  node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector<double>({0.0, 0.0, 0.2})));
+  node_->set_parameter(rclcpp::Parameter("tool.pos", std::vector<double>({0.0, 0.0, 0.2})));
   // accept second call to configure with valid parameters to already configured filter
   ASSERT_TRUE(filter_->configure("", "TestGravityCompensation",
     node_->get_node_logging_interface(), node_->get_node_parameters_interface()));

control_toolbox/test/control_filters/test_gravity_compensation_parameters.yaml

@@ -2,8 +2,9 @@ TestGravityCompensationAllParameters:
   ros__parameters:
     world_frame: world
     sensor_frame: sensor
-    CoG:
-      force: [0.0, 0.0, -49.05]
+    tool:
+      gravity_field: [0.0, 0.0, -9.81]
+      mass: 5.0
       pos: [0.0, 0.0, 0.0]
 
 TestGravityCompensationMissingParameters:
@@ -14,22 +15,25 @@ TestGravityCompensationInvalidThenFixedParameter:
   ros__parameters:
     world_frame: world
     sensor_frame: sensor
-    CoG:
-      force: [0.0, 0.0, -49.05]
+    tool:
+      gravity_field: [0.0, 0.0, -9.81]
+      mass: 5.0
       pos: [0.0, 0.0]
 
 TestGravityCompensationMissingTransform:
   ros__parameters:
     world_frame: world
     sensor_frame: sensor
-    CoG:
-      force: [0.0, 0.0, -49.05]
+    tool:
+      gravity_field: [0.0, 0.0, -9.81]
+      mass: 5.0
       pos: [0.0, 0.0, 0.0]
 
 TestGravityCompensationComputation:
   ros__parameters:
     world_frame: world
     sensor_frame: world
-    CoG:
-      force: [0.0, 0.0, -49.05]
+    tool:
+      gravity_field: [0.0, 0.0, -9.81]
+      mass: 5.0
       pos: [0.0, 0.0, 0.0]