From 9c343b951846cb8c78eafd9c8f2418f6ab07823c Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Wed, 4 Mar 2026 09:44:14 +0900 Subject: [PATCH] Add slow down and final approach parameters Signed-off-by: Tatsuro Sakaguchi --- ...configuring-graceful-motion-controller.rst | 60 +++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/configuration/packages/configuring-graceful-motion-controller.rst b/configuration/packages/configuring-graceful-motion-controller.rst index adcf501942..49dfc7a011 100644 --- a/configuration/packages/configuring-graceful-motion-controller.rst +++ b/configuration/packages/configuring-graceful-motion-controller.rst @@ -212,6 +212,61 @@ Graceful Controller Parameters Description Whether to use collision detection to avoid obstacles. +:footprint_scaling_linear_vel: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description + The linear velocity threshold (m/s) below which footprint scaling is not applied. When the simulated velocity exceeds this value, the footprint is expanded proportionally to the ratio between the current velocity and ``v_linear_max``. If the trajectory is in collision at the current speed, the controller retries with reduced speeds down to this threshold in steps of ``footprint_scaling_step``. + +:footprint_scaling_factor: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.25 + ============== ============================= + + Description + The maximum additional scaling factor applied to the robot footprint at ``v_linear_max``. A value of ``0.25`` means the footprint can be expanded up to 125% of its original size at maximum velocity. The actual scaling increases linearly with velocity above ``footprint_scaling_linear_vel``. + +:footprint_scaling_step: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + The step size (m/s) for reducing the simulated velocity when a trajectory is in collision at the current speed. The controller iterates from ``v_linear_max`` down to ``footprint_scaling_linear_vel`` in steps of this size, attempting to find a collision-free trajectory at a lower speed with a smaller footprint. + +:obstacle_cost_margin: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 1 + ============== ============================= + + Description + The cost margin below the maximum valid cost that is considered "close to obstacles." When the maximum cost along the final approach trajectory is within this margin of the maximum valid cost, the controller searches for a safer alternative approach angle using spiral curves. A higher value triggers the search more aggressively. Must be less than the maximum non-obstacle cost value. + +:final_rotation_search_step: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + The angular step size (rad) used when searching for an alternative final approach angle that avoids obstacles. The controller sweeps through orientations in steps of this size to find a spiral curve approach that keeps the trajectory farther from obstacles. Smaller values provide finer search granularity at the cost of increased computation. + :allow_parameter_qos_overrides: ============== ============================= @@ -263,3 +318,8 @@ Example v_angular_max: 5.0 v_angular_min_in_place: 0.25 slowdown_radius: 1.5 + footprint_scaling_linear_vel: 0.5 + footprint_scaling_factor: 0.25 + footprint_scaling_step: 0.1 + obstacle_cost_margin: 1 + final_rotation_search_step: 0.1