Add derivative filter and discretization methods #1909
+117
−0
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Overview
Add a first-order derivative filter and selectable discretization methods for the I and D paths, keeping previous behavior as the default.
What was added/changed in this PR
Derivative filter (D path): added
derivative_filter_time
(seconds) ingains.__map_dof_names
.Continuous form:
C_D(s) = D * s / (1 + s * derivative_filter_time)
. Default0.0
preserves the prior (unfiltered) behavior; non-negative validation.Discretization methods: added per-DOF options
integration_method
:forward_euler
(default),backward_euler
,trapezoidal
(Tustin).derivative_method
:forward_euler
(default),backward_euler
,trapezoidal
(Tustin).Defaults match the previous implicit scheme, so existing setups remain unchanged unless these parameters are set.
About tests
Builds cleanly and passes pre-commit and
colcon test
.About change in tests
Added
PidControllerTest.all_parameters_set_configure_success_full_gains
, which loadstest_pid_controller_full_gains
, asserts successfulon_configure(...)
, and verifies parsing/mapping of all standard fields plus the newderivative_filter_time
,integration_method
, andderivative_method
values.Related PR's
Final notes
I'm very open to any recommendations to improve this code.