Distance-Based Velocity Scaling for SafetyPositionController by Joschi3 · Pull Request #25 · tu-darmstadt-ros-pkg/hector_ros_controllers

Joschi3 · 2026-02-25T22:09:33Z

Summary

Replaces the binary block_if_too_far toggle with mandatory distance-based velocity scaling when self-collision checks are enabled.

Changes:

CollisionChecker now returns a CollisionResult struct with both collision flag and minimum clearance distance (previously only a boolean)
New collision_safety_zone parameter: Between collision_padding and collision_safety_zone, joint velocities are linearly scaled from 0% to 100%. Beyond the safety zone, full speed is allowed. At or below padding, motion is blocked entirely.
Velocity limiting is always enforced when check_self_collisions is enabled (removed the user-disableable block_if_too_far parameter)

Copilot

Pull request overview

This PR updates the SafetyPositionController to replace the legacy binary “block-if-too-far” behavior with mandatory distance-based velocity scaling when self-collision checking is enabled, and extends collision checking to report clearance distances and optional per-pair gradients for direction-aware scaling and diagnostics.

Changes:

Introduces CollisionResult (collision flag + minimum distance + optional safety-zone pair gradients) and integrates it into the controller’s distance-based (and optionally direction-aware) velocity scaling.
Adds new parameters and diagnostics/status reporting for safety-zone scaling, directional derivatives, broadphase usage, and manipulability.
Expands the test suite substantially, including standalone collision-checker unit tests, controller scaling tests, and a Google Benchmark target (with Athena URDF/SRDF fixtures).

Reviewed changes

Copilot reviewed 15 out of 15 changed files in this pull request and generated 4 comments.

Show a summary per file

File	Description
`hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg`	Adds status fields for distance-based scaling, directional diagnostics, manipulability, and current-limit reporting.
`hector_ros_controllers/src/safety_position_controller.cpp`	Implements distance-based + directional velocity scaling, periodic status publishing, and enhanced collision-check flow.
`hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp`	Updates controller API (renames velocity-limiting helper) and adds state for scaling/diagnostics.
`hector_ros_controllers/src/collision_checker.cpp`	Implements `CollisionResult`, broadphase distance traversal, safety-zone pair collection, gradients, and manipulability helper.
`hector_ros_controllers/include/safety_position_controller/collision_checker.hpp`	Declares `CollisionResult` and new APIs (broadphase toggles, velocity-space helpers, directional viz info, manipulability).
`hector_ros_controllers/params/safety_position_controller_parameters.yaml`	Adds/updates parameters for safety zone, directional scaling, broadphase, visualization, manipulability, and status rate.
`hector_ros_controllers/test/test_safety_position_controller.cpp`	Updates existing tests and adds extensive coverage for distance/directional scaling and activation validation.
`hector_ros_controllers/test/test_collision_checker.cpp`	New standalone unit tests validating distances/gradients and broadphase correctness vs brute-force reference.
`hector_ros_controllers/test/benchmark_collision_checker.cpp`	Adds Google Benchmark coverage for collision-check performance (brute-force and broadphase variants).
`hector_ros_controllers/test/test_helpers.hpp`	Expands/clarifies rationale for exposing private/protected members in tests.
`hector_ros_controllers/test/config/athena.urdf`	Adds Athena robot URDF fixture for correctness/perf tests.
`hector_ros_controllers/test/config/athena.srdf`	Adds Athena SRDF fixture for correctness/perf tests.
`hector_ros_controllers/CMakeLists.txt`	Registers the new collision-checker gmock test and benchmark target.
`hector_ros_controllers/package.xml`	Adds benchmark test dependency.
`README.md`	Updates Safety Position Controller documentation to reflect the new scaling/diagnostics behavior and parameters.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

- collision_checker: replace per-call safety_zone_threshold argument with setSafetyZoneThreshold() / getSafetyZoneThreshold() so the cache key reflects the threshold; widening the threshold invalidates q_last_ to prevent reuse of a result that omitted now-relevant pairs - collision_checker: make computeManipulability() self-sufficient by calling computeFrameJacobian directly and guarding against stale q_last_, removing the implicit dependency on a prior checkCollision() pass - safety_position_controller: drop the simulated current-limits fallback; joint_names/current_limits now stay empty when set_current_limits is disabled, matching the documented message contract - test_safety_position_controller: rename "...But Still Limits Velocity" section header to match the actual "Allows Relaxed Limits" assertion - tests + benchmark: migrate all callers to the new setSafetyZoneThreshold API, including the two BroadphaseMatchesBruteForce cases that previously declared a safety_zone but never applied it

Copilot

Pull request overview

This PR upgrades SafetyPositionController self-collision handling by replacing the previous binary “block if too far” behavior with mandatory distance-based velocity scaling (plus optional directional scaling) when self-collision checks are enabled, and expands diagnostics/benchmarking around collision checking.

Changes:

CollisionChecker now returns a richer CollisionResult (collision flag + minimum distance + optional per-pair gradients) and adds broadphase acceleration, gradient computation, and manipulability calculation.
SafetyPositionController enforces distance-based velocity limiting (and optional direction-aware override) whenever collision checking is active; adds periodic status publishing and new status fields.
Adds extensive tests (unit tests, integration-ish tests, and benchmarks) plus Athena URDF/SRDF fixtures and updated documentation/parameters.

Reviewed changes

Copilot reviewed 15 out of 15 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
`hector_ros_controllers/src/safety_position_controller.cpp`	Implements distance-based + directional velocity scaling; adds periodic status timer and new status fields.
`hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp`	Adds state for scaling/diagnostics and directional scaling helpers.
`hector_ros_controllers/src/collision_checker.cpp`	Implements `CollisionResult`, broadphase distance traversal, gradient computation, minimal marker publishing, manipulability.
`hector_ros_controllers/include/safety_position_controller/collision_checker.hpp`	Adds `CollisionResult` API and new helper methods/flags (broadphase, safety zone threshold, visualization).
`hector_ros_controllers/params/safety_position_controller_parameters.yaml`	Replaces old toggle with new safety-zone/directional/broadphase/status parameters and validations.
`hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg`	Adds diagnostics fields for distance scaling, directional derivative, manipulability, and current-limit reporting.
`hector_ros_controllers/test/test_safety_position_controller.cpp`	Updates existing tests and adds new distance/directional scaling behavior tests.
`hector_ros_controllers/test/test_collision_checker.cpp`	Adds standalone unit tests validating distance and gradients vs brute-force reference and broadphase.
`hector_ros_controllers/test/benchmark_collision_checker.cpp`	Adds Google Benchmark harness for collision checker performance comparisons.
`hector_ros_controllers/test/test_helpers.hpp`	Expands comment explaining private/protected exposure hack in tests.
`hector_ros_controllers/test/config/athena.urdf`	Adds Athena URDF fixture used by tests/benchmarks.
`hector_ros_controllers/test/config/athena.srdf`	Adds Athena SRDF fixture used by tests/benchmarks.
`hector_ros_controllers/CMakeLists.txt`	Registers new gmock test and benchmark target.
`hector_ros_controllers/package.xml`	Adds test dependency for Google Benchmark.
`README.md`	Documents new controller behavior, parameters, diagnostics, and visualization.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

- safety_position_controller: snapshot status fields into a realtime_tools::RealtimeBuffer<StatusSnapshot> written once per update() tick; publish_status() now reads the snapshot, removing the data race between the wall_timer callback and the controller update thread on last_min_distance_, last_safety_zone_pairs_, etc. - collision_checker: expose min_distance_pair_index on CollisionResult (already computed locally in checkCollisionQ); publishMinimalMarkers uses it as a fallback so collisions are visualized even when no gradients were requested (safety_zone_threshold == 0) - collision_checker: replace aggregate initializers with default- construct + assign so adding fields to CollisionResult doesn't trip -Wmissing-field-initializers - test/config/athena.urdf: strip absolute /home/... path from header - header hygiene: add <unistd.h> for _exit in test/benchmark mains, <vector>/<string>/<unordered_map>/<stdexcept> in benchmark, <vector> in collision_checker.hpp, <cstdint>/<limits> in safety_position_controller.hpp

Copilot

Pull request overview

This PR updates the SafetyPositionController safety behavior by replacing the previous binary “block-if-too-far” gating with mandatory distance-based velocity scaling whenever self-collision checks are enabled, and extends the CollisionChecker API to return richer distance/gradient diagnostics.

Changes:

Replace block_if_too_far behavior with distance-based scaling (collision_padding → stop, collision_safety_zone → ramp to full speed), plus optional direction-aware override using distance gradients.
Extend CollisionChecker to return a CollisionResult (collision flag + minimum distance + optional safety-zone pair gradients) and add broadphase distance acceleration, visualization, and manipulability support.
Expand status reporting/tests/benchmarks (new status fields, extensive new tests, Athena URDF/SRDF fixtures, and a Google Benchmark target).

Reviewed changes

Copilot reviewed 15 out of 15 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
`hector_ros_controllers/src/safety_position_controller.cpp`	Implements distance-based (and directional) velocity scaling, status snapshot publishing, collision checker integration, and timer-based status publishing.
`hector_ros_controllers/src/collision_checker.cpp`	Adds `CollisionResult`, broadphase distance callback, gradient computation, lightweight distance markers, and manipulability calculation.
`hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp`	Adds scaling/directional fields, status snapshot buffering, and new helper APIs.
`hector_ros_controllers/include/safety_position_controller/collision_checker.hpp`	Introduces `CollisionResult`, safety-zone threshold APIs, broadphase toggles, visualization controls, and manipulability API.
`hector_ros_controllers/params/safety_position_controller_parameters.yaml`	Replaces/extends parameters for safety-zone scaling, directional scaling, broadphase, visualization, status rate, and validations.
`hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg`	Adds distance-scaling diagnostics, manipulability, and per-joint active current limits.
`hector_ros_controllers/test/test_safety_position_controller.cpp`	Updates existing tests and adds extensive new tests for scaling, directional behavior, validation, bypass, status fields, etc.
`hector_ros_controllers/test/test_collision_checker.cpp`	Adds standalone CollisionChecker correctness/gradient/broadphase tests against brute-force reference and includes performance assertions.
`hector_ros_controllers/test/benchmark_collision_checker.cpp`	Adds Google Benchmark suite for CollisionChecker (brute-force and broadphase variants).
`hector_ros_controllers/test/test_helpers.hpp`	Documents the private/protected exposure macro rationale used by tests.
`hector_ros_controllers/test/config/athena.urdf` / `athena.srdf`	Adds real-robot fixtures for collision and benchmark validation.
`hector_ros_controllers/CMakeLists.txt` / `package.xml`	Adds benchmark/test targets and benchmark build dependency.
`README.md`	Updates Safety Position Controller documentation to reflect new scaling behavior and diagnostics.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

- Move status timer creation from on_init to on_activate so changes to status_publish_rate take effect on (re)activation; reset in on_deactivate. - Use NaN as the 'not evaluated' sentinel for worst_directional_derivative instead of numeric_limits::max(), so status consumers don't see 1.8e308 when directional scaling wasn't computed.

vel scaling close to collisions

44e9c39

Joschi3 self-assigned this Feb 25, 2026

Joschi3 added 8 commits March 3, 2026 14:48

added collision checker test

da4971c

directional scaling

7906603

code optimizations

4b74e03

updated Readme

27988de

fixed tests

4294534

controller improvements

94be713

added current limit output

80953be

debug visu

153a5a8

Joschi3 requested a review from Copilot April 27, 2026 13:42

Copilot started reviewing on behalf of Joschi3 April 27, 2026 13:42 View session

Copilot AI reviewed Apr 27, 2026

View reviewed changes

Joschi3 requested a review from Copilot April 27, 2026 15:07

Copilot started reviewing on behalf of Joschi3 April 27, 2026 15:08 View session

Copilot AI reviewed Apr 27, 2026

View reviewed changes

Joschi3 requested a review from Copilot April 27, 2026 16:06

Copilot started reviewing on behalf of Joschi3 April 27, 2026 16:06 View session

Copilot AI reviewed Apr 27, 2026

View reviewed changes

Comment thread hector_ros_controllers/src/safety_position_controller.cpp Outdated

Comment thread hector_ros_controllers/src/safety_position_controller.cpp

Comment thread hector_ros_controllers/src/collision_checker.cpp

Joschi3 added 4 commits April 28, 2026 16:21

merged jazzy

bfe6e6b

updated Readme

614cee0

removed unnecessary logging

92b575e

Joschi3 merged commit 7f6d463 into jazzy Apr 28, 2026
1 of 2 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Distance-Based Velocity Scaling for SafetyPositionController#25

Distance-Based Velocity Scaling for SafetyPositionController#25
Joschi3 merged 15 commits into
jazzyfrom
feature/saftey_position_controller_collision_vel_scaling

Joschi3 commented Feb 25, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

Joschi3 commented Feb 25, 2026

Summary

Changes:

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants