From ade41de0e3dd4b54e87537172b570fcfb83a647f Mon Sep 17 00:00:00 2001 From: "V. Oguz TOKMAK" <5289529+vahapt@users.noreply.github.com> Date: Sun, 22 Jun 2025 20:49:49 +0300 Subject: [PATCH 1/4] Added support for MPPI Controller to adjust wz_std parameter based on linear speed Signed-off-by: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- configuration/packages/configuring-mppic.rst | 38 ++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 4b2fb4ce7..2d1669877 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -305,6 +305,44 @@ MPPI Parameters Description Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. +Advanced Parameters +------------------- + +| **WARNING**: These parameters are experimental and subject to change; **use them at your own risk** as they may lead to unpredictable behavior in certain configurations. + +| ``wz_std_decay_strength`` and ``wz_std_decay_to`` defines a function that enables dynamic modification of wz_std (angular deviation) based on linear velocity of the robot. +| When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed. Lowering wz_std stabilizes the robot but then the maneuvers take more time. + +| Dynamically reducing wz_std as vx, vy increase (speed of the robot) solves both problems. +| Suggested values to start with are: ``wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0`` + +| The following is used as the decay function: +| ``f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to`` + +`Visualize the decay function here (a=wz_std, b=wz_std_decay_to, c=wz_std_decay_strength) `_ + +:advanced.wz_std_decay_strength: + + ============== =========================== + Type Default + -------------- --------------------------- + double -1.0 (disabled) + ============== =========================== + + Description + Defines the strength of the reduction function. + +:advanced.wz_std_decay_to: + + ============== =========================== + Type Default + -------------- --------------------------- + double 0.0 + ============== =========================== + + Description + Target wz_std value while linear speed goes to infinity. Must be between 0 and wz_std. Has no effect if ``advanced.wz_std_decay_strength <= 0.0`` + Trajectory Visualization ------------------------ From 46e6929d96dae03f4dad880de8bf8e09d607a67e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 26 Jun 2025 07:34:47 -0700 Subject: [PATCH 2/4] Update configuration/packages/configuring-mppic.rst Signed-off-by: Steve Macenski --- configuration/packages/configuring-mppic.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 2d1669877..c52af6522 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -309,7 +309,6 @@ Advanced Parameters ------------------- | **WARNING**: These parameters are experimental and subject to change; **use them at your own risk** as they may lead to unpredictable behavior in certain configurations. - | ``wz_std_decay_strength`` and ``wz_std_decay_to`` defines a function that enables dynamic modification of wz_std (angular deviation) based on linear velocity of the robot. | When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed. Lowering wz_std stabilizes the robot but then the maneuvers take more time. From 8b8288f6e471f09c3dce141d549847cee7f7189c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 26 Jun 2025 07:34:53 -0700 Subject: [PATCH 3/4] Update configuration/packages/configuring-mppic.rst Signed-off-by: Steve Macenski --- configuration/packages/configuring-mppic.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index c52af6522..08e697822 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -312,7 +312,7 @@ Advanced Parameters | ``wz_std_decay_strength`` and ``wz_std_decay_to`` defines a function that enables dynamic modification of wz_std (angular deviation) based on linear velocity of the robot. | When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed. Lowering wz_std stabilizes the robot but then the maneuvers take more time. -| Dynamically reducing wz_std as vx, vy increase (speed of the robot) solves both problems. +| Dynamically reducing wz_std as the speed of the robot solves both problems, which these parameters enable. | Suggested values to start with are: ``wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0`` | The following is used as the decay function: From fa9f5293a9460ada1a092688f28f7777308c5565 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 26 Jun 2025 07:35:51 -0700 Subject: [PATCH 4/4] Update configuration/packages/configuring-mppic.rst Signed-off-by: Steve Macenski --- configuration/packages/configuring-mppic.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 08e697822..f4742b846 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -311,7 +311,6 @@ Advanced Parameters | **WARNING**: These parameters are experimental and subject to change; **use them at your own risk** as they may lead to unpredictable behavior in certain configurations. | ``wz_std_decay_strength`` and ``wz_std_decay_to`` defines a function that enables dynamic modification of wz_std (angular deviation) based on linear velocity of the robot. | When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed. Lowering wz_std stabilizes the robot but then the maneuvers take more time. - | Dynamically reducing wz_std as the speed of the robot solves both problems, which these parameters enable. | Suggested values to start with are: ``wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0``