Skip to content

Commit 9c01c3a

Browse files
Rename parameter
1 parent 7e2d968 commit 9c01c3a

File tree

4 files changed

+10
-10
lines changed

4 files changed

+10
-10
lines changed

Diff for: control_toolbox/include/control_filters/gravity_compensation.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -53,9 +53,9 @@ class GravityCompensation : public filters::FilterBase<T>
5353
protected:
5454
void compute_internal_params()
5555
{
56-
cog_.vector.x = parameters_.tool.pos[0];
57-
cog_.vector.y = parameters_.tool.pos[1];
58-
cog_.vector.z = parameters_.tool.pos[2];
56+
cog_.vector.x = parameters_.tool.CoG[0];
57+
cog_.vector.y = parameters_.tool.CoG[1];
58+
cog_.vector.z = parameters_.tool.CoG[2];
5959
cst_ext_force_.vector.x = parameters_.tool.gravity_field[0]*parameters_.tool.mass;
6060
cst_ext_force_.vector.y = parameters_.tool.gravity_field[1]*parameters_.tool.mass;
6161
cst_ext_force_.vector.z = parameters_.tool.gravity_field[2]*parameters_.tool.mass;

Diff for: control_toolbox/src/control_filters/gravity_compensation_filter_parameters.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ gravity_compensation_filter:
1616
},
1717
}
1818
tool:
19-
pos: {
19+
CoG: {
2020
type: double_array,
2121
description: "Position of the CoG of tool attached to the FT sensor in the sensor frame.",
2222
validation: {

Diff for: control_toolbox/test/control_filters/test_gravity_compensation.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -47,13 +47,13 @@ TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter
4747
node_->get_node_logging_interface(), node_->get_node_parameters_interface()), std::exception);
4848

4949
// fixed wrong vector size
50-
node_->set_parameter(rclcpp::Parameter("tool.pos", std::vector<double>({0.0, 0.0, 0.0})));
50+
node_->set_parameter(rclcpp::Parameter("tool.CoG", std::vector<double>({0.0, 0.0, 0.0})));
5151
// all parameters correctly set AND second call to yet unconfigured filter
5252
ASSERT_TRUE(filter_->configure("", "TestGravityCompensation",
5353
node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
5454

5555
// change a parameter
56-
node_->set_parameter(rclcpp::Parameter("tool.pos", std::vector<double>({0.0, 0.0, 0.2})));
56+
node_->set_parameter(rclcpp::Parameter("tool.CoG", std::vector<double>({0.0, 0.0, 0.2})));
5757
// accept second call to configure with valid parameters to already configured filter
5858
ASSERT_TRUE(filter_->configure("", "TestGravityCompensation",
5959
node_->get_node_logging_interface(), node_->get_node_parameters_interface()));

Diff for: control_toolbox/test/control_filters/test_gravity_compensation_parameters.yaml

+4-4
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ TestGravityCompensationAllParameters:
55
tool:
66
gravity_field: [0.0, 0.0, -9.81]
77
mass: 5.0
8-
pos: [0.0, 0.0, 0.0]
8+
CoG: [0.0, 0.0, 0.0]
99

1010
TestGravityCompensationMissingParameters:
1111
ros__parameters:
@@ -18,7 +18,7 @@ TestGravityCompensationInvalidThenFixedParameter:
1818
tool:
1919
gravity_field: [0.0, 0.0, -9.81]
2020
mass: 5.0
21-
pos: [0.0, 0.0]
21+
CoG: [0.0, 0.0]
2222

2323
TestGravityCompensationMissingTransform:
2424
ros__parameters:
@@ -27,7 +27,7 @@ TestGravityCompensationMissingTransform:
2727
tool:
2828
gravity_field: [0.0, 0.0, -9.81]
2929
mass: 5.0
30-
pos: [0.0, 0.0, 0.0]
30+
CoG: [0.0, 0.0, 0.0]
3131

3232
TestGravityCompensationComputation:
3333
ros__parameters:
@@ -36,4 +36,4 @@ TestGravityCompensationComputation:
3636
tool:
3737
gravity_field: [0.0, 0.0, -9.81]
3838
mass: 5.0
39-
pos: [0.0, 0.0, 0.0]
39+
CoG: [0.0, 0.0, 0.0]

0 commit comments

Comments
 (0)