diff --git a/README.md b/README.md index 1240b9b..dbf484c 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,118 @@ All controllers are compatible with **ros2_control** and can be inserted as **ch --- -## 1. Safety Forward Controller +## 1. Safety Position Controller + +A **safety layer for joint position commands**, usable both + +* as a **chainable controller** behind e.g. a trajectory controller, or +* in **standalone mode** as a position controller receiving commands on `~/commands`. + +### Features + +* **Chainable or standalone**: + + * *Chained mode*: consumes reference interfaces from an upstream controller. + * *Non-chained mode*: acts as a position controller driven by `Float64MultiArray` on `~/commands`. +* **Continuous joint handling**: + + * Optionally unwraps continuous joints to the nearest equivalent angle. +* **Joint limit enforcement**: + + * Clamps commands to URDF limits (rev/prismatic/other) if enabled. +* **Self-collision check with distance-based velocity scaling**: + + * Uses URDF + SRDF via `CollisionChecker` (Pinocchio + hpp-fcl). + * Returns minimum pairwise clearance distance, not just a binary collision flag. + * When `check_self_collisions` is enabled, per-cycle joint motion is automatically limited based on URDF velocity limits (`block_velocity_scaling`) and **scaled down proportionally** as the robot approaches collision geometry. + * **Safety zone**: Between `collision_padding` and `collision_safety_zone`, velocity is linearly interpolated from 0% to 100%. Beyond `collision_safety_zone`, full velocity is allowed. At or below `collision_padding`, motion is blocked entirely. + * **Directional scaling** (`directional_collision_scaling`): When enabled, the controller computes collision distance gradients w.r.t. joint velocities for all pairs within the safety zone. Only motions that *decrease* a collision distance are slowed down — motions moving away from all collisions proceed at full speed. This prevents the robot from becoming trapped near collisions. The check is conservative: if *any* safety-zone pair's distance would decrease, the full distance-based scaling is applied. + * This replaces the former `block_if_too_far` parameter with a mandatory, distance-aware velocity limiting system. +* **Collision debug visualization** (RViz markers): + + * When `debug_visualize_collisions` is enabled, publishes `MarkerArray` on `~/debug_collision_geometry` with separate namespaces for toggling in RViz: + * `collision_geometry` — all collision geometry objects (gray = safe, red = colliding). + * `distance_lines_safe` — nearest-point lines for pairs outside the safety zone (gray). + * `distance_lines_safety_zone` — nearest-point lines for safety-zone pairs, colored per-pair: green = motion increases distance, red = motion decreases distance, yellow = no directional info. + * `distance_lines_collision` — nearest-point lines for colliding pairs (bright red, thicker). +* **Status topic** (`~/status`): + + * Publishes `SafetyPositionControllerStatus` (latched) with diagnostic fields: `min_collision_distance`, `distance_scale`, `effective_scale`, `worst_directional_derivative`, `num_pairs_in_safety_zone`, plus safety bypass, E-stop, and mode flags. +* **E-Stop**: + + * Subscribes to `~/safety_estop` (`std_msgs/Bool`). + * On activation → records current positions and *holds them* (no limit/collision checks) until E-Stop is released. +* **Safety Bypass Mode** (for folded arm positions): + + * Service `~/bypass_safety_checks` (`std_srvs/SetBool`) to temporarily disable collision checks and relax joint limits. + * Useful when driving the arm into folded positions where intentional collisions must be made. + * Adds configurable tolerance to joint limits (default 3%). + * Auto-disables after configurable timeout (default 60 seconds) for safety. + * **Note:** Joint wrapping for continuous joints remains **always active** even during bypass. +* **Optional current-limit control**: + + * Per-joint compliant/stiff current limits, written to `/current`. +* **Debug joint state publishers**: + + * If enabled, publishes incoming command references and outgoing commanded positions as `sensor_msgs/JointState`. + + +### Parameters (Safety Position Controller) + +| Parameter | Type | Default | Description | +| ---------------------------------- | ---------- | ------- | ------------------------------------------------------------------------------------------------------------------- | +| `joints` | `string[]` | `[]` | Names of joints controlled. Must match URDF. *(effectively read-only; set via params at startup)* | +| `unwrap_continuous_joints` | `bool` | `true` | If `true`, continuous joints are unwrapped to stay close to the current angle. | +| `enforce_position_limits` | `bool` | `true` | If `true`, clamps joint commands to URDF position limits. | +| `check_self_collisions` | `bool` | `true` | If `true`, performs self-collision checks and enables distance-based velocity scaling. *(read-only)* | +| `collision_padding` | `double` | `0.01` | Minimum allowed link-to-link distance [m]; distances ≤ padding are treated as collision. Bounds: [0.0, 1.0]. | +| `collision_safety_zone` | `double` | `0.05` | Outer safety zone distance [m]. Between `collision_padding` and this value, velocity is linearly scaled down. Must be > `collision_padding`. | +| `directional_collision_scaling` | `bool` | `true` | If `true`, velocity scaling near collisions is direction-aware: only motions that decrease any collision distance in the safety zone are slowed down. Motions moving away proceed at full speed. Requires `check_self_collisions`. | +| `collision_cache_epsilon` | `double` | `1e-6` | Threshold [rad] for reusing the previous collision result (skip recomputation if the pose change is below this value). | +| `use_broadphase` | `bool` | `true` | If `true`, uses AABB-tree broadphase acceleration for self-collision distance queries (3–5× speedup). Only relevant when `check_self_collisions` is true. *(read-only)* | +| `block_velocity_scaling` | `double` | `1.5` | Scales maximum per-cycle motion: allowed step = `velocity_limit / update_rate * block_velocity_scaling`. Bounds: [0.01, 15.0]. Only active when `check_self_collisions` is true. | +| `debug_visualize_collisions` | `bool` | `false` | If `true`, publishes collision debug markers for RViz (via `CollisionChecker`). Uses separate marker namespaces for toggling. | +| `publish_collision_distances` | `bool` | `false` | If `true`, publishes lightweight distance-only markers for safety zone / collision pairs via a realtime publisher. Cheaper than `debug_visualize_collisions`. Ignored when full debug visualization is active. | +| `set_current_limits` | `bool` | `false` | If `true`, enables writing `/current` limits for compliant/stiff modes. *(read-only)* | +| `current_limits.*.compliant_limit` | `double` | `3.0` | Per-joint current limit in **compliant** mode [A]. | +| `current_limits.*.stiff_limit` | `double` | `5.0` | Per-joint current limit in **stiff** mode [A]. | +| `publish_debug_joint_states` | `bool` | `false` | If `true`, publishes debug `JointState` messages for incoming references and outgoing commands. | +| `safety_bypass_timeout` | `double` | `60.0` | Time in seconds after which safety bypass auto-disables. Bounds: [0.1, 600.0]. | +| `safety_bypass_joint_limit_tolerance` | `double` | `0.03` | Tolerance factor added to joint limits during bypass. E.g., 0.03 = 3% of the total joint range added to both sides. Bounds: [0.0, 1.0]. | + +> **Note:** Parameters are read/updated at `on_activate()`. To apply runtime changes reliably, deactivate and reactivate the controller. + +--- + +### Topics & Services + +**Subscriptions** + +| Topic | Type | Used for | +| ---------------------------- | ----------------------------------- | ---------------------------------------------------------------------------- | +| `~/commands` | `std_msgs/Float64MultiArray` | **Non-chained mode**: incoming position commands (one per joint). | +| `~/safety_estop` | `std_msgs/Bool` | E-Stop control. `true` → latch and hold current positions; `false` → resume. | +| `robot_description_semantic` | `std_msgs/String` (transient local) | SRDF XML for self-collision checking. | + +**Publications** + +| Topic | Type | Content | Condition | +| ------------------------------- | --------------------------------------- | ---------------------------------------------------------------------- | --------- | +| `~/status` | `SafetyPositionControllerStatus` | Latched diagnostics: collision distances, scaling factors, safety state. | Always | +| `~/debug_collision_geometry` | `visualization_msgs/MarkerArray` | Collision geometry, distance lines, colored by directional scaling. | `debug_visualize_collisions = true` or `publish_collision_distances = true` | +| `~/debug_in_joint_states` | `sensor_msgs/JointState` | Names = `joints`, positions = current **references** (input commands). | `publish_debug_joint_states = true` | +| `~/debug_out_joint_states` | `sensor_msgs/JointState` | Names = `joints`, positions = final **commanded** joint positions. | `publish_debug_joint_states = true` | + +**Services** + +| Service | Type | Description | +| -------------------------- | ------------------ | ---------------------------------------------------------------------------------------------------- | +| `~/enforce_current_limits` | `std_srvs/SetBool` | Enable (`true`) or disable (`false`) compliant mode (switch between compliant/stiff current limits). Only available when `set_current_limits = true`. | +| `~/bypass_safety_checks` | `std_srvs/SetBool` | Enable (`true`) or disable (`false`) safety bypass mode. Disables collision checks and relaxes joint limits. Auto-disables after `safety_bypass_timeout` seconds. Joint wrapping remains active. | + +--- + +## 2. Safety Forward Controller The **Safety Forward Controller** is a chainable ROS 2 controller that forwards joint commands while enforcing three safety mechanisms: @@ -52,13 +163,6 @@ The **safety timer is disabled in chained mode** and is only active when the con --- -## 2. Self Collision Avoidance Controller - -Monitors joint states and **prevents the robot from moving into a configuration close to self-collision**. -Acts as a motion gatekeeper in real-time. - ---- - ## 3. Velocity-to-Position Command Controller Converts **velocity references** into **position commands** for joint hardware. Useful when an upstream controller outputs velocity commands but the hardware only accepts position commands. @@ -125,104 +229,9 @@ pos_cmd = desired_pos[i] + vel_p - vel_d + sync_correction | `~/debug_in_joint_states` | `sensor_msgs/JointState` | Incoming velocity references (only when `publish_debug_joint_states`). | | `~/debug_out_joint_states` | `sensor_msgs/JointState` | Outgoing position commands (only when `publish_debug_joint_states`). | ---- - -## 4. Safety Position Controller - -A **safety layer for joint position commands**, usable both - -* as a **chainable controller** behind e.g. a trajectory controller, or -* in **standalone mode** as a position controller receiving commands on `~/commands`. - -### Features - -* **Chainable or standalone**: - - * *Chained mode*: consumes reference interfaces from an upstream controller. - * *Non-chained mode*: acts as a position controller driven by `Float64MultiArray` on `~/commands`. -* **Continuous joint handling**: - - * Optionally unwraps continuous joints to the nearest equivalent angle. -* **Joint limit enforcement**: - - * Clamps commands to URDF limits (rev/prismatic/other) if enabled. -* **Target-only self-collision check**: - - * Uses URDF + SRDF via `CollisionChecker`. - * Only the *target pose* is checked; large jumps are prevented via `block_if_too_far`. -* **Block-if-too-far**: - - * Limits per-cycle motion based on URDF velocity limits and controller update rate. -* **E-Stop**: - - * Subscribes to `~/safety_estop` (`std_msgs/Bool`). - * On activation → records current positions and *holds them* (no limit/collision checks) until E-Stop is released. -* **Safety Bypass Mode** (for folded arm positions): - - * Service `~/bypass_safety_checks` (`std_srvs/SetBool`) to temporarily disable collision checks and relax joint limits. - * Useful when driving the arm into folded positions where intentional collisions must be made. - * Adds configurable tolerance to joint limits (default 3%). - * Auto-disables after configurable timeout (default 60 seconds) for safety. - * **Note:** Joint wrapping for continuous joints remains **always active** even during bypass. -* **Optional current-limit control**: - - * Per-joint compliant/stiff current limits, written to `/current`. -* **Debug joint state publishers**: - - * If enabled, publishes incoming command references and outgoing commanded positions as `sensor_msgs/JointState`. - - -### Parameters (Safety Position Controller) - -| Parameter | Type | Default | Description | -| ---------------------------------- | ---------- | ------- | ------------------------------------------------------------------------------------------------------------------- | -| `joints` | `string[]` | `[]` | Names of joints controlled. Must match URDF. *(effectively read-only; set via params at startup)* | -| `unwrap_continuous_joints` | `bool` | `true` | If `true`, continuous joints are unwrapped to stay close to the current angle. | -| `enforce_position_limits` | `bool` | `true` | If `true`, clamps joint commands to URDF position limits. | -| `check_self_collisions` | `bool` | `true` | If `true`, performs a self-collision check on the **target** pose before sending commands. | -| `collision_padding` | `double` | `0.0` | Minimum allowed link-to-link distance [m]; distances ≤ padding are treated as collision. | -| `collision_cache_epsilon` | `double` | `1e-6` | Threshold for reusing the previous collision result (skip recomputation if the pose change is below this value). | -| `block_if_too_far` | `bool` | `true` | If `true`, blocks/limits commands that are too far from the current state. Auto-enabled if `check_self_collisions`. | -| `block_velocity_scaling` | `double` | `1.5` | Scales maximum per-cycle motion: allowed step ≈ `velocity_limit / update_rate * block_velocity_scaling`. | -| `debug_visualize_collisions` | `bool` | `false` | If `true`, publishes collision debug markers for RViz (via `CollisionChecker`). | -| `set_current_limits` | `bool` | `false` | If `true`, enables writing `/current` limits for compliant/stiff modes. *(configured at startup)* | -| `current_limits.*.compliant_limit` | `double` | `3.0` | Per-joint current limit in **compliant** mode [A]. | -| `current_limits.*.stiff_limit` | `double` | `5.0` | Per-joint current limit in **stiff** mode [A]. | -| `publish_debug_joint_states` | `bool` | `false` | If `true`, publishes debug `JointState` messages for incoming references and outgoing commands. | -| `safety_bypass_timeout` | `double` | `60.0` | Time in seconds after which safety bypass auto-disables. Must be positive. | -| `safety_bypass_joint_limit_tolerance` | `double` | `0.03` | Tolerance factor (0.0-1.0) added to joint limits during bypass. E.g., 0.03 = 3% beyond normal limits. | - -> **Note:** Parameters are read/updated at `on_activate()`. To apply runtime changes reliably, deactivate and reactivate the controller. --- -### Topics & Services - -**Subscriptions** - -| Topic | Type | Used for | -| ---------------------------- | ----------------------------------- | ---------------------------------------------------------------------------- | -| `~/commands` | `std_msgs/Float64MultiArray` | **Non-chained mode**: incoming position commands (one per joint). | -| `~/safety_estop` | `std_msgs/Bool` | E-Stop control. `true` → latch and hold current positions; `false` → resume. | -| `robot_description_semantic` | `std_msgs/String` (transient local) | SRDF XML for self-collision checking. | - -**Publications (debug, optional)** - -Enabled if `publish_debug_joint_states = true`: - -| Topic | Type | Content | -| -------------------------- | ------------------------ | ---------------------------------------------------------------------- | -| `~/debug_in_joint_states` | `sensor_msgs/JointState` | Names = `joints`, positions = current **references** (input commands). | -| `~/debug_out_joint_states` | `sensor_msgs/JointState` | Names = `joints`, positions = final **commanded** joint positions. | - -**Services** - -| Service | Type | Description | -| -------------------------- | ------------------ | ---------------------------------------------------------------------------------------------------- | -| `~/enforce_current_limits` | `std_srvs/SetBool` | Enable (`true`) or disable (`false`) compliant mode (switch between compliant/stiff current limits). | -| `~/bypass_safety_checks` | `std_srvs/SetBool` | Enable (`true`) or disable (`false`) safety bypass mode. Disables collision checks and relaxes joint limits. Auto-disables after `safety_bypass_timeout` seconds. Joint wrapping remains active. | - - ## Example Configuration ```yaml @@ -247,8 +256,10 @@ Enabled if `publish_debug_joint_states = true`: enforce_position_limits: true check_self_collisions: true collision_padding: 0.01 + collision_safety_zone: 0.05 + directional_collision_scaling: true debug_visualize_collisions: false - block_velocity_scaling: 3.0 + block_velocity_scaling: 1.5 set_current_limits: true current_limits: arm_joint_1: { compliant_limit: 3.0, stiff_limit: 9.0 } diff --git a/hector_ros_controllers/CMakeLists.txt b/hector_ros_controllers/CMakeLists.txt index 03ab475..499cf6a 100644 --- a/hector_ros_controllers/CMakeLists.txt +++ b/hector_ros_controllers/CMakeLists.txt @@ -140,6 +140,15 @@ if(BUILD_TESTING) safety_forward_controller_parameters safety_position_controller_parameters) + ament_add_gmock(test_collision_checker test/test_collision_checker.cpp) + target_include_directories( + test_collision_checker PRIVATE ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/test) + target_link_libraries( + test_collision_checker ${PROJECT_NAME}::controllers_test_doubles + ament_index_cpp::ament_index_cpp) + target_compile_definitions(test_collision_checker PRIVATE ${DEFS}) + ament_add_gmock(test_safety_forward_controller test/test_safety_forward_controller.cpp) target_include_directories( @@ -160,6 +169,19 @@ if(BUILD_TESTING) ament_index_cpp::ament_index_cpp) target_compile_definitions(test_safety_position_controller PRIVATE ${DEFS}) + find_package(ament_cmake_google_benchmark REQUIRED) + ament_add_google_benchmark(benchmark_collision_checker + test/benchmark_collision_checker.cpp) + if(TARGET benchmark_collision_checker) + target_include_directories( + benchmark_collision_checker PRIVATE ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/test) + target_link_libraries( + benchmark_collision_checker ${PROJECT_NAME}::controllers_test_doubles + ament_index_cpp::ament_index_cpp) + target_compile_definitions(benchmark_collision_checker PRIVATE ${DEFS}) + endif() + ament_add_gmock(test_velocity_to_position_command_controller test/test_velocity_to_position_command_controller.cpp) target_include_directories( diff --git a/hector_ros_controllers/include/safety_position_controller/collision_checker.hpp b/hector_ros_controllers/include/safety_position_controller/collision_checker.hpp index 231b49f..86b4a8c 100644 --- a/hector_ros_controllers/include/safety_position_controller/collision_checker.hpp +++ b/hector_ros_controllers/include/safety_position_controller/collision_checker.hpp @@ -7,16 +7,37 @@ #include #include +#include #include #include #include #include +#include + +#include +#include #include +#include +#include #include -// #define SAFETY_CC_ENABLE_TIMING +// #define SAFETY_CC_ENABLE_TIMING // TODO: remove when no longer needed for optimization + +/// Result of a collision query: collision flag + minimum clearance. +struct CollisionResult { + bool in_collision{ false }; ///< true if any pair distance <= padding + double min_distance{ std::numeric_limits::max() }; ///< global minimum pairwise distance [m] + + /// Per-pair info for pairs within the safety zone (only populated when gradient computation requested). + struct PairInfo { + std::size_t pair_index; ///< index into geom_model_.collisionPairs + double distance; ///< pairwise distance [m] + Eigen::VectorXd gradient; ///< dd/dv (size model_.nv): positive = moving apart + }; + std::vector safety_zone_pairs; ///< pairs with distance < safety_zone_threshold +}; /// Self-collision checker using Pinocchio + hpp-fcl; optional RViz debug markers. class CollisionChecker @@ -30,7 +51,7 @@ class CollisionChecker * @param pub_debug_geometry publish MarkerArray on ~/debug_collision_geometry */ explicit CollisionChecker( const rclcpp_lifecycle::LifecycleNode::SharedPtr &node, - double collision_padding = 0.0, double collision_cache_epsilon = 1e-4, + double collision_padding = 0.0, double collision_cache_epsilon = 1e-6, bool pub_debug_geometry = false ); /** @@ -54,23 +75,38 @@ class CollisionChecker /** * @brief Collision check from name→position map. - * - nq==1: assign directly - * - continuous revolute (nq=2,nv=1): [cos(θ), sin(θ)] - * - others: left at neutral with warning * @param joint_positions rad (rev) / m (prismatic) - * @return true if any distance ≤ padding + * @param safety_zone_threshold when > 0, computes per-pair distance gradients (dd/dv) + * for all pairs with distance < threshold. Set to 0 to skip gradient computation. + * @return CollisionResult with collision flag, minimum clearance, and optional per-pair gradients */ - bool checkCollision( const std::unordered_map &joint_positions ); + CollisionResult checkCollision( const std::unordered_map &joint_positions, + double safety_zone_threshold = 0.0 ); /** * @brief Collision check for full q. - * - FK + update placements - * - computeDistances() with nearest points + cached GJK - * - cache: reuse if last collision result if q change ≤ epsilon * @param q size == model_.nq - * @return true if any distance ≤ padding + * @param safety_zone_threshold when > 0, computes per-pair distance gradients (dd/dv) + * for all pairs with distance < threshold. Set to 0 to skip gradient computation. + * @return CollisionResult with collision flag, minimum clearance, and optional per-pair gradients + */ + CollisionResult checkCollisionQ( const Eigen::VectorXd &q, double safety_zone_threshold = 0.0 ); + + /** + * @brief Get the velocity-space index for a named joint. + * @return starting index in model_.nv, or -1 if not found + */ + int getJointVelocityIndex( const std::string &joint_name ) const; + + /** + * @brief Get the total velocity-space dimension. */ - bool checkCollisionQ( const Eigen::VectorXd &q ); + int getNv() const; + + /** + * @brief Get the number of collision pairs. + */ + std::size_t getNumCollisionPairs() const; /** * @brief Set collision padding [m]. @@ -90,13 +126,61 @@ class CollisionChecker */ void updateCollisionCacheEpsilon( double epsilon ); + /** + * @brief Set per-pair directional derivatives for visualization coloring. + * Must be called before the next collision check if you want colors to reflect motion direction. + * @param derivatives one value per collision pair; NaN = no info, >=0 = moving away, <0 = moving closer + * @param safety_zone_threshold the threshold used to classify pairs into safety zone vs safe + */ + void setDirectionalInfo( const std::vector &derivatives, double safety_zone_threshold ); + + /** + * @brief Toggle lightweight collision distance visualization. + * Publishes only safety-zone and collision distance lines via a realtime publisher. + * Ignored when full debug visualization is active. + * @param enable on/off + */ + void updatePublishCollisionDistances( bool enable ); + + /** + * @brief Enable/disable broadphase AABB-tree acceleration for distance queries. + * Call before initFromXml, or re-call initFromXml after changing. + */ + void setBroadphase( bool enable ); + + /** + * @brief Whether broadphase acceleration is enabled. + */ + bool isBroadphaseEnabled() const; + + /** + * @brief Compute the Yoshikawa manipulability index for a given end-effector frame. + * Requires FK and joint Jacobians to have been called (i.e., after checkCollision). + * @param ee_frame_name name of the end-effector frame in the URDF + * @return w = sqrt(det(J * J^T)), 0 if singular or frame not found + */ + double computeManipulability( const std::string &ee_frame_name ); + private: /** - * @brief Publish geometry and nearest-point LINE_LIST markers. - * - red if part of a pair with distance ≤ 0 + * @brief Compute the distance gradient for a single collision pair. + * Requires FK + computeJointJacobians to have been called already. + * @param pair_k index into geom_model_.collisionPairs + * @return gradient vector of size model_.nv + */ + Eigen::VectorXd computePairGradient( std::size_t pair_k ); + + /** + * @brief Publish geometry and nearest-point markers with namespace-separated categories. */ void publishMarkers() const; + /** + * @brief Publish lightweight distance-only markers for safety zone and collision pairs. + * Uses realtime publisher (non-blocking). Skips geometry markers and safe-pair lines. + */ + void publishMinimalMarkers(); + /** * @brief Keep only pairs attached to controlled joints (and ancestors). * @param controlled_joints names defining relevance @@ -113,17 +197,42 @@ class CollisionChecker Eigen::VectorXd q_default_; Eigen::VectorXd q_last_; - bool last_collision_state_{ false }; + CollisionResult last_collision_result_; double collision_padding_{ 0.0 }; - double collision_cache_epsilon_{ 1e-4 }; + double collision_cache_epsilon_{ 1e-6 }; bool pub_debug_geometry_{ false }; + bool pub_collision_distances_{ false }; + std::shared_ptr> rt_markers_pub_; std::unordered_map name_to_id_; + // Per-pair directional derivatives for visualization (set by controller via setDirectionalInfo) + std::vector viz_directional_derivatives_; ///< one per collision pair; NaN = no info + double viz_safety_zone_threshold_{ 0.0 }; + + // Pre-allocated Jacobian workspace (sized in initFromXml) + Eigen::MatrixXd J1_workspace_; ///< 6 × nv + Eigen::MatrixXd J2_workspace_; ///< 6 × nv + + // Broadphase acceleration + bool use_broadphase_{ true }; + using BroadPhaseManager = + pinocchio::BroadPhaseManagerTpl; + std::unique_ptr broadphase_manager_; + #ifdef SAFETY_CC_ENABLE_TIMING - double sum_timings_{ 0.0 }; - int n_timings_{ 0 }; + struct TimingStats { + double fk_us{ 0 }; + double placement_us{ 0 }; + double distance_us{ 0 }; + double jacobian_us{ 0 }; + double gradient_us{ 0 }; + double total_us{ 0 }; + int count{ 0 }; + std::size_t num_safety_zone_pairs{ 0 }; + }; + TimingStats timing_stats_; #endif }; diff --git a/hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp b/hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp index 4d8ced0..cec2310 100644 --- a/hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp +++ b/hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -127,10 +128,11 @@ class SafetyPositionController final : public controller_interface::ChainableCon void enforce_limits(); /** - * @brief Limit per-cycle motion by vel_limit/update_rate (scaled). - * Reduces command toward current position if excessive. + * @brief Limit per-cycle motion by vel_limit/update_rate, scaled by + * block_velocity_scaling and the distance-based scaling factor. + * @param distance_scale [0,1] factor from distance-based scaling (1.0 = full speed, 0.0 = hold) */ - void block_if_too_far(); + void apply_velocity_limits( double distance_scale ); /** * @brief Write current-limit commands (stiff/compliant) if enabled. @@ -185,14 +187,22 @@ class SafetyPositionController final : public controller_interface::ChainableCon */ bool wait_for_srdf(); + /** + * @brief Compute the directional derivative of collision distance along the + * commanded motion direction for a given gradient vector. + * @param gradient dd/dv vector (size model_.nv) from CollisionChecker + * @return dot product (positive = moving away from collision) + */ + double compute_directional_derivative( const Eigen::VectorXd &gradient ) const; + void publish_debug_joint_state_in(); void publish_debug_joint_state_out( const std::vector &positions ); void update_debug_publishers( bool enable ); void publish_status(); // ---- Configuration / mode ---- - bool is_chained_ = true; ///< chained-only controller (hold if false) - bool in_compliant_mode_ = false; ///< selects compliant vs. stiff current limits + bool is_chained_ = true; ///< chained-only controller (hold if false) + std::atomic in_compliant_mode_{ false }; ///< selects compliant vs. stiff current limits // ---- E-stop ---- std::atomic estop_active_{ false }; ///< last requested E-stop state @@ -221,13 +231,23 @@ class SafetyPositionController final : public controller_interface::ChainableCon std::unordered_map cc_positions_; ///< name→position map for CC std::unique_ptr collision_checker_; ///< optional self-collision checker std::string srdf_; ///< SRDF XML (semantic) - bool srdf_received_ = false; ///< latched SRDF received + std::atomic srdf_received_{ false }; ///< latched SRDF received + double last_min_distance_{ + std::numeric_limits::max() }; ///< previous cycle's min collision distance + double last_manipulability_{ 0.0 }; ///< latest Yoshikawa manipulability index + + // ---- Directional collision scaling ---- + std::vector joint_v_index_; ///< maps controlled joint index → pinocchio velocity-space index + std::vector + last_safety_zone_pairs_; ///< safety-zone pairs from previous collision check + double last_distance_scale_{ 1.0 }; + double last_effective_scale_{ 1.0 }; + double last_worst_directional_derivative_{ std::numeric_limits::max() }; // ---- Command/state buffers (aligned with params_.joints) ---- std::vector cmd_positions_; ///< post-enforcement commands std::vector current_positions_; ///< latest measured positions - bool on_hold_{ false }; ///< non-chained fallback - std::vector hold_positions_; ///< positions to hold when not chained + std::vector hold_positions_; ///< positions to hold during E-stop // ---- Parameters ---- std::shared_ptr param_listener_; @@ -252,8 +272,9 @@ class SafetyPositionController final : public controller_interface::ChainableCon rclcpp::Publisher::SharedPtr debug_in_js_pub_; rclcpp::Publisher::SharedPtr debug_out_js_pub_; - // Status publisher (latched) + // Status publisher (latched) + periodic timer rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::TimerBase::SharedPtr status_timer_; static constexpr int throttle_logging_msg = 2000; ///< ms; throttle for WARN/ERROR logs }; diff --git a/hector_ros_controllers/package.xml b/hector_ros_controllers/package.xml index e092a91..566f51a 100644 --- a/hector_ros_controllers/package.xml +++ b/hector_ros_controllers/package.xml @@ -31,6 +31,7 @@ visualization_msgs ament_cmake_gmock + ament_cmake_google_benchmark ament_index_cpp lifecycle_msgs rtest diff --git a/hector_ros_controllers/params/safety_position_controller_parameters.yaml b/hector_ros_controllers/params/safety_position_controller_parameters.yaml index 0bb3a00..57c3d67 100644 --- a/hector_ros_controllers/params/safety_position_controller_parameters.yaml +++ b/hector_ros_controllers/params/safety_position_controller_parameters.yaml @@ -19,32 +19,71 @@ safety_position_controller: type: bool default_value: true description: If true, the controller checks for potential self-collisions at the target pose before applying position commands. + read_only: true collision_padding: type: double - default_value: 0.0 + default_value: 0.01 description: Minimum allowed distance in [m] between any two links to consider the robot safe from self-collisions when 'check_self_collisions' is true. + validation: + bounds<>: [0.0, 1.0] collision_cache_epsilon: type: double default_value: 0.000001 description: Epsilon in [rad] used to speed up self-collision checks by caching previous distance computations when 'check_self_collisions' is true. - block_if_too_far: + use_broadphase: type: bool default_value: true - description: If true, the controller blocks position commands that are too far from the current position. Automatically true when 'check_self_collisions' is true. + description: > + If true, uses AABB-tree broadphase acceleration for self-collision distance queries. + This prunes far-apart geometry pairs before running narrow-phase GJK, typically + yielding a 3-5x speedup. Only relevant when 'check_self_collisions' is true. + read_only: true block_velocity_scaling: type: double default_value: 1.5 - description: Scaling factor block_if_too_far. it is blocked when (target_position - current_position) > block_velocity_scaling * dt * velocity limit. + description: > + Scaling factor for velocity-limited stepping. The maximum per-cycle step is + velocity_limit / update_rate * block_velocity_scaling. Only active when + 'check_self_collisions' is true. + validation: + bounds<>: [0.01, 15.0] + + collision_safety_zone: + type: double + default_value: 0.05 + description: > + Outer safety zone distance in [m]. When the minimum collision distance falls between + 'collision_padding' and 'collision_safety_zone', joint velocities are linearly scaled + down to zero. Must be strictly greater than 'collision_padding'. + + directional_collision_scaling: + type: bool + default_value: true + description: > + If true, velocity scaling near collisions is direction-aware: only motions + that decrease any collision distance in the safety zone are slowed down. + Motions moving away from all collisions proceed at full speed. + Requires 'check_self_collisions' to be true. debug_visualize_collisions: type: bool default_value: false description: If true, the controller publishes visualization markers for potential self-collisions to help with debugging. + publish_collision_distances: + type: bool + default_value: false + description: > + If true, publishes lightweight visualization markers showing only the distance + lines for collision pairs within the safety zone. Much cheaper than + 'debug_visualize_collisions' as it skips geometry markers and safe-pair lines. + Uses a realtime publisher for non-blocking operation. Ignored if + 'debug_visualize_collisions' is true. + publish_debug_joint_states: type: bool default_value: false @@ -67,12 +106,32 @@ safety_position_controller: default_value: 5.0 description: Default current limit for each joint in the group in Ampere. Will be set if current interface requested but compliant mode is off. + manipulability_ee_frame: + type: string + default_value: arm_end_link + description: > + Name of the URDF frame used for manipulability index computation + (Yoshikawa w = sqrt(det(J*J^T))). Set to empty string to disable. + + status_publish_rate: + type: double + default_value: 10.0 + description: > + Rate in Hz at which the safety controller status (including collision distance + and manipulability) is published. Set to 0 to only publish on events. + validation: + bounds<>: [0.0, 100.0] + safety_bypass_timeout: type: double default_value: 60.0 description: Time in seconds after which safety checks are automatically re-enabled when bypassed via service. Must be positive. + validation: + bounds<>: [0.1, 600.0] safety_bypass_joint_limit_tolerance: type: double default_value: 0.03 description: Tolerance factor (0.0-1.0) added to joint limits when safety bypass is active. E.g., 0.03 means 3% tolerance beyond normal limits. + validation: + bounds<>: [0.0, 1.0] diff --git a/hector_ros_controllers/src/collision_checker.cpp b/hector_ros_controllers/src/collision_checker.cpp index b651969..5fa2496 100644 --- a/hector_ros_controllers/src/collision_checker.cpp +++ b/hector_ros_controllers/src/collision_checker.cpp @@ -3,19 +3,103 @@ // #include "safety_position_controller/collision_checker.hpp" +#include #include +#include #include #include #include #include +#include #include "pinocchio/collision/distance.hpp" #include #include +#include #include #include #include +namespace +{ +/// Custom hpp-fcl broadphase distance callback that collects all safety-zone pairs +/// and tracks the global minimum distance. Follows the same pattern as +/// pinocchio::CollisionCallBackDefault (broadphase-callbacks.hpp:90-96). +struct SafetyZoneDistanceCallback : hpp::fcl::DistanceCallBackBase { + // Inputs (set before each broadphase scan) + const pinocchio::GeometryModel *geom_model_ptr{ nullptr }; + pinocchio::GeometryData *geom_data_ptr{ nullptr }; + double safety_zone_threshold{ 0.0 }; + bool compute_nearest_points{ false }; ///< true in single-pass (debug_viz) mode + + // Outputs (filled during traversal) + double global_min_distance{ std::numeric_limits::max() }; + std::size_t min_distance_pair{ 0 }; + std::vector safety_zone_indices; + + void init() override + { + global_min_distance = std::numeric_limits::max(); + min_distance_pair = 0; + safety_zone_indices.clear(); + } + + bool distance( hpp::fcl::CollisionObject *o1, hpp::fcl::CollisionObject *o2, + hpp::fcl::FCL_REAL &dist ) override + { + // Cast to pinocchio::CollisionObject to get geometry indices + // (safe: pinocchio creates these objects in BroadPhaseManagerTpl::init) + auto &co1 = reinterpret_cast( *o1 ); + auto &co2 = reinterpret_cast( *o2 ); + + auto go1 = static_cast( co1.geometryObjectIndex ); + auto go2 = static_cast( co2.geometryObjectIndex ); + + // collisionPairMapping is upper triangular: needs go1 < go2 + if ( go1 > go2 ) + std::swap( go1, go2 ); + + // Look up collision pair index (-1 if not a tracked pair) + const int pair_index = geom_model_ptr->collisionPairMapping( go1, go2 ); + if ( pair_index < 0 ) + return false; // not a tracked pair, skip + + const auto k = static_cast( pair_index ); + + // Check if pair is active + if ( !geom_data_ptr->activeCollisionPairs[k] ) + return false; + + // Run narrow-phase distance via hpp-fcl + auto &dreq = geom_data_ptr->distanceRequests[k]; + auto &dres = geom_data_ptr->distanceResults[k]; + dreq.enable_nearest_points = compute_nearest_points; + dres.clear(); + + hpp::fcl::distance( o1, o2, dreq, dres ); + const double d = dres.min_distance; + + // Update outputs + if ( d < global_min_distance ) { + global_min_distance = d; + min_distance_pair = k; + } + + if ( safety_zone_threshold > 0.0 && d < safety_zone_threshold ) { + safety_zone_indices.push_back( k ); + } + + // Set the AABB pruning bound: the tree skips subtrees whose AABB lower + // bound exceeds dist. We need both the global minimum AND all safety zone + // pairs, so the bound must be max(global_min, safety_zone_threshold). + dist = ( safety_zone_threshold > 0.0 ) ? std::max( global_min_distance, safety_zone_threshold ) + : global_min_distance; + + return false; // never stop early — we need ALL safety zone pairs + } +}; +} // namespace + CollisionChecker::CollisionChecker( const rclcpp_lifecycle::LifecycleNode::SharedPtr &node, double collision_padding, double collision_cache_epsilon, bool pub_debug_geometry ) @@ -53,6 +137,14 @@ bool CollisionChecker::initFromXml( const std::string &urdf_xml, const std::stri } // Filter collision pairs based on controlled joints filterCollisionPairs( controlled_joints ); + + // Pre-allocate Jacobian workspace matrices + J1_workspace_ = Eigen::MatrixXd::Zero( 6, model_.nv ); + J2_workspace_ = Eigen::MatrixXd::Zero( 6, model_.nv ); + + // Always create broadphase manager (cheap); use_broadphase_ controls which path is taken + broadphase_manager_ = std::make_unique( &model_, &geom_model_, &geom_data_ ); + return true; } catch ( const std::exception &e ) { RCLCPP_ERROR( node_->get_logger(), "CollisionChecker init failed: %s", e.what() ); @@ -108,6 +200,15 @@ void CollisionChecker::filterCollisionPairs( const std::vector &con geom_model_.collisionPairs.swap( filtered ); + // Rebuild collisionPairMapping matrix to match new pair indices + geom_model_.collisionPairMapping.setConstant( -1 ); + for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) { + const auto &cp = geom_model_.collisionPairs[k]; + geom_model_.collisionPairMapping( static_cast( cp.first ), + static_cast( cp.second ) ) = + static_cast( k ); + } + // re-create GeometryData so requests/results match new pair count geom_data_ = pinocchio::GeometryData( geom_model_ ); @@ -126,21 +227,23 @@ std::vector CollisionChecker::getJointNames() const return out; } -bool CollisionChecker::checkCollision( const std::unordered_map &joint_positions ) +CollisionResult +CollisionChecker::checkCollision( const std::unordered_map &joint_positions, + double safety_zone_threshold ) { if ( model_.nq == 0 ) { RCLCPP_ERROR( node_->get_logger(), "Model not initialized." ); - return true; + return CollisionResult{ true, 0.0, {} }; } - // return true if any position is Nan or Inf + // return collision if any position is Nan or Inf for ( const auto &[name, position] : joint_positions ) { if ( std::isnan( position ) || std::isinf( position ) ) { RCLCPP_ERROR( node_->get_logger(), "Joint position for joint '%s' is NaN or Inf (%.3f). Assuming the robot is in collision.", name.c_str(), position ); - return true; + return CollisionResult{ true, 0.0, {} }; } } // transforms the joint positions into the pinocchio format @@ -173,73 +276,187 @@ bool CollisionChecker::checkCollision( const std::unordered_mapget_logger(), "q size (%ld) != model.nq (%d)", long( q.size() ), model_.nq ); - return true; + return CollisionResult{ true, 0.0, {} }; } // check if robot moved since the last check - if ( q.size() == q_last_.size() && ( q - q_last_ ).cwiseAbs().maxCoeff() < 1e-4 ) { + if ( q.size() == q_last_.size() && + ( q - q_last_ ).cwiseAbs().maxCoeff() < collision_cache_epsilon_ ) { // no movement -> no need to recompute distances - return last_collision_state_; + return last_collision_result_; } q_last_ = q; // Kinematics + placements pinocchio::forwardKinematics( model_, data_, q ); +#ifdef SAFETY_CC_ENABLE_TIMING + const auto t_fk = clock::now(); +#endif pinocchio::updateGeometryPlacements( model_, data_, geom_model_, geom_data_ ); +#ifdef SAFETY_CC_ENABLE_TIMING + const auto t_placement = clock::now(); +#endif - // Configure distance queries: nearest points + GJK guess caching - for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) { - auto &dreq = geom_data_.distanceRequests[k]; - dreq.enable_nearest_points = true; + const bool single_pass = pub_debug_geometry_; + + double global_min_distance = std::numeric_limits::max(); + std::size_t min_distance_pair = 0; + bool has_safety_zone_pairs = false; + std::vector safety_zone_indices; + + if ( use_broadphase_ && broadphase_manager_ ) { + // --- Broadphase path: AABB-tree pruned distance scan --- + broadphase_manager_->update( false ); // sync transforms from oMg, rebuild AABB tree + + SafetyZoneDistanceCallback callback; + callback.geom_model_ptr = &geom_model_; + callback.geom_data_ptr = &geom_data_; + callback.safety_zone_threshold = safety_zone_threshold; + callback.compute_nearest_points = single_pass; + callback.init(); + + broadphase_manager_->getManager().distance( &callback ); + + global_min_distance = callback.global_min_distance; + min_distance_pair = callback.min_distance_pair; + safety_zone_indices = std::move( callback.safety_zone_indices ); + has_safety_zone_pairs = !safety_zone_indices.empty(); + + // Pass 2: recompute safety-zone pairs with nearest points (for gradients) + if ( !single_pass && has_safety_zone_pairs ) { + for ( const std::size_t k : safety_zone_indices ) { + geom_data_.distanceRequests[k].enable_nearest_points = true; + geom_data_.distanceResults[k].clear(); + pinocchio::computeDistance( geom_model_, geom_data_, k ); + } + } + } else { + // --- Brute-force path: compute distances for ALL pairs --- + for ( auto &dreq : geom_data_.distanceRequests ) { dreq.enable_nearest_points = single_pass; } + pinocchio::computeDistances( geom_model_, geom_data_ ); + + for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) { + const auto &dres = geom_data_.distanceResults[k]; + + if ( dres.min_distance < global_min_distance ) { + global_min_distance = dres.min_distance; + min_distance_pair = k; + } + + if ( safety_zone_threshold > 0.0 && dres.min_distance < safety_zone_threshold ) { + has_safety_zone_pairs = true; + safety_zone_indices.push_back( k ); + } + } + + // Pass 2: recompute only safety-zone pairs with nearest points (for gradients) + if ( !single_pass && has_safety_zone_pairs ) { + for ( const std::size_t k : safety_zone_indices ) { + geom_data_.distanceRequests[k].enable_nearest_points = true; + geom_data_.distanceResults[k].clear(); + pinocchio::computeDistance( geom_model_, geom_data_, k ); + } + } } - // Distance pass (fills distanceResults + caches) - pinocchio::computeDistances( geom_model_, geom_data_ ); +#ifdef SAFETY_CC_ENABLE_TIMING + const auto t_distance = clock::now(); +#endif - bool in_collision = false; - for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) { - const auto &cp = geom_model_.collisionPairs[k]; + // Log collision outside the hot loop (throttled) + if ( global_min_distance <= collision_padding_ ) { + const auto &cp = geom_model_.collisionPairs[min_distance_pair]; const auto &o1 = geom_model_.geometryObjects[cp.first]; const auto &o2 = geom_model_.geometryObjects[cp.second]; - const auto &dres = geom_data_.distanceResults[k]; + RCLCPP_WARN_STREAM_THROTTLE( node_->get_logger(), *node_->get_clock(), 1000, + "Collision (or contact) distance " + << global_min_distance << " between " + << model_.frames[o1.parentFrame].name << " and " + << model_.frames[o2.parentFrame].name ); + } - // hpp-fcl distance is >= 0 for separated; 0 when touching - if ( dres.min_distance <= collision_padding_ ) { - in_collision = true; - RCLCPP_WARN_STREAM_THROTTLE( node_->get_logger(), *node_->get_clock(), 1000, - "Collision (or contact) distance " - << dres.min_distance << " between " - << model_.frames[o1.parentFrame].name << " and " - << model_.frames[o2.parentFrame].name ); - break; + CollisionResult result; + result.in_collision = ( global_min_distance <= collision_padding_ ); + result.min_distance = global_min_distance; + + // Compute per-pair distance gradients (lazy: only if pairs actually exist in safety zone) + if ( has_safety_zone_pairs ) { + pinocchio::computeJointJacobians( model_, data_ ); + +#ifdef SAFETY_CC_ENABLE_TIMING + const auto t_jacobian = clock::now(); +#endif + + for ( const std::size_t k : safety_zone_indices ) { + CollisionResult::PairInfo info; + info.pair_index = k; + info.distance = geom_data_.distanceResults[k].min_distance; + info.gradient = computePairGradient( k ); + result.safety_zone_pairs.push_back( std::move( info ) ); } + +#ifdef SAFETY_CC_ENABLE_TIMING + const auto t_gradient = clock::now(); + timing_stats_.jacobian_us += + static_cast( + std::chrono::duration_cast( t_jacobian - t_distance ).count() ) / + 1000.0; + timing_stats_.gradient_us += + static_cast( + std::chrono::duration_cast( t_gradient - t_jacobian ).count() ) / + 1000.0; +#endif } #ifdef SAFETY_CC_ENABLE_TIMING const auto t_end = clock::now(); - const auto us = std::chrono::duration_cast( t_end - t_begin ).count(); - sum_timings_ += static_cast( us ); - n_timings_++; - RCLCPP_INFO_THROTTLE( node_->get_logger(), *node_->get_clock(), 2000, - "[CC timing] checkCollisionQ (distances) avg = %.3f µs (pairs=%zu)", - sum_timings_ / n_timings_, geom_model_.collisionPairs.size() ); + timing_stats_.fk_us += + static_cast( std::chrono::duration_cast( t_fk - t0 ).count() ) / + 1000.0; + timing_stats_.placement_us += + static_cast( + std::chrono::duration_cast( t_placement - t_fk ).count() ) / + 1000.0; + timing_stats_.distance_us += + static_cast( + std::chrono::duration_cast( t_distance - t_placement ).count() ) / + 1000.0; + timing_stats_.total_us += + static_cast( + std::chrono::duration_cast( t_end - t0 ).count() ) / + 1000.0; + timing_stats_.num_safety_zone_pairs += safety_zone_indices.size(); + timing_stats_.count++; + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "[CC timing] avg total=%.1f µs (FK=%.1f, placement=%.1f, dist=%.1f, " + "jac=%.1f, grad=%.1f) pairs=%zu, zone_pairs=%.1f", + timing_stats_.total_us / timing_stats_.count, timing_stats_.fk_us / timing_stats_.count, + timing_stats_.placement_us / timing_stats_.count, + timing_stats_.distance_us / timing_stats_.count, + timing_stats_.jacobian_us / timing_stats_.count, + timing_stats_.gradient_us / timing_stats_.count, geom_model_.collisionPairs.size(), + static_cast( timing_stats_.num_safety_zone_pairs ) / timing_stats_.count ); #endif - last_collision_state_ = in_collision; + last_collision_result_ = result; if ( pub_debug_geometry_ ) publishMarkers(); - return in_collision; + else if ( pub_collision_distances_ ) + publishMinimalMarkers(); + return result; } void CollisionChecker::updateDoDebugVisualization( const bool pub_debug_geometry ) @@ -260,13 +477,204 @@ void CollisionChecker::updateCollisionCacheEpsilon( const double epsilon ) collision_cache_epsilon_ = epsilon; } +void CollisionChecker::setBroadphase( bool enable ) { use_broadphase_ = enable; } + +bool CollisionChecker::isBroadphaseEnabled() const { return use_broadphase_; } + +void CollisionChecker::updatePublishCollisionDistances( bool enable ) +{ + pub_collision_distances_ = enable; + if ( enable && !rt_markers_pub_ ) { + auto pub = node_->create_publisher( + "~/debug_collision_geometry", 1 ); + rt_markers_pub_ = + std::make_shared>( + pub ); + } +} + +void CollisionChecker::publishMinimalMarkers() +{ + if ( !rt_markers_pub_ || !rt_markers_pub_->trylock() ) + return; + + auto &arr = rt_markers_pub_->msg_; + arr.markers.clear(); + + const rclcpp::Time now = node_->now(); + + // Delete all previous markers + visualization_msgs::msg::Marker delete_all; + delete_all.header.frame_id = "base_link"; + delete_all.header.stamp = now; + delete_all.action = visualization_msgs::msg::Marker::DELETEALL; + arr.markers.push_back( std::move( delete_all ) ); + + const auto &result = last_collision_result_; + if ( result.safety_zone_pairs.empty() && !result.in_collision ) { + rt_markers_pub_->unlockAndPublish(); + return; + } + + // Check if directional info is available + const bool have_dir_info = !viz_directional_derivatives_.empty() && + viz_directional_derivatives_.size() == geom_model_.collisionPairs.size(); + + // Build LINE_LIST markers for safety zone and collision pairs + auto make_line_marker = [&]( const std::string &ns, int id, double thickness ) { + visualization_msgs::msg::Marker m; + m.header.frame_id = "base_link"; + m.header.stamp = now; + m.ns = ns; + m.id = id; + m.type = visualization_msgs::msg::Marker::LINE_LIST; + m.action = visualization_msgs::msg::Marker::ADD; + m.scale.x = thickness; + m.lifetime = rclcpp::Duration::from_seconds( 0.0 ); + m.pose.orientation.w = 1.0; + return m; + }; + + visualization_msgs::msg::Marker lines_zone = + make_line_marker( "distance_lines_safety_zone", 0, 0.005 ); + visualization_msgs::msg::Marker lines_coll = + make_line_marker( "distance_lines_collision", 0, 0.006 ); + + std_msgs::msg::ColorRGBA bright_red; + bright_red.r = 1.0f; + bright_red.g = 0.0f; + bright_red.b = 0.0f; + bright_red.a = 1.0f; + lines_coll.color = bright_red; + + for ( const auto &pair : result.safety_zone_pairs ) { + const auto &dres = geom_data_.distanceResults[pair.pair_index]; + if ( dres.nearest_points[0].hasNaN() || dres.nearest_points[1].hasNaN() || + !dres.nearest_points[0].allFinite() || !dres.nearest_points[1].allFinite() ) + continue; + + geometry_msgs::msg::Point pA, pB; + pA.x = dres.nearest_points[0][0]; + pA.y = dres.nearest_points[0][1]; + pA.z = dres.nearest_points[0][2]; + pB.x = dres.nearest_points[1][0]; + pB.y = dres.nearest_points[1][1]; + pB.z = dres.nearest_points[1][2]; + + if ( pair.distance <= collision_padding_ ) { + lines_coll.points.push_back( pA ); + lines_coll.points.push_back( pB ); + } else { + std_msgs::msg::ColorRGBA color; + if ( have_dir_info && !std::isnan( viz_directional_derivatives_[pair.pair_index] ) ) { + if ( viz_directional_derivatives_[pair.pair_index] >= 0.0 ) { + color.r = 0.0f; + color.g = 1.0f; + color.b = 0.0f; + color.a = 1.0f; + } else { + color.r = 1.0f; + color.g = 0.0f; + color.b = 0.0f; + color.a = 1.0f; + } + } else { + color.r = 1.0f; + color.g = 0.8f; + color.b = 0.0f; + color.a = 0.9f; + } + lines_zone.points.push_back( pA ); + lines_zone.colors.push_back( color ); + lines_zone.points.push_back( pB ); + lines_zone.colors.push_back( color ); + } + } + + arr.markers.push_back( std::move( lines_zone ) ); + arr.markers.push_back( std::move( lines_coll ) ); + + rt_markers_pub_->unlockAndPublish(); +} + +Eigen::VectorXd CollisionChecker::computePairGradient( std::size_t pair_k ) +{ + Eigen::VectorXd grad = Eigen::VectorXd::Zero( model_.nv ); + + const auto &dres = geom_data_.distanceResults[pair_k]; + + // Validate nearest points + if ( dres.nearest_points[0].hasNaN() || dres.nearest_points[1].hasNaN() || + !dres.nearest_points[0].allFinite() || !dres.nearest_points[1].allFinite() ) { + return grad; // zero gradient = no directional preference (safe fallback) + } + + const auto &cp = geom_model_.collisionPairs[pair_k]; + const pinocchio::JointIndex j1 = geom_model_.geometryObjects[cp.first].parentJoint; + const pinocchio::JointIndex j2 = geom_model_.geometryObjects[cp.second].parentJoint; + + // Get 6×nv Jacobians in LOCAL_WORLD_ALIGNED frame (reuse pre-allocated workspace) + J1_workspace_.setZero(); + J2_workspace_.setZero(); + pinocchio::getJointJacobian( model_, data_, j1, pinocchio::LOCAL_WORLD_ALIGNED, J1_workspace_ ); + pinocchio::getJointJacobian( model_, data_, j2, pinocchio::LOCAL_WORLD_ALIGNED, J2_workspace_ ); + + // Nearest points in world frame + const Eigen::Vector3d p1 = dres.nearest_points[0]; + const Eigen::Vector3d p2 = dres.nearest_points[1]; + + // Offsets from joint origins to nearest points + const Eigen::Vector3d r1 = p1 - data_.oMi[j1].translation(); + const Eigen::Vector3d r2 = p2 - data_.oMi[j2].translation(); + + // Direction vector: from p1 to p2 (positive distance direction) + const Eigen::Vector3d diff = p2 - p1; + const double dist_norm = diff.norm(); + if ( dist_norm < 1e-12 ) { + return grad; // points coincide, gradient undefined + } + const Eigen::Vector3d n = diff / dist_norm; + + // Point Jacobians and gradient: dd/dv = n^T * (Jp2 - Jp1) + // where Jp = J_linear - skew(r) * J_angular + const Eigen::MatrixXd Jp1 = + J1_workspace_.topRows( 3 ) - pinocchio::skew( r1 ) * J1_workspace_.bottomRows( 3 ); + const Eigen::MatrixXd Jp2 = + J2_workspace_.topRows( 3 ) - pinocchio::skew( r2 ) * J2_workspace_.bottomRows( 3 ); + + grad = ( n.transpose() * ( Jp2 - Jp1 ) ).transpose(); + return grad; +} + +int CollisionChecker::getJointVelocityIndex( const std::string &joint_name ) const +{ + const auto it = name_to_id_.find( joint_name ); + if ( it == name_to_id_.end() ) + return -1; + return model_.idx_vs[it->second]; +} + +int CollisionChecker::getNv() const { return model_.nv; } + +std::size_t CollisionChecker::getNumCollisionPairs() const +{ + return geom_model_.collisionPairs.size(); +} + +void CollisionChecker::setDirectionalInfo( const std::vector &derivatives, + double safety_zone_threshold ) +{ + viz_directional_derivatives_ = derivatives; + viz_safety_zone_threshold_ = safety_zone_threshold; +} + void CollisionChecker::publishMarkers() const { if ( !markers_pub_ ) return; visualization_msgs::msg::MarkerArray arr; - arr.markers.reserve( geom_model_.geometryObjects.size() + 1 ); + arr.markers.reserve( geom_model_.geometryObjects.size() + geom_model_.collisionPairs.size() + 4 ); const rclcpp::Time now = node_->now(); // Build a quick lookup of objects involved in "distance<=0" for coloring @@ -290,7 +698,7 @@ void CollisionChecker::publishMarkers() const } } - // 1) Geometry markers (unchanged) + // 1) Geometry markers for ( std::size_t i = 0; i < geom_model_.geometryObjects.size(); ++i ) { const auto &go = geom_model_.geometryObjects[i]; const auto &M = geom_data_.oMg[i]; @@ -359,55 +767,145 @@ void CollisionChecker::publishMarkers() const arr.markers.push_back( std::move( m ) ); } - // 2) Lines between nearest points for ALL pairs (distance visualization) - { - visualization_msgs::msg::Marker lines; - lines.header.frame_id = "base_link"; - lines.header.stamp = now; - lines.ns = "nearest_pairs"; - lines.id = 999999; // single marker containing all segments - lines.type = visualization_msgs::msg::Marker::LINE_LIST; - lines.action = visualization_msgs::msg::Marker::ADD; - lines.scale.x = 0.004; // line thickness (m) - lines.color.r = 1.0f; - lines.color.g = 0.8f; - lines.color.b = 0.0f; - lines.color.a = 0.9f; - lines.lifetime = rclcpp::Duration::from_seconds( 0.0 ); - - lines.points.reserve( geom_model_.collisionPairs.size() * 2 ); + // Helper to check if nearest points are valid + auto valid_nearest_points = []( const hpp::fcl::DistanceResult &dres ) -> bool { + return !dres.nearest_points[0].hasNaN() && !dres.nearest_points[1].hasNaN() && + dres.nearest_points[0].allFinite() && dres.nearest_points[1].allFinite(); + }; - for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) { - const auto &dres = geom_data_.distanceResults[k]; + // Helper to create a line marker between nearest points of a pair + auto make_line_points = []( const hpp::fcl::DistanceResult &dres ) + -> std::pair { + geometry_msgs::msg::Point pA, pB; + pA.x = dres.nearest_points[0][0]; + pA.y = dres.nearest_points[0][1]; + pA.z = dres.nearest_points[0][2]; + pB.x = dres.nearest_points[1][0]; + pB.y = dres.nearest_points[1][1]; + pB.z = dres.nearest_points[1][2]; + return { pA, pB }; + }; - // check if nearest points are valid (no nans or infs) - if ( dres.nearest_points[0].hasNaN() || dres.nearest_points[1].hasNaN() || - !dres.nearest_points[0].allFinite() || !dres.nearest_points[1].allFinite() ) { - RCLCPP_WARN_STREAM( - node_->get_logger(), - "Skipping invalid nearest points for pair " - << k << " (distance=" << dres.min_distance << "names " - << geom_model_.geometryObjects[geom_model_.collisionPairs[k].first].name << " - " - << geom_model_.geometryObjects[geom_model_.collisionPairs[k].second].name << ")" ); - continue; - } + // Check if directional info is available for a given pair + const bool have_dir_info = !viz_directional_derivatives_.empty() && + viz_directional_derivatives_.size() == geom_model_.collisionPairs.size(); + + // 2) Distance lines — separated into namespaces by category + // Initialize LINE_LIST markers for each category + auto make_line_marker = [&]( const std::string &ns, int id, double thickness ) { + visualization_msgs::msg::Marker m; + m.header.frame_id = "base_link"; + m.header.stamp = now; + m.ns = ns; + m.id = id; + m.type = visualization_msgs::msg::Marker::LINE_LIST; + m.action = visualization_msgs::msg::Marker::ADD; + m.scale.x = thickness; + m.lifetime = rclcpp::Duration::from_seconds( 0.0 ); + // pose defaults to identity + m.pose.orientation.w = 1.0; + return m; + }; - // dres.nearest_points[0] and [1] should be in the base_link frame (after placements) - geometry_msgs::msg::Point pA, pB; - pA.x = dres.nearest_points[0][0]; - pA.y = dres.nearest_points[0][1]; - pA.z = dres.nearest_points[0][2]; + // Lines for pairs outside safety zone (gray, thin) + visualization_msgs::msg::Marker lines_safe = make_line_marker( "distance_lines_safe", 0, 0.002 ); + // Lines for pairs in safety zone — per-point colors used + visualization_msgs::msg::Marker lines_zone = + make_line_marker( "distance_lines_safety_zone", 0, 0.005 ); + // Lines for pairs at/below collision padding (bright red, thick) + visualization_msgs::msg::Marker lines_coll = + make_line_marker( "distance_lines_collision", 0, 0.006 ); + + lines_safe.points.reserve( geom_model_.collisionPairs.size() * 2 ); + lines_zone.points.reserve( geom_model_.collisionPairs.size() * 2 ); + lines_zone.colors.reserve( geom_model_.collisionPairs.size() * 2 ); + lines_coll.points.reserve( geom_model_.collisionPairs.size() * 2 ); + + // Default colors + std_msgs::msg::ColorRGBA gray; + gray.r = 0.5f; + gray.g = 0.5f; + gray.b = 0.5f; + gray.a = 0.5f; + std_msgs::msg::ColorRGBA bright_red; + bright_red.r = 1.0f; + bright_red.g = 0.0f; + bright_red.b = 0.0f; + bright_red.a = 1.0f; + + lines_safe.color = gray; + lines_coll.color = bright_red; - pB.x = dres.nearest_points[1][0]; - pB.y = dres.nearest_points[1][1]; - pB.z = dres.nearest_points[1][2]; + for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) { + const auto &dres = geom_data_.distanceResults[k]; + if ( !valid_nearest_points( dres ) ) + continue; - lines.points.push_back( pA ); - lines.points.push_back( pB ); - } + auto [pA, pB] = make_line_points( dres ); - arr.markers.push_back( std::move( lines ) ); + if ( dres.min_distance <= collision_padding_ ) { + // Collision pair + lines_coll.points.push_back( pA ); + lines_coll.points.push_back( pB ); + } else if ( viz_safety_zone_threshold_ > 0.0 && dres.min_distance < viz_safety_zone_threshold_ ) { + // Safety zone pair — color by directional derivative + std_msgs::msg::ColorRGBA color; + if ( have_dir_info && !std::isnan( viz_directional_derivatives_[k] ) ) { + if ( viz_directional_derivatives_[k] >= 0.0 ) { + // Moving away: green + color.r = 0.0f; + color.g = 1.0f; + color.b = 0.0f; + color.a = 1.0f; + } else { + // Moving closer: red + color.r = 1.0f; + color.g = 0.0f; + color.b = 0.0f; + color.a = 1.0f; + } + } else { + // No directional info: yellow + color.r = 1.0f; + color.g = 0.8f; + color.b = 0.0f; + color.a = 0.9f; + } + lines_zone.points.push_back( pA ); + lines_zone.colors.push_back( color ); + lines_zone.points.push_back( pB ); + lines_zone.colors.push_back( color ); + } else { + // Safe pair (outside safety zone) + lines_safe.points.push_back( pA ); + lines_safe.points.push_back( pB ); + } } + arr.markers.push_back( std::move( lines_safe ) ); + arr.markers.push_back( std::move( lines_zone ) ); + arr.markers.push_back( std::move( lines_coll ) ); + markers_pub_->publish( arr ); } + +double CollisionChecker::computeManipulability( const std::string &ee_frame_name ) +{ + if ( !model_.existFrame( ee_frame_name ) ) { + return 0.0; + } + const auto frame_id = model_.getFrameId( ee_frame_name ); + + // Compute frame Jacobian in LOCAL_WORLD_ALIGNED frame + Eigen::MatrixXd J = Eigen::MatrixXd::Zero( 6, model_.nv ); + // data_ has been updated by checkCollision (FK + computeJointJacobians already called) + pinocchio::getFrameJacobian( model_, data_, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J ); + + // Yoshikawa manipulability: w = sqrt(det(J * J^T)) + const Eigen::MatrixXd JJt = J * J.transpose(); // 6x6 + const double det = JJt.determinant(); + if ( det <= 0.0 ) { + return 0.0; + } + return std::sqrt( det ); +} diff --git a/hector_ros_controllers/src/safety_position_controller.cpp b/hector_ros_controllers/src/safety_position_controller.cpp index 698996b..904e90e 100644 --- a/hector_ros_controllers/src/safety_position_controller.cpp +++ b/hector_ros_controllers/src/safety_position_controller.cpp @@ -1,5 +1,6 @@ #include "safety_position_controller/safety_position_controller.hpp" +#include #include #include @@ -54,6 +55,7 @@ controller_interface::CallbackReturn SafetyPositionController::on_init() if ( params_.check_self_collisions ) { collision_checker_ = std::make_unique( node, params_.collision_padding, + params_.collision_cache_epsilon, params_.debug_visualize_collisions ); } @@ -119,6 +121,12 @@ controller_interface::CallbackReturn SafetyPositionController::on_init() node->create_publisher( "~/status", qos_latched ); + // Periodic status publishing + if ( params_.status_publish_rate > 0.0 ) { + const auto period = std::chrono::duration( 1.0 / params_.status_publish_rate ); + status_timer_ = node->create_wall_timer( period, [this]() { publish_status(); } ); + } + // Debug joint state publishers (dynamically reconfigurable) param_subscriber_ = std::make_shared( node ); update_debug_publishers( params_.publish_debug_joint_states ); @@ -150,7 +158,22 @@ SafetyPositionController::on_configure( const rclcpp_lifecycle::State & ) } if ( collision_checker_ ) { - collision_checker_->initFromXml( this->get_robot_description(), srdf_, params_.joints, false ); + collision_checker_->setBroadphase( params_.use_broadphase ); + if ( !collision_checker_->initFromXml( this->get_robot_description(), srdf_, params_.joints, + false ) ) { + RCLCPP_ERROR( node->get_logger(), "Failed to initialize collision checker from URDF/SRDF." ); + return controller_interface::CallbackReturn::ERROR; + } + + // Build velocity-space index mapping for directional collision scaling + joint_v_index_.resize( params_.joints.size(), -1 ); + for ( size_t i = 0; i < params_.joints.size(); ++i ) { + joint_v_index_[i] = collision_checker_->getJointVelocityIndex( params_.joints[i] ); + if ( joint_v_index_[i] < 0 ) { + RCLCPP_WARN( node->get_logger(), "Joint '%s' not found in collision model velocity space", + params_.joints[i].c_str() ); + } + } } const size_t n = params_.joints.size(); @@ -172,20 +195,40 @@ SafetyPositionController::on_configure( const rclcpp_lifecycle::State & ) controller_interface::CallbackReturn SafetyPositionController::on_activate( const rclcpp_lifecycle::State & ) { - on_hold_ = false; - // reset reference interfaces for ( auto &ref : reference_interfaces_ ) { ref = std::numeric_limits::quiet_NaN(); } // update params in case they changed param_listener_->try_update_params( params_ ); - params_.block_if_too_far = params_.check_self_collisions ? true : params_.block_if_too_far; + if ( params_.check_self_collisions && params_.collision_safety_zone <= params_.collision_padding ) { + RCLCPP_ERROR( get_node()->get_logger(), + "collision_safety_zone (%.4f) must be > collision_padding (%.4f)", + params_.collision_safety_zone, params_.collision_padding ); + return controller_interface::CallbackReturn::ERROR; + } + last_min_distance_ = std::numeric_limits::max(); + last_safety_zone_pairs_.clear(); + last_distance_scale_ = 1.0; + last_effective_scale_ = 1.0; + last_worst_directional_derivative_ = std::numeric_limits::max(); if ( collision_checker_ ) { collision_checker_->updateCollisionPadding( params_.collision_padding ); collision_checker_->updateCollisionCacheEpsilon( params_.collision_cache_epsilon ); collision_checker_->updateDoDebugVisualization( params_.debug_visualize_collisions ); + collision_checker_->updatePublishCollisionDistances( params_.publish_collision_distances ); } + RCLCPP_INFO( get_node()->get_logger(), + "SafetyPositionController config: joints=%zu, collisions=%s, broadphase=%s, " + "padding=%.4f, safety_zone=%.4f, cache_eps=%.1e, directional=%s, debug_viz=%s, " + "publish_distances=%s", + params_.joints.size(), params_.check_self_collisions ? "ON" : "OFF", + params_.use_broadphase ? "ON" : "OFF", params_.collision_padding, + params_.collision_safety_zone, params_.collision_cache_epsilon, + params_.directional_collision_scaling ? "ON" : "OFF", + params_.debug_visualize_collisions ? "ON" : "OFF", + params_.publish_collision_distances ? "ON" : "OFF" ); + // compute max allowed distance per cycle for ( size_t n = 0; n < params_.joints.size(); ++n ) { max_allowed_distance_per_cycle_[n] = @@ -218,8 +261,8 @@ SafetyPositionController::on_activate( const rclcpp_lifecycle::State & ) } } ); - if ( is_chained_ ) { - // Non-chained command subscriber (RT buffer) + if ( !is_chained_ ) { + // Non-chained mode: re-create command subscriber (destroyed in on_deactivate) joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this]( const CmdType::SharedPtr msg ) { rt_command_ptr_.writeFromNonRT( msg ); } ); @@ -368,20 +411,56 @@ SafetyPositionController::update_and_write_commands( const rclcpp::Time &, const // resolve continuous joints & enforce limits enforce_limits(); - // make sure movement is not too large - if ( params_.block_if_too_far ) { - block_if_too_far(); + + // ---- Distance-based velocity scaling ---- + const bool bypass_active = safety_bypass_active_.load( std::memory_order_relaxed ); + const bool collision_checks_active = + !bypass_active && params_.check_self_collisions && collision_checker_; + + // Compute velocity scale factor from previous cycle's min distance + double distance_scale = 1.0; + if ( collision_checks_active ) { + const auto &d = last_min_distance_; + const auto &d_pad = params_.collision_padding; + const auto &d_zone = params_.collision_safety_zone; + + if ( d <= d_pad ) { + distance_scale = 0.0; + } else if ( d < d_zone ) { + distance_scale = ( d - d_pad ) / ( d_zone - d_pad ); + } + // else: distance_scale remains 1.0 (full speed) + } + + // Always apply velocity-limited stepping when collision checks are active + double effective_scale = distance_scale; + double worst_directional_derivative = std::numeric_limits::max(); + if ( collision_checks_active ) { + // Directional scaling: only slow down if moving toward any collision in safety zone + if ( distance_scale < 1.0 && params_.directional_collision_scaling && + !last_safety_zone_pairs_.empty() ) { + for ( const auto &pair_info : last_safety_zone_pairs_ ) { + const double dir_deriv = compute_directional_derivative( pair_info.gradient ); + worst_directional_derivative = std::min( worst_directional_derivative, dir_deriv ); + } + if ( worst_directional_derivative >= 0.0 ) { + // ALL safety-zone pairs say motion moves away or is tangent → allow full speed + effective_scale = 1.0; + } + // else: at least one pair worsens → keep distance_scale + } + apply_velocity_limits( effective_scale ); } + last_distance_scale_ = distance_scale; + last_effective_scale_ = effective_scale; + last_worst_directional_derivative_ = worst_directional_derivative; + if ( params_.set_current_limits ) { success &= write_current_limits(); } - // check collisions with the new commands (skip if bypass is active) - const bool bypass_active = safety_bypass_active_.load( std::memory_order_relaxed ); - const bool skip_collision_check = - bypass_active || !params_.check_self_collisions || !collision_checker_; - - if ( skip_collision_check ) { + // ---- Collision check ---- + if ( !collision_checks_active ) { if ( bypass_active && params_.check_self_collisions && collision_checker_ ) { RCLCPP_DEBUG_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), throttle_logging_msg, "Safety bypass active: skipping collision check" ); @@ -399,23 +478,51 @@ SafetyPositionController::update_and_write_commands( const rclcpp::Time &, const } } for ( size_t i = 0; i < n; ++i ) { cc_positions_[params_.joints[i]] = cmd_positions_[i]; } - if ( success_cc_setup && !collision_checker_->checkCollision( cc_positions_ ) ) { - // write commands if no collision detected - write_position_commands( cmd_positions_ ); - } else { - if ( !success_cc_setup ) { - RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), - throttle_logging_msg, "Failed to setup collision checking." ); + + if ( success_cc_setup ) { + // Request gradient computation when inside safety zone (or when directional scaling is on) + const double gradient_threshold = ( params_.directional_collision_scaling && + last_min_distance_ < params_.collision_safety_zone ) + ? params_.collision_safety_zone + : 0.0; + const auto cc_result = collision_checker_->checkCollision( cc_positions_, gradient_threshold ); + last_min_distance_ = cc_result.min_distance; + last_safety_zone_pairs_ = cc_result.safety_zone_pairs; + + // Provide directional derivative info to collision checker for visualization + if ( params_.debug_visualize_collisions || params_.publish_collision_distances ) { + const std::size_t num_pairs = collision_checker_->getNumCollisionPairs(); + std::vector per_pair_dir_derivs( num_pairs, std::numeric_limits::quiet_NaN() ); + for ( const auto &pi : last_safety_zone_pairs_ ) { + if ( pi.pair_index < num_pairs ) { + per_pair_dir_derivs[pi.pair_index] = compute_directional_derivative( pi.gradient ); + } + } + collision_checker_->setDirectionalInfo( per_pair_dir_derivs, params_.collision_safety_zone ); + } + + if ( !cc_result.in_collision ) { + write_position_commands( cmd_positions_ ); } else { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), throttle_logging_msg, - "Collision detected! Holding current positions." ); + "Collision detected (min_dist=%.4f)! Holding current positions.", + cc_result.min_distance ); + write_position_commands( current_positions_ ); } + } else { + RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), + throttle_logging_msg, "Failed to setup collision checking." ); write_position_commands( current_positions_ ); - // success = false; // make parent controllers unload if desired } } + // Compute manipulability index if collision checker is available (FK already done) + if ( collision_checker_ && !params_.manipulability_ee_frame.empty() ) { + last_manipulability_ = + collision_checker_->computeManipulability( params_.manipulability_ee_frame ); + } + return success ? controller_interface::return_type::OK : controller_interface::return_type::ERROR; } @@ -481,30 +588,60 @@ void SafetyPositionController::enforce_limits() } } -void SafetyPositionController::block_if_too_far() +void SafetyPositionController::apply_velocity_limits( const double distance_scale ) { - // check if any joint command is too far from the current position + // distance_scale: 1.0 → full speed, 0.0 → stop; can be used to smoothly reduce speed when close to collisions + const double clamped_scale = std::clamp( distance_scale, 0.0, 1.0 ); for ( size_t i = 0; i < params_.joints.size(); ++i ) { if ( !std::isnan( velocity_limits_[i] ) ) { // shortest signed distance from current -> command - const double diff = get_signed_distance( current_positions_[i], cmd_positions_[i] ); - const double max_step = max_allowed_distance_per_cycle_[i]; + const double diff = ( kinds_[i] == JointType::CONTINUOUS ) + ? get_signed_distance( current_positions_[i], cmd_positions_[i] ) + : ( cmd_positions_[i] - current_positions_[i] ); + const double max_step = max_allowed_distance_per_cycle_[i] * clamped_scale; + + // RCLCPP_INFO( get_node()->get_logger(), + // "Joint '%s': distance=%.4f, max_step=%.4f, scale=%.3f", + // params_.joints[i].c_str(), diff, max_step, clamped_scale ); if ( std::abs( diff ) > max_step ) { RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), throttle_logging_msg, - "Joint '%s' command is too far (|diff|=%.3f > allowed=%.3f). " - "Limiting step. [current=%.3f, cmd=%.3f]", - params_.joints[i].c_str(), std::abs( diff ), max_step, + "Joint '%s' step limited (|diff|=%.4f > allowed=%.4f, " + "dist_scale=%.3f). [current=%.3f, cmd=%.3f]", + params_.joints[i].c_str(), std::abs( diff ), max_step, clamped_scale, current_positions_[i], cmd_positions_[i] ); - // Limit the commanded position to a max step in the direction of diff - cmd_positions_[i] = current_positions_[i] + std::copysign( max_step, diff ); + if ( max_step <= 0.0 ) { + cmd_positions_[i] = current_positions_[i]; // zero speed = hold + } else { + cmd_positions_[i] = current_positions_[i] + std::copysign( max_step, diff ); + } } } } } +double SafetyPositionController::compute_directional_derivative( const Eigen::VectorXd &gradient ) const +{ + double dot_product = 0.0; + for ( size_t i = 0; i < params_.joints.size(); ++i ) { + if ( joint_v_index_[i] < 0 || joint_v_index_[i] >= gradient.size() ) + continue; + + // For continuous joints, use shortest-path angular distance + double delta_q_i; + if ( kinds_[i] == JointType::CONTINUOUS ) { + delta_q_i = get_signed_distance( current_positions_[i], cmd_positions_[i] ); + } else { + delta_q_i = cmd_positions_[i] - current_positions_[i]; + } + + dot_product += gradient[joint_v_index_[i]] * delta_q_i; + } + return dot_product; +} + bool SafetyPositionController::write_current_limits() { bool success = true; @@ -739,6 +876,33 @@ void SafetyPositionController::publish_status() msg.collision_check_enabled = params_.check_self_collisions; msg.estop_engaged = estop_engaged_.load( std::memory_order_relaxed ); msg.position_limits_enforced = params_.enforce_position_limits; + msg.min_collision_distance = last_min_distance_; + msg.distance_scale = last_distance_scale_; + msg.effective_scale = last_effective_scale_; + msg.worst_directional_derivative = last_worst_directional_derivative_; + msg.num_pairs_in_safety_zone = static_cast( last_safety_zone_pairs_.size() ); + msg.manipulability = last_manipulability_; + + // Populate active current limits per joint + if ( params_.set_current_limits ) { + msg.joint_names = params_.joints; + msg.current_limits.reserve( params_.joints.size() ); + for ( size_t i = 0; i < params_.joints.size(); ++i ) { + const auto &limit = in_compliant_mode_ + ? params_.current_limits.joints_map[params_.joints[i]].compliant_limit + : params_.current_limits.joints_map[params_.joints[i]].stiff_limit; + msg.current_limits.push_back( limit ); + } + } else { + // Simulate current limits for testing: start at 10A for joint 1, subtract 1A every 2 joints + msg.joint_names = params_.joints; + msg.current_limits.reserve( params_.joints.size() ); + for ( size_t i = 0; i < params_.joints.size(); ++i ) { + const double simulated_limit = 10.0 - static_cast( i / 2 ); + msg.current_limits.push_back( simulated_limit ); + } + } + status_pub_->publish( msg ); } diff --git a/hector_ros_controllers/test/benchmark_collision_checker.cpp b/hector_ros_controllers/test/benchmark_collision_checker.cpp new file mode 100644 index 0000000..2106c41 --- /dev/null +++ b/hector_ros_controllers/test/benchmark_collision_checker.cpp @@ -0,0 +1,342 @@ +// +// Google Benchmark for CollisionChecker using the real Athena robot. +// Compares single-pass (debug_viz ON) vs two-pass (debug_viz OFF) performance. +// +#include + +#include +#include +#include + +#include "safety_position_controller/collision_checker.hpp" + +#include +#include +#include + +#include +#include +#include + +// CollisionChecker is in the global namespace + +namespace +{ + +std::string loadFile( const std::string &filename ) +{ + const std::string path = + ament_index_cpp::get_package_share_directory( "hector_ros_controllers" ) + "/test/config/" + + filename; + std::ifstream ifs( path ); + if ( !ifs.is_open() ) { + throw std::runtime_error( "Cannot open file: " + path ); + } + return std::string( std::istreambuf_iterator( ifs ), std::istreambuf_iterator() ); +} + +// Athena arm joints +const std::vector ARM_JOINTS = { "arm_joint_1", "arm_joint_2", "arm_joint_3", + "arm_joint_4", "arm_joint_5", "arm_joint_6", + "arm_joint_7" }; + +// Predefined arm poses from SRDF +struct ArmPose { + std::string name; + std::unordered_map positions; +}; + +const std::vector POSES = { + { "folded", + { { "arm_joint_1", 3.14 }, + { "arm_joint_2", 1.65 }, + { "arm_joint_3", 0.0 }, + { "arm_joint_4", -1.51 }, + { "arm_joint_5", 0.0 }, + { "arm_joint_6", -1.38 }, + { "arm_joint_7", 0.0 } } }, + { "front", + { { "arm_joint_1", 0.0 }, + { "arm_joint_2", 0.4 }, + { "arm_joint_3", 0.0 }, + { "arm_joint_4", -0.4 }, + { "arm_joint_5", 3.14 }, + { "arm_joint_6", 0.9 }, + { "arm_joint_7", 0.0 } } }, + { "zero", + { { "arm_joint_1", 0.0 }, + { "arm_joint_2", 0.0 }, + { "arm_joint_3", 0.0 }, + { "arm_joint_4", 0.0 }, + { "arm_joint_5", 0.0 }, + { "arm_joint_6", 0.0 }, + { "arm_joint_7", 0.0 } } }, +}; + +/// Shared state for benchmarks (avoids re-parsing URDF per iteration) +struct BenchmarkState { + rclcpp_lifecycle::LifecycleNode::SharedPtr node; + std::string urdf_xml; + std::string srdf_xml; + pinocchio::Model model; + + // Pre-generated random configs (Pinocchio q-space, handles cos/sin for continuous joints) + std::vector> random_configs; + + static BenchmarkState &instance() + { + static BenchmarkState s; + return s; + } + + void init() + { + if ( node ) + return; // already initialized + rclcpp::NodeOptions opts; + node = std::make_shared( "benchmark_collision_checker", opts ); + urdf_xml = loadFile( "athena.urdf" ); + srdf_xml = loadFile( "athena.srdf" ); + pinocchio::urdf::buildModelFromXML( urdf_xml, model ); + + // Generate random configs + std::mt19937 rng( 42 ); + std::uniform_real_distribution dist( -M_PI, M_PI ); + random_configs.resize( 500 ); + for ( auto &config : random_configs ) { + for ( const auto &joint : ARM_JOINTS ) { config[joint] = dist( rng ); } + } + } + + std::unique_ptr makeChecker( double padding, bool debug_viz ) + { + auto checker = std::make_unique( node, padding, 0.0, debug_viz ); + checker->setBroadphase( false ); // brute-force baseline + bool ok = checker->initFromXml( urdf_xml, srdf_xml, ARM_JOINTS ); + if ( !ok ) + throw std::runtime_error( "Failed to init CollisionChecker" ); + return checker; + } + + std::unique_ptr makeBroadphaseChecker( double padding, bool debug_viz ) + { + auto checker = std::make_unique( node, padding, 0.0, debug_viz ); + checker->setBroadphase( true ); + bool ok = checker->initFromXml( urdf_xml, srdf_xml, ARM_JOINTS ); + if ( !ok ) + throw std::runtime_error( "Failed to init broadphase CollisionChecker" ); + return checker; + } +}; + +// ---- Benchmark: Two-pass (debug_viz OFF) with random configs ---- +void BM_CollisionChecker_TwoPass( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeChecker( padding, /*debug_viz=*/false ); + + std::size_t i = 0; + for ( auto _ : state ) { + const auto &config = bs.random_configs[i % bs.random_configs.size()]; + auto result = checker->checkCollision( config, safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); + state.counters["pairs"] = static_cast( checker->getNumCollisionPairs() ); +} +BENCHMARK( BM_CollisionChecker_TwoPass )->Unit( benchmark::kMicrosecond ); + +// ---- Benchmark: Single-pass (debug_viz ON) with random configs ---- +void BM_CollisionChecker_SinglePass( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeChecker( padding, /*debug_viz=*/true ); + + std::size_t i = 0; + for ( auto _ : state ) { + const auto &config = bs.random_configs[i % bs.random_configs.size()]; + auto result = checker->checkCollision( config, safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); + state.counters["pairs"] = static_cast( checker->getNumCollisionPairs() ); +} +BENCHMARK( BM_CollisionChecker_SinglePass )->Unit( benchmark::kMicrosecond ); + +// Generate configs with small random perturbations around a base pose (defeats cache) +std::vector> +generatePerturbedConfigs( const std::unordered_map &base, std::size_t count ) +{ + std::mt19937 rng( 123 ); + std::uniform_real_distribution perturb( -0.1, 0.1 ); + std::vector> configs( count ); + for ( auto &config : configs ) { + for ( const auto &[name, val] : base ) { config[name] = val + perturb( rng ); } + } + return configs; +} + +// ---- Benchmark: Two-pass with perturbed folded poses (near collision) ---- +void BM_CollisionChecker_TwoPass_Folded( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeChecker( padding, /*debug_viz=*/false ); + auto configs = generatePerturbedConfigs( POSES[0].positions, 500 ); + + std::size_t i = 0; + for ( auto _ : state ) { + auto result = checker->checkCollision( configs[i % configs.size()], safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); +} +BENCHMARK( BM_CollisionChecker_TwoPass_Folded )->Unit( benchmark::kMicrosecond ); + +// ---- Benchmark: Single-pass with perturbed folded poses (near collision) ---- +void BM_CollisionChecker_SinglePass_Folded( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeChecker( padding, /*debug_viz=*/true ); + auto configs = generatePerturbedConfigs( POSES[0].positions, 500 ); + + std::size_t i = 0; + for ( auto _ : state ) { + auto result = checker->checkCollision( configs[i % configs.size()], safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); +} +BENCHMARK( BM_CollisionChecker_SinglePass_Folded )->Unit( benchmark::kMicrosecond ); + +// ---- Benchmark: Two-pass, no safety zone (distance-only, no gradients) ---- +void BM_CollisionChecker_DistanceOnly( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + auto checker = bs.makeChecker( padding, /*debug_viz=*/false ); + + std::size_t i = 0; + for ( auto _ : state ) { + const auto &config = bs.random_configs[i % bs.random_configs.size()]; + auto result = checker->checkCollision( config, 0.0 ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); + state.counters["pairs"] = static_cast( checker->getNumCollisionPairs() ); +} +BENCHMARK( BM_CollisionChecker_DistanceOnly )->Unit( benchmark::kMicrosecond ); + +// ======== Broadphase variants ======== + +// ---- Benchmark: Broadphase two-pass (debug_viz OFF) with random configs ---- +void BM_Broadphase_TwoPass( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeBroadphaseChecker( padding, /*debug_viz=*/false ); + + std::size_t i = 0; + for ( auto _ : state ) { + const auto &config = bs.random_configs[i % bs.random_configs.size()]; + auto result = checker->checkCollision( config, safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); + state.counters["pairs"] = static_cast( checker->getNumCollisionPairs() ); +} +BENCHMARK( BM_Broadphase_TwoPass )->Unit( benchmark::kMicrosecond ); + +// ---- Benchmark: Broadphase single-pass (debug_viz ON) with random configs ---- +void BM_Broadphase_SinglePass( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeBroadphaseChecker( padding, /*debug_viz=*/true ); + + std::size_t i = 0; + for ( auto _ : state ) { + const auto &config = bs.random_configs[i % bs.random_configs.size()]; + auto result = checker->checkCollision( config, safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); + state.counters["pairs"] = static_cast( checker->getNumCollisionPairs() ); +} +BENCHMARK( BM_Broadphase_SinglePass )->Unit( benchmark::kMicrosecond ); + +// ---- Benchmark: Broadphase two-pass with perturbed folded poses (near collision) ---- +void BM_Broadphase_TwoPass_Folded( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + constexpr double safety_zone = 0.05; + auto checker = bs.makeBroadphaseChecker( padding, /*debug_viz=*/false ); + auto configs = generatePerturbedConfigs( POSES[0].positions, 500 ); + + std::size_t i = 0; + for ( auto _ : state ) { + auto result = checker->checkCollision( configs[i % configs.size()], safety_zone ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); +} +BENCHMARK( BM_Broadphase_TwoPass_Folded )->Unit( benchmark::kMicrosecond ); + +// ---- Benchmark: Broadphase distance-only (no gradients) ---- +void BM_Broadphase_DistanceOnly( benchmark::State &state ) +{ + auto &bs = BenchmarkState::instance(); + constexpr double padding = 0.01; + auto checker = bs.makeBroadphaseChecker( padding, /*debug_viz=*/false ); + + std::size_t i = 0; + for ( auto _ : state ) { + const auto &config = bs.random_configs[i % bs.random_configs.size()]; + auto result = checker->checkCollision( config, 0.0 ); + benchmark::DoNotOptimize( result ); + ++i; + } + state.SetItemsProcessed( static_cast( state.iterations() ) ); + state.counters["pairs"] = static_cast( checker->getNumCollisionPairs() ); +} +BENCHMARK( BM_Broadphase_DistanceOnly )->Unit( benchmark::kMicrosecond ); + +} // namespace + +int main( int argc, char **argv ) +{ + rclcpp::init( argc, argv ); + BenchmarkState::instance().init(); + + benchmark::Initialize( &argc, argv ); + benchmark::RunSpecifiedBenchmarks(); + benchmark::Shutdown(); + + BenchmarkState::instance().node.reset(); + rclcpp::shutdown(); + +#if defined( __GNUC__ ) + extern void __gcov_dump() __attribute__( ( weak ) ); + if ( __gcov_dump ) + __gcov_dump(); +#endif + _exit( 0 ); +} diff --git a/hector_ros_controllers/test/config/athena.srdf b/hector_ros_controllers/test/config/athena.srdf new file mode 100644 index 0000000..8a098cc --- /dev/null +++ b/hector_ros_controllers/test/config/athena.srdf @@ -0,0 +1,286 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hector_ros_controllers/test/config/athena.urdf b/hector_ros_controllers/test/config/athena.urdf new file mode 100644 index 0000000..a03cb26 --- /dev/null +++ b/hector_ros_controllers/test/config/athena.urdf @@ -0,0 +1,2270 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + /athena/front_lidar + 10 + front_lidar_laser_frame + 1 + True + + + + 100 + 1 + 0 + 6.283185307179586 + + + 360 + 1 + -0.12601277199399058 + 0.9637708129512688 + + + + 0.08 + 10.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + athena/front_wideangle/image_raw + front_wideangle_optical_frame + + athena/front_wideangle/camera_info + 4.1887902047863905 + + R8G8B8 + 3840 + 3032 + + + 0.01 + 100 + + + equidistant + 2.0943951023931953 + true + 512 + + + + + + + + true + true + 15 + /athena/front_rgbd/color/image_raw + front_rgbd_color_link + + /athena/front_rgbd/color/camera_info + front_rgbd_color_optical_frame + 1.2479104151759457 + 0.9896016858807849 + + R8G8B8 + 640 + 480 + + + 0.05 + 10 + + + + + + true + true + 15 + /athena/front_rgbd/depth/image_raw + front_rgbd_depth_link + + /athena/front_rgbd/depth/camera_info + front_rgbd_depth_optical_frame + 1.2479104151759457 + 0.9896016858807849 + + R_FLOAT32 + 640 + 480 + + + + 0.25 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + /athena/back_lidar + 10 + back_lidar_laser_frame + 1 + True + + + + 100 + 1 + 0 + 6.283185307179586 + + + 360 + 1 + -0.12601277199399058 + 0.9637708129512688 + + + + 0.08 + 10.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + athena/back_wideangle/image_raw + back_wideangle_optical_frame + + athena/back_wideangle/camera_info + 4.1887902047863905 + + R8G8B8 + 3840 + 3032 + + + 0.01 + 100 + + + equidistant + 2.0943951023931953 + true + 512 + + + + + + + + true + true + 15 + /athena/back_rgbd/color/image_raw + back_rgbd_color_link + + /athena/back_rgbd/color/camera_info + back_rgbd_color_optical_frame + 1.2479104151759457 + 0.9896016858807849 + + R8G8B8 + 640 + 480 + + + 0.05 + 10 + + + + + + true + true + 15 + /athena/back_rgbd/depth/image_raw + back_rgbd_depth_link + + /athena/back_rgbd/depth/camera_info + back_rgbd_depth_optical_frame + 1.2479104151759457 + 0.9896016858807849 + + R_FLOAT32 + 640 + 480 + + + + 0.25 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + 30 + athena/gripper_rgb/image_raw + gripper_rgb_optical_frame + + athena/gripper_rgb/camera_info + 1.085594794740473 + + R8G8B8 + 1640 + 1232 + + + 0.01 + 100 + + + + + + + + true + true + 15 + /athena/gripper_rgbd/color/image_raw + gripper_rgbd_color_frame + + /athena/gripper_rgbd/color/camera_info + gripper_rgbd_color_optical_frame + 1.0995574287564276 + 0.8726646259971648 + + R8G8B8 + 640 + 480 + + + 0.05 + 10 + + + + + + true + true + 15 + /athena/gripper_rgbd/depth/image_raw + gripper_rgbd_depth_frame + + /athena/gripper_rgbd/depth/camera_info + gripper_rgbd_depth_optical_frame + 1.0995574287564276 + 0.8726646259971648 + + R_FLOAT32 + 640 + 480 + + + + 0.15 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gripper_servo_joint + + gripper_joint_r2 + 0.909 + 0.0 + + + + gripper_joint_l2 + 0.909 + + + + gripper_joint_r1 + + + + gripper_joint_l1 + + + + + + + + + + + + + + + + + + + + + 30.0 + true + true + gripper_joint_link_l2 + /athena/force_torque_sensor/finger_l/wrench + + child + child_to_parent + + + + + + + + 30.0 + true + true + gripper_joint_link_r2 + /athena/force_torque_sensor/finger_r/wrench + + child + child_to_parent + + + + + + + + dynamixel_ros_control/DynamixelHardwareInterface + false + /dev/ttyUSB_manipulator_arm + 1000000 + true + false + + + 11 + 3.141592653589793 + 255 + extended_position + + + + + + + + transmission_interface/SimpleTransmission + + + 1.0 + 2.356194490192345 + + + + 12 + 0.24434609527920614 + 255 + + + + + + + + 13 + -0.7853981633974483 + 255 + extended_position + + + + + + + + 14 + 0 + 255 + 22 + + + + + + + + + 15 + -0.7853981633974483 + 255 + extended_position + + + + + + + + 16 + 0 + 255 + + + + + + + + 17 + -1.0471975511965976 + 255 + extended_position + + + + + + + + 18 + 255 + current_based_position + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + dynamixel_ros_control/DynamixelHardwareInterface + false + /dev/ttyUSB_flippers + 1000000 + true + false + + + 1 + + + + + + + + extended_position + + + hector_transmission_interface/AdjustableOffsetTransmission + + + -2.0 + 0.684 + + + + 2 + + + + + + + + extended_position + + + hector_transmission_interface/AdjustableOffsetTransmission + + + 2.0 + -1.177 + + + + 3 + + + + + + + + extended_position + + + hector_transmission_interface/AdjustableOffsetTransmission + + + 2.0 + -0.949 + + + + 4 + + + + + + + + extended_position + + + hector_transmission_interface/AdjustableOffsetTransmission + + + -2.0 + -0.77 + + + + + + + /home/aljoscha-schmidt/hector/install/athena_description/share/athena_description/config/athena_simulation_controllers.yaml + + athena + + /tf:=/athena/tf + /tf_static:=/athena/tf_static + + + + 20.0 + base_link + athena/odom + athena/odom_covariance + 3 + odom + athena/tf + 0.0 + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + Gazebo/Black + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + Gazebo/Black + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + Gazebo/Black + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + Gazebo/Black + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + Gazebo/Black + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + + + + + 1.0 + 0.6 + 100000.0 + 50.0 + 0.001 + 0.1 + + + + + Gazebo/Black + + + true + + + true + + + + + + main_track_left_link + + + flipper_fl_link + + + flipper_bl_link + + + main_track_right_link + + + flipper_fr_link + + + flipper_br_link + + 1.0 + 0.215 + 1.0 + + -1.5 + 1.5 + + + + -2.0 + 2.0 + + + 50 + athena/cmd_vel_out + athena/steering_efficiency + athena/odom_wheel + + odom + base_link + + + main_track_left_link + + -1.5 + 1.5 + + + + flipper_fl_link + + -1.5 + 1.5 + + + + flipper_bl_link + 3.141592653589793 0 0 + -1.5 + 1.5 + + + + main_track_right_link + + -1.5 + 1.5 + + + + flipper_fr_link + + -1.5 + 1.5 + + + + flipper_br_link + 3.141592653589793 0 0 + -1.5 + 1.5 + + + + + Gazebo/DarkGray + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + diff --git a/hector_ros_controllers/test/test_collision_checker.cpp b/hector_ros_controllers/test/test_collision_checker.cpp new file mode 100644 index 0000000..97a5603 --- /dev/null +++ b/hector_ros_controllers/test/test_collision_checker.cpp @@ -0,0 +1,900 @@ +// +// Standalone unit tests for CollisionChecker with broadphase distance computation. +// Verifies correctness against brute-force Pinocchio reference. +// +// NOTE on broadphase distance in penetration: +// When geometries are deeply penetrating (negative distances), the AABB tree's +// distance pruning may not find the most-negative pair because AABB overlap gives +// a distance of zero, making the pruning bound less conservative. This means the +// broadphase min_distance may be less negative than brute-force in penetration. +// For safety, this is fine: we only need collision detection (dist <= padding), +// not the exact penetration depth. For non-penetrating configurations, broadphase +// and brute-force should agree exactly. +// +#include +#include + +#include +#include +#include + +#include "safety_position_controller/collision_checker.hpp" + +#include +#include +#include +#include +#include +#include + +#include "pinocchio/collision/distance.hpp" + +#include +#include +#include +#include +#include +#include + +namespace +{ + +std::string loadUrdfFile( const std::string &filename ) +{ + const std::string path = + ament_index_cpp::get_package_share_directory( "hector_ros_controllers" ) + "/test/config/" + + filename; + std::ifstream ifs( path ); + if ( !ifs.is_open() ) { + throw std::runtime_error( "Cannot open test URDF file: " + path ); + } + return std::string( std::istreambuf_iterator( ifs ), std::istreambuf_iterator() ); +} + +/// Brute-force reference: compute minimum distance using pinocchio::computeDistances() +double bruteForceMinDistance( pinocchio::Model &model, pinocchio::Data &data, + pinocchio::GeometryModel &geom_model, + pinocchio::GeometryData &geom_data, const Eigen::VectorXd &q ) +{ + pinocchio::forwardKinematics( model, data, q ); + pinocchio::updateGeometryPlacements( model, data, geom_model, geom_data ); + pinocchio::computeDistances( geom_model, geom_data ); + + double min_dist = std::numeric_limits::max(); + for ( std::size_t k = 0; k < geom_model.collisionPairs.size(); ++k ) { + min_dist = std::min( min_dist, geom_data.distanceResults[k].min_distance ); + } + return min_dist; +} + +/// Build a q vector from a name->value map (handles continuous joints as [cos, sin]) +Eigen::VectorXd buildQ( const pinocchio::Model &model, + const std::unordered_map &joint_positions ) +{ + Eigen::VectorXd q = pinocchio::neutral( model ); + for ( const auto &[name, position] : joint_positions ) { + pinocchio::JointIndex jid = 0; + for ( pinocchio::JointIndex j = 1; j < model.joints.size(); ++j ) { + if ( model.names[j] == name ) { + jid = j; + break; + } + } + if ( jid == 0 ) + continue; + + const int nq_j = model.joints[jid].nq(); + const int nv_j = model.joints[jid].nv(); + const int iq = model.idx_qs[jid]; + + if ( nq_j == 1 ) { + q[iq] = position; + } else if ( nq_j == 2 && nv_j == 1 ) { + q[iq] = std::cos( position ); + q[iq + 1] = std::sin( position ); + } + } + return q; +} + +/// Compare broadphase result against brute-force reference. +/// For non-penetrating: exact match. +/// For penetrating: broadphase min_distance >= bf_min (less negative is ok), +/// but both must agree that collision is present. +void expectDistanceMatch( double broadphase_min, double bf_min, double padding, + const std::string &label ) +{ + if ( bf_min > 0.0 ) { + // Non-penetrating: broadphase should find exact same minimum + EXPECT_NEAR( broadphase_min, bf_min, 1e-10 ) << label; + } else { + // Penetrating: broadphase may report less-negative distance (AABB pruning), + // but must still detect collision + EXPECT_LE( broadphase_min, padding ) + << "Broadphase should detect collision when brute-force says penetrating. " << label; + EXPECT_GE( broadphase_min, bf_min ) + << "Broadphase should not report more penetration than brute-force. " << label; + } +} + +} // namespace + +class CollisionCheckerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::NodeOptions opts; + node_ = std::make_shared( "test_collision_checker", opts ); + urdf_xml_ = loadUrdfFile( "test_robot_collision.urdf" ); + + // Build reference model for brute-force comparison + pinocchio::urdf::buildModelFromXML( urdf_xml_, ref_model_ ); + ref_data_ = pinocchio::Data( ref_model_ ); + std::istringstream urdf_stream( urdf_xml_ ); + pinocchio::urdf::buildGeom( ref_model_, urdf_stream, pinocchio::COLLISION, ref_geom_model_ ); + ref_geom_model_.addAllCollisionPairs(); + ref_geom_data_ = pinocchio::GeometryData( ref_geom_model_ ); + } + + void TearDown() override { node_.reset(); } + + /// Create a CollisionChecker and init with test URDF + std::unique_ptr makeChecker( double padding = 0.0, double cache_epsilon = 0.0, + bool debug_viz = false ) + { + auto checker = std::make_unique( node_, padding, cache_epsilon, debug_viz ); + // Pass all joints as controlled so no filtering occurs (fair comparison with reference) + std::vector all_joints; + for ( pinocchio::JointIndex jid = 1; jid < ref_model_.joints.size(); ++jid ) { + all_joints.push_back( ref_model_.names[jid] ); + } + bool ok = checker->initFromXml( urdf_xml_, "", all_joints ); + EXPECT_TRUE( ok ); + return checker; + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::string urdf_xml_; + + // Reference model for brute-force distance + pinocchio::Model ref_model_; + pinocchio::Data ref_data_; + pinocchio::GeometryModel ref_geom_model_; + pinocchio::GeometryData ref_geom_data_; +}; + +// ---- Test 1: Zero configuration (non-penetrating) ---- +TEST_F( CollisionCheckerTest, ZeroConfig ) +{ + auto checker = makeChecker(); + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 0.0 }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + EXPECT_FALSE( result.in_collision ); + EXPECT_GT( result.min_distance, 0.0 ); + EXPECT_NEAR( result.min_distance, bf_min, 1e-10 ); +} + +// ---- Test 2: Collision configuration ---- +TEST_F( CollisionCheckerTest, CollisionConfig ) +{ + auto checker = makeChecker( 0.0 ); + // Fold the chain so link1 sphere (r=0.12) meets link4 sphere (r=0.12) + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", M_PI }, { "joint3", -M_PI / 2.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + // Both should detect collision + EXPECT_TRUE( result.in_collision ); + EXPECT_LT( bf_min, 0.0 ); + // Broadphase may report less-negative distance in penetration + EXPECT_GE( result.min_distance, bf_min ); + EXPECT_LE( result.min_distance, 0.0 ); +} + +// ---- Test 3: Sweep joint2 and compare broadphase vs brute-force ---- +TEST_F( CollisionCheckerTest, SweepJoint2 ) +{ + auto checker = makeChecker(); + + const int steps = 20; + for ( int i = 0; i <= steps; ++i ) { + const double angle = -M_PI + ( 2.0 * M_PI * i ) / steps; + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", angle }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = + bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + expectDistanceMatch( result.min_distance, bf_min, 0.0, + "joint2=" + std::to_string( angle ) + " rad" ); + } +} + +// ---- Test 4: Random configurations ---- +TEST_F( CollisionCheckerTest, RandomConfigurations ) +{ + auto checker = makeChecker(); + + std::mt19937 rng( 42 ); // fixed seed for reproducibility + std::uniform_real_distribution dist( -M_PI, M_PI ); + + const int n_configs = 50; + for ( int i = 0; i < n_configs; ++i ) { + std::unordered_map positions = { { "joint1", dist( rng ) }, + { "joint2", dist( rng ) }, + { "joint3", dist( rng ) }, + { "joint4", dist( rng ) } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = + bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + expectDistanceMatch( result.min_distance, bf_min, 0.0, "config " + std::to_string( i ) ); + } +} + +// ---- Test 5: Cache works ---- +TEST_F( CollisionCheckerTest, CacheStillWorks ) +{ + const double cache_epsilon = 1e-4; + auto checker = makeChecker( 0.0, cache_epsilon ); + + std::unordered_map positions = { + { "joint1", 0.5 }, { "joint2", 0.3 }, { "joint3", -0.2 }, { "joint4", 0.0 } }; + + auto result1 = checker->checkCollision( positions ); + auto result2 = checker->checkCollision( positions ); + + // Identical call should return cached result + EXPECT_DOUBLE_EQ( result1.min_distance, result2.min_distance ); + EXPECT_EQ( result1.in_collision, result2.in_collision ); + + // Move beyond epsilon — should recompute + positions["joint1"] += cache_epsilon * 10.0; + auto result3 = checker->checkCollision( positions ); + + // Should still be valid (not necessarily equal since position changed) + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + expectDistanceMatch( result3.min_distance, bf_min, 0.0, "cache recompute" ); +} + +// ---- Test 6: Continuous joint encoding ---- +TEST_F( CollisionCheckerTest, ContinuousJointEncoding ) +{ + auto checker = makeChecker(); + + // Test angles including large multiples of 2*PI. + // Note: M_PI and -M_PI fold joint4 to flip link4 back, which may cause collision. + std::vector angles = { 0.0, M_PI / 2.0, M_PI, + -M_PI, 4.0 * M_PI + 0.1, -6.0 * M_PI - 0.5 }; + + for ( double angle : angles ) { + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 0.0 }, { "joint3", 0.0 }, { "joint4", angle } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = + bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + expectDistanceMatch( result.min_distance, bf_min, 0.0, + "continuous joint angle=" + std::to_string( angle ) ); + } +} + +// ---- Test 7: NaN input returns safe result ---- +TEST_F( CollisionCheckerTest, NaNInputReturnsSafeResult ) +{ + auto checker = makeChecker(); + + std::unordered_map positions = { + { "joint1", 0.0 }, + { "joint2", std::numeric_limits::quiet_NaN() }, + { "joint3", 0.0 }, + { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + EXPECT_TRUE( result.in_collision ) << "NaN input should report collision (safe default)"; +} + +// ---- Test 8: Pair filtering preserved ---- +TEST_F( CollisionCheckerTest, PairFilteringPreserved ) +{ + // Create checker with only subset of joints + auto checker = std::make_unique( node_, 0.0, 0.0, false ); + std::vector subset_joints = { "joint1", "joint2" }; + bool ok = checker->initFromXml( urdf_xml_, "", subset_joints ); + ASSERT_TRUE( ok ); + + // Should have fewer pairs than the reference (which has all) + auto joint_names = checker->getJointNames(); + EXPECT_GT( joint_names.size(), 0u ); + + // Distance should still be correct for the reduced pair set + std::unordered_map positions = { + { "joint1", 0.5 }, { "joint2", 0.3 }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + EXPECT_GT( result.min_distance, 0.0 ); + EXPECT_FALSE( result.in_collision ); +} + +// ---- Test 9: Collision with padding ---- +TEST_F( CollisionCheckerTest, CollisionPaddingWorks ) +{ + // At zero config, find actual distance + auto checker_no_pad = makeChecker( 0.0 ); + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 0.0 }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result_no_pad = checker_no_pad->checkCollision( positions ); + ASSERT_FALSE( result_no_pad.in_collision ); + const double actual_dist = result_no_pad.min_distance; + + // With padding larger than actual distance, should be in collision + auto checker_with_pad = makeChecker( actual_dist + 0.01 ); + auto result_with_pad = checker_with_pad->checkCollision( positions ); + EXPECT_TRUE( result_with_pad.in_collision ); + EXPECT_NEAR( result_with_pad.min_distance, actual_dist, 1e-10 ); +} + +// ---- Test 10: Multi-joint sweep ---- +TEST_F( CollisionCheckerTest, MultiJointSweep ) +{ + auto checker = makeChecker(); + + // Sweep both joint2 and joint3 simultaneously + const int steps = 10; + for ( int i = 0; i <= steps; ++i ) { + for ( int j = 0; j <= steps; ++j ) { + const double a2 = -M_PI + ( 2.0 * M_PI * i ) / steps; + const double a3 = -M_PI + ( 2.0 * M_PI * j ) / steps; + + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", a2 }, { "joint3", a3 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = + bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + expectDistanceMatch( result.min_distance, bf_min, 0.0, + "joint2=" + std::to_string( a2 ) + ", joint3=" + std::to_string( a3 ) ); + } + } +} + +// ---- Test 11: Non-penetrating configs always match exactly ---- +TEST_F( CollisionCheckerTest, NonPenetratingExactMatch ) +{ + auto checker = makeChecker(); + + // Small angles that keep the chain extended (no collision) + std::vector> angles = { + { 0.0, 0.0 }, { 0.3, 0.0 }, { 0.0, 0.3 }, { 0.3, 0.3 }, + { -0.3, 0.3 }, { 0.5, -0.5 }, { -0.5, 0.5 }, { 0.1, 0.1 }, + }; + + for ( const auto &[a2, a3] : angles ) { + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", a2 }, { "joint3", a3 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = + bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + ASSERT_GT( bf_min, 0.0 ) << "Config should be non-penetrating"; + EXPECT_NEAR( result.min_distance, bf_min, 1e-10 ) + << "Non-penetrating config must match exactly at j2=" << a2 << ", j3=" << a3; + } +} + +// ============================================================ +// Gradient tests for directional collision velocity scaling +// ============================================================ + +// ---- Test 12: Gradient matches finite difference ---- +TEST_F( CollisionCheckerTest, GradientMatchesFiniteDifference ) +{ + auto checker = makeChecker(); + const double eps = 1e-6; + const double safety_zone = 1.0; // large threshold to always compute gradients + + // Test multiple configurations (non-penetrating with various clearances) + std::vector> configs = { + { { "joint1", 0.0 }, { "joint2", 0.5 }, { "joint3", -0.3 }, { "joint4", 0.0 } }, + { { "joint1", 0.3 }, { "joint2", 2.0 }, { "joint3", -1.0 }, { "joint4", 0.5 } }, + { { "joint1", -0.2 }, { "joint2", 1.5 }, { "joint3", -0.8 }, { "joint4", -0.3 } }, + { { "joint1", 0.0 }, { "joint2", 0.0 }, { "joint3", 0.0 }, { "joint4", 0.0 } }, + }; + + for ( size_t ci = 0; ci < configs.size(); ++ci ) { + const auto &positions = configs[ci]; + + // Get result with gradient + auto result = checker->checkCollision( positions, safety_zone ); + // Skip if no pairs in safety zone (e.g. all pairs have distance > safety_zone) + if ( result.safety_zone_pairs.empty() ) + continue; + + // Find the closest pair (min distance) — broadphase may return pairs in any order + const auto &closest = + *std::min_element( result.safety_zone_pairs.begin(), result.safety_zone_pairs.end(), + []( const auto &a, const auto &b ) { return a.distance < b.distance; } ); + const auto &gradient = closest.gradient; + + // Finite-difference validation for each controlled joint + for ( const auto &[name, val] : positions ) { + auto perturbed = positions; + perturbed[name] = val + eps; + + // Need to invalidate cache — use a different checker or large perturbation + // Actually, the cache epsilon is 0 for this checker, so different q always recomputes + auto result_plus = checker->checkCollision( perturbed, safety_zone ); + + double fd_gradient = ( result_plus.min_distance - result.min_distance ) / eps; + + int v_idx = checker->getJointVelocityIndex( name ); + ASSERT_GE( v_idx, 0 ) << "Joint " << name << " not found"; + ASSERT_LT( v_idx, gradient.size() ); + + EXPECT_NEAR( gradient[v_idx], fd_gradient, 1e-3 ) + << "Gradient mismatch for joint " << name << " at config " << ci + << " (analytical=" << gradient[v_idx] << ", fd=" << fd_gradient << ")"; + } + } +} + +// ---- Test 13: Gradient sign for approaching motion ---- +TEST_F( CollisionCheckerTest, GradientSignApproaching ) +{ + auto checker = makeChecker(); + const double safety_zone = 1.0; + + // Start from straight chain, fold joint2 toward PI (approaching collision) + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 1.5 }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions, safety_zone ); + ASSERT_FALSE( result.safety_zone_pairs.empty() ); + + const auto &gradient = result.safety_zone_pairs[0].gradient; + int v_idx_j2 = checker->getJointVelocityIndex( "joint2" ); + ASSERT_GE( v_idx_j2, 0 ); + + // Motion toward PI (positive delta for joint2) folds the chain → should decrease distance + // So gradient[v_idx_j2] * (+delta) should be negative → gradient[v_idx_j2] < 0 + // (or the reverse depending on the chain geometry — let's use the finite difference to verify sign) + double delta = 0.1; + auto positions_plus = positions; + positions_plus["joint2"] += delta; + auto result_plus = checker->checkCollision( positions_plus, safety_zone ); + + // If distance decreased, the motion is approaching + if ( result_plus.min_distance < result.min_distance ) { + // The directional derivative should be negative + double dir_deriv = gradient[v_idx_j2] * delta; + EXPECT_LT( dir_deriv, 0.0 ) << "Gradient should indicate approaching when distance decreases. " + << "grad[j2]=" << gradient[v_idx_j2] << " delta=" << delta; + } +} + +// ---- Test 14: Gradient sign for retreating motion ---- +TEST_F( CollisionCheckerTest, GradientSignRetreating ) +{ + auto checker = makeChecker(); + const double safety_zone = 1.0; + + // Near collision but NOT penetrating: joint2 folded partway + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 2.0 }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions, safety_zone ); + ASSERT_FALSE( result.safety_zone_pairs.empty() ); + ASSERT_GT( result.min_distance, 0.0 ) + << "Config must be non-penetrating for gradient to be valid"; + + const auto &gradient = result.safety_zone_pairs[0].gradient; + int v_idx_j2 = checker->getJointVelocityIndex( "joint2" ); + ASSERT_GE( v_idx_j2, 0 ); + + // Motion back toward 0 (negative delta for joint2) unfolds the chain → should increase distance + double delta = -0.1; + auto positions_minus = positions; + positions_minus["joint2"] += delta; + auto result_minus = checker->checkCollision( positions_minus, safety_zone ); + + if ( result_minus.min_distance > result.min_distance ) { + // The directional derivative should be positive (moving away) + double dir_deriv = gradient[v_idx_j2] * delta; + EXPECT_GT( dir_deriv, 0.0 ) << "Gradient should indicate retreating when distance increases. " + << "grad[j2]=" << gradient[v_idx_j2] << " delta=" << delta; + } +} + +// ---- Test 15: Multiple pairs in safety zone get gradients ---- +TEST_F( CollisionCheckerTest, GradientComputedForAllSafetyZonePairs ) +{ + auto checker = makeChecker(); + const double safety_zone = 2.0; // very large to capture all pairs + + // Configuration with multiple pairs relatively close but NOT penetrating + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 0.8 }, { "joint3", -0.3 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions, safety_zone ); + + // With a large safety zone, multiple pairs should have gradients + EXPECT_GE( result.safety_zone_pairs.size(), 1u ) + << "Expected at least 1 pair in safety zone with threshold=" << safety_zone; + + // Each pair should have a non-empty gradient vector + std::size_t non_zero_gradient_count = 0; + for ( const auto &pi : result.safety_zone_pairs ) { + EXPECT_GT( pi.gradient.size(), 0 ) << "Pair " << pi.pair_index << " has empty gradient"; + if ( pi.gradient.norm() > 0.0 ) { + non_zero_gradient_count++; + } + // Note: some pairs may have zero gradient if both bodies share the same parent joint + // (distance between them is invariant to any joint motion), or if they are penetrating. + } + // At least one pair should have a non-zero gradient + EXPECT_GT( non_zero_gradient_count, 0u ) << "Expected at least one pair with non-zero gradient"; +} + +// ---- Test 16: No gradient when threshold is zero ---- +TEST_F( CollisionCheckerTest, NoGradientWhenThresholdZero ) +{ + auto checker = makeChecker(); + + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 1.5 }, { "joint3", -0.5 }, { "joint4", 0.0 } }; + + // Default threshold = 0 → no gradient computation + auto result = checker->checkCollision( positions ); + EXPECT_TRUE( result.safety_zone_pairs.empty() ) + << "No safety zone pairs should be computed with threshold=0"; +} + +// ---- Test 17: getJointVelocityIndex and getNv ---- +TEST_F( CollisionCheckerTest, VelocitySpaceHelpers ) +{ + auto checker = makeChecker(); + + EXPECT_GT( checker->getNv(), 0 ); + EXPECT_GE( checker->getJointVelocityIndex( "joint1" ), 0 ); + EXPECT_GE( checker->getJointVelocityIndex( "joint2" ), 0 ); + EXPECT_GE( checker->getJointVelocityIndex( "joint3" ), 0 ); + EXPECT_GE( checker->getJointVelocityIndex( "joint4" ), 0 ); + EXPECT_EQ( checker->getJointVelocityIndex( "nonexistent_joint" ), -1 ); + + // All indices should be distinct + std::set indices; + for ( const auto &name : { "joint1", "joint2", "joint3", "joint4" } ) { + int idx = checker->getJointVelocityIndex( name ); + EXPECT_TRUE( indices.insert( idx ).second ) << "Duplicate velocity index for " << name; + } +} + +// ---- Test 18: Performance benchmark ---- +TEST_F( CollisionCheckerTest, PerformanceBenchmark ) +{ + auto checker = makeChecker(); + const double safety_zone = 0.05; // realistic threshold + + std::mt19937 rng( 123 ); + std::uniform_real_distribution dist( -M_PI, M_PI ); + + // Pre-generate configs + constexpr int N = 1000; + std::vector> configs( N ); + for ( int i = 0; i < N; ++i ) { + configs[i] = { { "joint1", dist( rng ) }, + { "joint2", dist( rng ) }, + { "joint3", dist( rng ) }, + { "joint4", dist( rng ) } }; + } + + // Warm up + for ( int i = 0; i < 10; ++i ) { checker->checkCollision( configs[i], safety_zone ); } + + // Benchmark + auto t0 = std::chrono::steady_clock::now(); + std::size_t total_safety_pairs = 0; + for ( int i = 0; i < N; ++i ) { + auto result = checker->checkCollision( configs[i], safety_zone ); + total_safety_pairs += result.safety_zone_pairs.size(); + } + auto t1 = std::chrono::steady_clock::now(); + + const double elapsed_us = + static_cast( std::chrono::duration_cast( t1 - t0 ).count() ); + const double avg_us = elapsed_us / static_cast( N ); + + std::cout << "[Benchmark] avg checkCollision: " << avg_us << " us" + << " | pairs: " << checker->getNumCollisionPairs() << " | avg safety_zone_pairs: " + << ( static_cast( total_safety_pairs ) / static_cast( N ) ) << std::endl; + + // Soft assertion: should complete within 10ms per call + EXPECT_LT( avg_us, 10000.0 ) << "Collision check too slow"; +} + +// ============================================================ +// Broadphase correctness tests +// ============================================================ + +// Test fixture that runs tests with broadphase enabled +class CollisionCheckerBroadphaseTest : public CollisionCheckerTest +{ +protected: + std::unique_ptr makeChecker( double padding = 0.0, double cache_epsilon = 0.0, + bool debug_viz = false ) + { + auto checker = std::make_unique( node_, padding, cache_epsilon, debug_viz ); + checker->setBroadphase( true ); + std::vector all_joints; + for ( pinocchio::JointIndex jid = 1; jid < ref_model_.joints.size(); ++jid ) { + all_joints.push_back( ref_model_.names[jid] ); + } + bool ok = checker->initFromXml( urdf_xml_, "", all_joints ); + EXPECT_TRUE( ok ); + EXPECT_TRUE( checker->isBroadphaseEnabled() ); + return checker; + } +}; + +// Re-run key correctness tests with broadphase +TEST_F( CollisionCheckerBroadphaseTest, ZeroConfig ) +{ + auto checker = makeChecker(); + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", 0.0 }, { "joint3", 0.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + EXPECT_FALSE( result.in_collision ); + EXPECT_GT( result.min_distance, 0.0 ); + EXPECT_NEAR( result.min_distance, bf_min, 1e-10 ); +} + +TEST_F( CollisionCheckerBroadphaseTest, CollisionConfig ) +{ + auto checker = makeChecker( 0.0 ); + std::unordered_map positions = { + { "joint1", 0.0 }, { "joint2", M_PI }, { "joint3", -M_PI / 2.0 }, { "joint4", 0.0 } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + EXPECT_TRUE( result.in_collision ); + EXPECT_LT( bf_min, 0.0 ); + EXPECT_GE( result.min_distance, bf_min ); + EXPECT_LE( result.min_distance, 0.0 ); +} + +TEST_F( CollisionCheckerBroadphaseTest, RandomConfigurations ) +{ + auto checker = makeChecker(); + + std::mt19937 rng( 42 ); + std::uniform_real_distribution dist( -M_PI, M_PI ); + + for ( int i = 0; i < 50; ++i ) { + std::unordered_map positions = { { "joint1", dist( rng ) }, + { "joint2", dist( rng ) }, + { "joint3", dist( rng ) }, + { "joint4", dist( rng ) } }; + + auto result = checker->checkCollision( positions ); + + Eigen::VectorXd q = buildQ( ref_model_, positions ); + double bf_min = + bruteForceMinDistance( ref_model_, ref_data_, ref_geom_model_, ref_geom_data_, q ); + + expectDistanceMatch( result.min_distance, bf_min, 0.0, + "broadphase config " + std::to_string( i ) ); + } +} + +TEST_F( CollisionCheckerBroadphaseTest, GradientMatchesFiniteDifference ) +{ + auto checker = makeChecker(); + const double eps = 1e-6; + const double safety_zone = 1.0; + + std::vector> configs = { + { { "joint1", 0.0 }, { "joint2", 0.5 }, { "joint3", -0.3 }, { "joint4", 0.0 } }, + { { "joint1", 0.3 }, { "joint2", 2.0 }, { "joint3", -1.0 }, { "joint4", 0.5 } }, + { { "joint1", -0.2 }, { "joint2", 1.5 }, { "joint3", -0.8 }, { "joint4", -0.3 } }, + { { "joint1", 0.0 }, { "joint2", 0.0 }, { "joint3", 0.0 }, { "joint4", 0.0 } }, + }; + + for ( size_t ci = 0; ci < configs.size(); ++ci ) { + const auto &positions = configs[ci]; + auto result = checker->checkCollision( positions, safety_zone ); + if ( result.safety_zone_pairs.empty() ) + continue; + + // Find the closest pair (min distance) — broadphase may return pairs in any order + const auto &closest = + *std::min_element( result.safety_zone_pairs.begin(), result.safety_zone_pairs.end(), + []( const auto &a, const auto &b ) { return a.distance < b.distance; } ); + const auto &gradient = closest.gradient; + + for ( const auto &[name, val] : positions ) { + auto perturbed = positions; + perturbed[name] = val + eps; + auto result_plus = checker->checkCollision( perturbed, safety_zone ); + double fd_gradient = ( result_plus.min_distance - result.min_distance ) / eps; + + int v_idx = checker->getJointVelocityIndex( name ); + ASSERT_GE( v_idx, 0 ) << "Joint " << name << " not found"; + ASSERT_LT( v_idx, gradient.size() ); + + EXPECT_NEAR( gradient[v_idx], fd_gradient, 1e-3 ) + << "Broadphase gradient mismatch for joint " << name << " at config " << ci; + } + } +} + +// ---- Direct comparison: broadphase vs brute-force on same configs ---- +TEST_F( CollisionCheckerTest, BroadphaseMatchesBruteForce ) +{ + auto bf_checker = makeChecker(); + bf_checker->setBroadphase( false ); // force brute-force for comparison + // Create broadphase checker + auto bp_checker = std::make_unique( node_, 0.0, 0.0, false ); + bp_checker->setBroadphase( true ); + std::vector all_joints; + for ( pinocchio::JointIndex jid = 1; jid < ref_model_.joints.size(); ++jid ) { + all_joints.push_back( ref_model_.names[jid] ); + } + ASSERT_TRUE( bp_checker->initFromXml( urdf_xml_, "", all_joints ) ); + + std::mt19937 rng( 999 ); + std::uniform_real_distribution dist( -M_PI, M_PI ); + const double safety_zone = 0.1; + + for ( int i = 0; i < 200; ++i ) { + std::unordered_map positions = { { "joint1", dist( rng ) }, + { "joint2", dist( rng ) }, + { "joint3", dist( rng ) }, + { "joint4", dist( rng ) } }; + + auto bf_result = bf_checker->checkCollision( positions, safety_zone ); + auto bp_result = bp_checker->checkCollision( positions, safety_zone ); + + EXPECT_EQ( bp_result.in_collision, bf_result.in_collision ) << "Config " << i; + + if ( bf_result.min_distance > 0.0 ) { + EXPECT_NEAR( bp_result.min_distance, bf_result.min_distance, 1e-9 ) + << "Non-penetrating distance mismatch at config " << i; + } else { + EXPECT_LE( bp_result.min_distance, 0.0 ) << "Both should detect collision at config " << i; + } + + // Sort both by pair_index (broadphase traverses in AABB-tree order, not sequential) + auto sort_by_pair = []( auto &pairs ) { + std::sort( pairs.begin(), pairs.end(), + []( const auto &a, const auto &b ) { return a.pair_index < b.pair_index; } ); + }; + sort_by_pair( bp_result.safety_zone_pairs ); + sort_by_pair( bf_result.safety_zone_pairs ); + + EXPECT_EQ( bp_result.safety_zone_pairs.size(), bf_result.safety_zone_pairs.size() ) + << "Safety zone pair count mismatch at config " << i; + + // Compare gradients + for ( size_t j = 0; + j < std::min( bp_result.safety_zone_pairs.size(), bf_result.safety_zone_pairs.size() ); + ++j ) { + EXPECT_EQ( bp_result.safety_zone_pairs[j].pair_index, bf_result.safety_zone_pairs[j].pair_index ) + << "Pair index mismatch at config " << i << " pair " << j; + + if ( bp_result.safety_zone_pairs[j].gradient.size() == + bf_result.safety_zone_pairs[j].gradient.size() ) { + for ( int k = 0; k < bp_result.safety_zone_pairs[j].gradient.size(); ++k ) { + EXPECT_NEAR( bp_result.safety_zone_pairs[j].gradient[k], + bf_result.safety_zone_pairs[j].gradient[k], 1e-6 ) + << "Gradient mismatch at config " << i << " pair " << j << " element " << k; + } + } + } + } +} + +// ---- Broadphase vs brute-force with Athena robot ---- +TEST_F( CollisionCheckerTest, BroadphaseMatchesBruteForceAthena ) +{ + std::string athena_urdf, athena_srdf; + try { + athena_urdf = loadUrdfFile( "athena.urdf" ); + athena_srdf = loadUrdfFile( "athena.srdf" ); + } catch ( ... ) { + GTEST_SKIP() << "Athena URDF/SRDF not available"; + } + + const std::vector arm_joints = { "arm_joint_1", "arm_joint_2", "arm_joint_3", + "arm_joint_4", "arm_joint_5", "arm_joint_6", + "arm_joint_7" }; + + auto bf_checker = std::make_unique( node_, 0.01, 0.0, false ); + bf_checker->setBroadphase( false ); // force brute-force for comparison + ASSERT_TRUE( bf_checker->initFromXml( athena_urdf, athena_srdf, arm_joints ) ); + + auto bp_checker = std::make_unique( node_, 0.01, 0.0, false ); + bp_checker->setBroadphase( true ); + ASSERT_TRUE( bp_checker->initFromXml( athena_urdf, athena_srdf, arm_joints ) ); + + std::mt19937 rng( 77 ); + std::uniform_real_distribution dist( -M_PI, M_PI ); + const double safety_zone = 0.05; + + for ( int i = 0; i < 100; ++i ) { + std::unordered_map positions; + for ( const auto &name : arm_joints ) { positions[name] = dist( rng ); } + + auto bf_result = bf_checker->checkCollision( positions, safety_zone ); + auto bp_result = bp_checker->checkCollision( positions, safety_zone ); + + EXPECT_EQ( bp_result.in_collision, bf_result.in_collision ) << "Athena config " << i; + + if ( bf_result.min_distance > 0.0 ) { + // Broadphase uses hpp::fcl::distance directly on manager objects while brute-force + // uses pinocchio::computeDistances — small numerical differences are expected from GJK. + EXPECT_NEAR( bp_result.min_distance, bf_result.min_distance, 1e-6 ) + << "Athena distance mismatch at config " << i; + } + + EXPECT_EQ( bp_result.safety_zone_pairs.size(), bf_result.safety_zone_pairs.size() ) + << "Athena safety zone pair count mismatch at config " << i; + } +} + +// __gcov_dump is only available when compiled with --coverage. +// Use a weak symbol so the call is a no-op in normal (non-coverage) builds. +#if defined( __GNUC__ ) +extern "C" void __gcov_dump() __attribute__( ( weak ) ); +#endif + +int main( int argc, char **argv ) +{ + testing::InitGoogleTest( &argc, argv ); + rclcpp::init( argc, argv ); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + // Use _exit to avoid double-free in global destructors caused by + // pinocchio/hpp-fcl library cleanup ordering issues. + // All test resources are cleaned up in TearDown before reaching this point. +#if defined( __GNUC__ ) + if ( __gcov_dump ) + __gcov_dump(); // Flush coverage data before _exit +#endif + _exit( result ); +} diff --git a/hector_ros_controllers/test/test_helpers.hpp b/hector_ros_controllers/test/test_helpers.hpp index 75652b5..a0d4479 100644 --- a/hector_ros_controllers/test/test_helpers.hpp +++ b/hector_ros_controllers/test/test_helpers.hpp @@ -7,7 +7,12 @@ #include #include -// Access private/protected members for testing +// Expose private/protected members for testing. This is technically undefined +// behavior per [macro.names], but is necessary here because tests must access +// protected members of the ros2_control base classes (command_interfaces_, +// state_interfaces_, reference_interfaces_) which cannot have friend +// declarations added to third-party headers. In practice this is safe on +// GCC/Clang as they do not reorder members across access specifiers. #define private public #define protected public #include diff --git a/hector_ros_controllers/test/test_safety_position_controller.cpp b/hector_ros_controllers/test/test_safety_position_controller.cpp index d70b649..4275b45 100644 --- a/hector_ros_controllers/test/test_safety_position_controller.cpp +++ b/hector_ros_controllers/test/test_safety_position_controller.cpp @@ -113,8 +113,8 @@ class SafetyPositionControllerTest rclcpp::Parameter( "unwrap_continuous_joints", true ), rclcpp::Parameter( "enforce_position_limits", true ), rclcpp::Parameter( "check_self_collisions", check_self_collisions ), - rclcpp::Parameter( "block_if_too_far", !check_self_collisions ), rclcpp::Parameter( "block_velocity_scaling", 1.5 ), + rclcpp::Parameter( "collision_safety_zone", 0.05 ), rclcpp::Parameter( "set_current_limits", set_current_limits ), rclcpp::Parameter( "safety_bypass_timeout", 60.0 ), rclcpp::Parameter( "safety_bypass_joint_limit_tolerance", 0.03 ), @@ -270,30 +270,30 @@ TEST_F( SafetyPositionControllerTest, EnforceLimitsUnwrapsContinuous ) } // ============================================================================ -// block_if_too_far Tests +// Velocity Limiting Tests (no collision fixture — collision checks disabled) // ============================================================================ -TEST_F( SafetyPositionControllerTest, BlockIfTooFarLimitsStep ) +TEST_F( SafetyPositionControllerTest, NoVelocityLimitingWithoutCollisionChecks ) { - initController(); + // When check_self_collisions=false, velocity limiting is not applied + initController(); // check_self_collisions=false by default configureController(); setupHardwareInterfaces(); findMocks(); EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); activateController(); - // Current at 0.0, command at 0.5 (within limits but large step) - // velocity_limit=1.0, update_rate=100, scaling=1.5 -> max_step = 1.0/100 * 1.5 = 0.015 for ( auto &v : hw_state_values_ ) v = 0.0; - controller_->reference_interfaces_[0] = 0.5; // large jump for joint1 + // Large jump within joint limits + controller_->reference_interfaces_[0] = 0.5; controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; callUpdate(); - double max_step = 1.0 / kUpdateRate * 1.5; - EXPECT_NEAR( hw_cmd_values_[0], max_step, 1e-6 ); + // Without collision checks, no velocity limiting → full step passes through + EXPECT_NEAR( hw_cmd_values_[0], 0.5, 1e-6 ); } // ============================================================================ @@ -707,7 +707,8 @@ class SafetyPositionControllerCollisionTest void SetUp() override { controller_ = std::make_shared(); } void initWithCollisions( const std::vector &joints = {}, - const std::string &urdf_file = "test_robot_collision.urdf" ) + const std::string &urdf_file = "test_robot_collision.urdf", + const std::vector &extra_overrides = {} ) { auto cj = joints.empty() ? controlled_joints_ : joints; const auto urdf = hector_test::loadUrdfFile( urdf_file ); @@ -719,14 +720,13 @@ class SafetyPositionControllerCollisionTest params.controller_manager_update_rate = kUpdateRate; params.node_namespace = ""; - rclcpp::NodeOptions opts; - opts.parameter_overrides( { + std::vector overrides = { rclcpp::Parameter( "joints", cj ), rclcpp::Parameter( "unwrap_continuous_joints", true ), rclcpp::Parameter( "enforce_position_limits", true ), rclcpp::Parameter( "check_self_collisions", true ), - rclcpp::Parameter( "block_if_too_far", true ), // auto-set true when collisions on - rclcpp::Parameter( "block_velocity_scaling", 100.0 ), // high scaling to not limit step + rclcpp::Parameter( "block_velocity_scaling", 3.0 ), // max allowed scaling + rclcpp::Parameter( "collision_safety_zone", 0.05 ), rclcpp::Parameter( "set_current_limits", false ), rclcpp::Parameter( "safety_bypass_timeout", 60.0 ), rclcpp::Parameter( "safety_bypass_joint_limit_tolerance", 0.03 ), @@ -734,7 +734,24 @@ class SafetyPositionControllerCollisionTest rclcpp::Parameter( "collision_padding", 0.0 ), rclcpp::Parameter( "collision_cache_epsilon", 0.0 ), // disable cache for tests rclcpp::Parameter( "debug_visualize_collisions", false ), - } ); + }; + // Apply extra overrides (replaces matching parameters by name) + for ( const auto &extra : extra_overrides ) { + bool found = false; + for ( auto &base : overrides ) { + if ( base.get_name() == extra.get_name() ) { + base = extra; + found = true; + break; + } + } + if ( !found ) { + overrides.push_back( extra ); + } + } + + rclcpp::NodeOptions opts; + opts.parameter_overrides( overrides ); params.node_options = opts; auto result = controller_->init( params ); @@ -981,6 +998,620 @@ TEST_F( SafetyPositionControllerCollisionTest, ContinuousJointCollisionDetected EXPECT_DOUBLE_EQ( hw_cmd_values_[3], 0.0 ); } +// ============================================================================ +// Velocity Limiting & Distance-Based Scaling Tests (collision fixture) +// ============================================================================ + +TEST_F( SafetyPositionControllerCollisionTest, VelocityLimitingWithCollisionChecks ) +{ + // Override block_velocity_scaling to a known low value + initWithCollisions( {}, "test_robot_collision.urdf", + { rclcpp::Parameter( "block_velocity_scaling", 1.5 ) } ); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // First cycle: last_min_distance_ = max -> distance_scale = 1.0 (full speed) + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.5; // large jump + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // max_step = velocity_limit / update_rate * block_velocity_scaling = 1.0/100 * 1.5 = 0.015 + double max_step = 1.0 / kUpdateRate * 1.5; + EXPECT_NEAR( hw_cmd_values_[0], max_step, 1e-6 ); +} + +TEST_F( SafetyPositionControllerCollisionTest, DistanceBasedScalingReducesVelocity ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // Set last_min_distance_ to halfway in the safety zone + // collision_padding=0.0, collision_safety_zone=0.05 + // d=0.025 -> scale = (0.025 - 0.0) / (0.05 - 0.0) = 0.5 + controller_->last_min_distance_ = 0.025; + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.5; // large jump + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // max_step = velocity_limit / update_rate * block_velocity_scaling * distance_scale + // = 1.0 / 100 * 3.0 * 0.5 = 0.015 + double full_max_step = 1.0 / kUpdateRate * 3.0; + double expected_step = full_max_step * 0.5; + EXPECT_NEAR( hw_cmd_values_[0], expected_step, 1e-6 ); +} + +TEST_F( SafetyPositionControllerCollisionTest, DistanceScaleZeroHoldsPosition ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // Set last_min_distance_ to exactly at collision_padding (0.0) -> scale = 0 + controller_->last_min_distance_ = 0.0; + + setStateValue( "joint1", 0.3 ); + setStateValue( "joint2", 0.0 ); + setStateValue( "joint3", 0.0 ); + + controller_->reference_interfaces_[0] = 0.5; // wants to move + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // With distance_scale=0, apply_velocity_limits should hold at current position. + // The collision check at the held position should be safe (straight chain at [0.3,0,0]). + // So the final written command should be the velocity-limited position (= current = 0.3). + EXPECT_NEAR( hw_cmd_values_[0], 0.3, 1e-6 ); +} + +// ============================================================================ +// Directional Collision Scaling Tests +// ============================================================================ + +TEST_F( SafetyPositionControllerCollisionTest, DirectionalScaling_AwayNotScaled ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // Set last_min_distance_ halfway in safety zone -> distance_scale = 0.5 + controller_->last_min_distance_ = 0.025; + + // Create a fake safety zone pair with a gradient that says joint1 positive = moving away + CollisionResult::PairInfo fake_pair; + fake_pair.pair_index = 0; + fake_pair.distance = 0.025; + fake_pair.gradient = Eigen::VectorXd::Zero( controller_->collision_checker_->getNv() ); + // Gradient: positive for joint1's velocity index means positive motion increases distance + int v_idx_j1 = controller_->collision_checker_->getJointVelocityIndex( "joint1" ); + ASSERT_GE( v_idx_j1, 0 ); + fake_pair.gradient[v_idx_j1] = 1.0; // moving joint1 positively moves AWAY + controller_->last_safety_zone_pairs_ = { fake_pair }; + + // Current position: all zero + for ( auto &v : hw_state_values_ ) v = 0.0; + + // Command positive joint1 motion (away from collision) + controller_->reference_interfaces_[0] = 0.5; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // Since motion is away from collision, effective_scale should be 1.0 (not 0.5) + // max_step = velocity_limit / update_rate * block_velocity_scaling * 1.0 + // = 1.0 / 100 * 3.0 = 0.03 + double full_max_step = 1.0 / kUpdateRate * 3.0; + EXPECT_NEAR( hw_cmd_values_[0], full_max_step, 1e-6 ) + << "Motion away from collision should not be scaled down"; +} + +TEST_F( SafetyPositionControllerCollisionTest, DirectionalScaling_TowardIsScaled ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // Set last_min_distance_ halfway in safety zone -> distance_scale = 0.5 + controller_->last_min_distance_ = 0.025; + + // Create a fake safety zone pair: joint1 positive = moving TOWARD collision + CollisionResult::PairInfo fake_pair; + fake_pair.pair_index = 0; + fake_pair.distance = 0.025; + fake_pair.gradient = Eigen::VectorXd::Zero( controller_->collision_checker_->getNv() ); + int v_idx_j1 = controller_->collision_checker_->getJointVelocityIndex( "joint1" ); + ASSERT_GE( v_idx_j1, 0 ); + fake_pair.gradient[v_idx_j1] = -1.0; // moving joint1 positively moves TOWARD collision + controller_->last_safety_zone_pairs_ = { fake_pair }; + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.5; // positive = toward collision + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // Motion toward collision -> effective_scale = distance_scale = 0.5 + double full_max_step = 1.0 / kUpdateRate * 3.0; + double expected_step = full_max_step * 0.5; + EXPECT_NEAR( hw_cmd_values_[0], expected_step, 1e-6 ) + << "Motion toward collision should be scaled down"; +} + +TEST_F( SafetyPositionControllerCollisionTest, DirectionalScaling_AtPaddingCanEscape ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // At collision padding: distance_scale = 0.0 + controller_->last_min_distance_ = 0.0; + + // Create a safety zone pair: moving joint1 positive = AWAY from collision + CollisionResult::PairInfo fake_pair; + fake_pair.pair_index = 0; + fake_pair.distance = 0.0; + fake_pair.gradient = Eigen::VectorXd::Zero( controller_->collision_checker_->getNv() ); + int v_idx_j1 = controller_->collision_checker_->getJointVelocityIndex( "joint1" ); + ASSERT_GE( v_idx_j1, 0 ); + fake_pair.gradient[v_idx_j1] = 1.0; // away + controller_->last_safety_zone_pairs_ = { fake_pair }; + + setStateValue( "joint1", 0.3 ); + setStateValue( "joint2", 0.0 ); + setStateValue( "joint3", 0.0 ); + + // Command motion away + controller_->reference_interfaces_[0] = 0.5; // away from collision + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // Even though distance_scale=0, directional scaling overrides to 1.0 + // because motion is away from collision + double full_max_step = 1.0 / kUpdateRate * 3.0; + double expected_cmd = 0.3 + full_max_step; // current + max step + EXPECT_NEAR( hw_cmd_values_[0], expected_cmd, 1e-6 ) + << "Robot should be able to escape when moving away from collision at padding boundary"; +} + +TEST_F( SafetyPositionControllerCollisionTest, DirectionalScaling_TwoPairsOneWorsening ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + controller_->last_min_distance_ = 0.025; + + int v_idx_j1 = controller_->collision_checker_->getJointVelocityIndex( "joint1" ); + ASSERT_GE( v_idx_j1, 0 ); + int nv = controller_->collision_checker_->getNv(); + + // Pair 1: joint1 positive = AWAY + CollisionResult::PairInfo pair1; + pair1.pair_index = 0; + pair1.distance = 0.025; + pair1.gradient = Eigen::VectorXd::Zero( nv ); + pair1.gradient[v_idx_j1] = 1.0; // away + + // Pair 2: joint1 positive = TOWARD + CollisionResult::PairInfo pair2; + pair2.pair_index = 1; + pair2.distance = 0.03; + pair2.gradient = Eigen::VectorXd::Zero( nv ); + pair2.gradient[v_idx_j1] = -0.5; // toward + + controller_->last_safety_zone_pairs_ = { pair1, pair2 }; + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.5; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // One pair says away, one says toward → worst case is toward → scaling applied + double full_max_step = 1.0 / kUpdateRate * 3.0; + double expected_step = full_max_step * 0.5; // distance_scale = 0.5 + EXPECT_NEAR( hw_cmd_values_[0], expected_step, 1e-6 ) + << "With any pair worsening, motion should be scaled conservatively"; +} + +TEST_F( SafetyPositionControllerCollisionTest, DirectionalScaling_DisabledByParam ) +{ + initWithCollisions( {}, "test_robot_collision.urdf", + { rclcpp::Parameter( "directional_collision_scaling", false ) } ); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + controller_->last_min_distance_ = 0.025; + + // Even with gradient saying "away", scaling should still be applied (param disabled) + CollisionResult::PairInfo fake_pair; + fake_pair.pair_index = 0; + fake_pair.distance = 0.025; + fake_pair.gradient = Eigen::VectorXd::Zero( controller_->collision_checker_->getNv() ); + int v_idx_j1 = controller_->collision_checker_->getJointVelocityIndex( "joint1" ); + fake_pair.gradient[v_idx_j1] = 1.0; // away + controller_->last_safety_zone_pairs_ = { fake_pair }; + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.5; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // With directional scaling disabled, should use distance_scale=0.5 even though moving away + double full_max_step = 1.0 / kUpdateRate * 3.0; + double expected_step = full_max_step * 0.5; + EXPECT_NEAR( hw_cmd_values_[0], expected_step, 1e-6 ) + << "With directional scaling disabled, should always use distance-based scale"; +} + +// ============================================================================ +// Current Limits Tests +// ============================================================================ + +TEST_F( SafetyPositionControllerTest, CurrentLimitsWriteStiffByDefault ) +{ + initController( {}, /*check_self_collisions=*/false, /*set_current_limits=*/true ); + configureController(); + + // Setup hardware interfaces with current command interfaces + auto cj = controlled_joints_; + hw_state_values_.assign( controller_->all_joint_names_.size(), 0.0 ); + hw_cmd_values_.assign( cj.size() * 2, 0.0 ); // position + current + + cmd_ifaces_.clear(); + state_ifaces_.clear(); + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + for ( size_t i = 0; i < cj.size(); ++i ) { + cmd_ifaces_.push_back( std::make_shared( + cj[i], "position", &hw_cmd_values_[i] ) ); + } + for ( size_t i = 0; i < cj.size(); ++i ) { + cmd_ifaces_.push_back( std::make_shared( + cj[i], "current", &hw_cmd_values_[cj.size() + i] ) ); + } + for ( size_t i = 0; i < controller_->all_joint_names_.size(); ++i ) { + state_ifaces_.push_back( std::make_shared( + controller_->all_joint_names_[i], "position", &hw_state_values_[i] ) ); + } +#pragma GCC diagnostic pop + + controller_->command_interfaces_.clear(); + controller_->state_interfaces_.clear(); + for ( auto &ci : cmd_ifaces_ ) { + controller_->command_interfaces_.emplace_back( ci, []() { } ); + } + for ( auto &si : state_ifaces_ ) { controller_->state_interfaces_.emplace_back( si ); } + + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // Default mode is stiff -> current limits should be stiff_limit (5.0) + for ( size_t i = 0; i < cj.size(); ++i ) { + EXPECT_DOUBLE_EQ( hw_cmd_values_[cj.size() + i], 5.0 ) + << "Joint " << cj[i] << " should have stiff current limit"; + } +} + +TEST_F( SafetyPositionControllerTest, CurrentLimitsWriteCompliantWhenEnabled ) +{ + initController( {}, /*check_self_collisions=*/false, /*set_current_limits=*/true ); + configureController(); + + auto cj = controlled_joints_; + hw_state_values_.assign( controller_->all_joint_names_.size(), 0.0 ); + hw_cmd_values_.assign( cj.size() * 2, 0.0 ); + + cmd_ifaces_.clear(); + state_ifaces_.clear(); + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + for ( size_t i = 0; i < cj.size(); ++i ) { + cmd_ifaces_.push_back( std::make_shared( + cj[i], "position", &hw_cmd_values_[i] ) ); + } + for ( size_t i = 0; i < cj.size(); ++i ) { + cmd_ifaces_.push_back( std::make_shared( + cj[i], "current", &hw_cmd_values_[cj.size() + i] ) ); + } + for ( size_t i = 0; i < controller_->all_joint_names_.size(); ++i ) { + state_ifaces_.push_back( std::make_shared( + controller_->all_joint_names_[i], "position", &hw_state_values_[i] ) ); + } +#pragma GCC diagnostic pop + + controller_->command_interfaces_.clear(); + controller_->state_interfaces_.clear(); + for ( auto &ci : cmd_ifaces_ ) { + controller_->command_interfaces_.emplace_back( ci, []() { } ); + } + for ( auto &si : state_ifaces_ ) { controller_->state_interfaces_.emplace_back( si ); } + + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + // Enable compliant mode + controller_->in_compliant_mode_.store( true ); + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // Compliant mode -> current limits should be compliant_limit (3.0) + for ( size_t i = 0; i < cj.size(); ++i ) { + EXPECT_DOUBLE_EQ( hw_cmd_values_[cj.size() + i], 3.0 ) + << "Joint " << cj[i] << " should have compliant current limit"; + } +} + +// ============================================================================ +// Safety Bypass Joint Limit Tolerance Tests +// ============================================================================ + +TEST_F( SafetyPositionControllerTest, SafetyBypassRelaxesJointLimits ) +{ + initController(); + configureController(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + for ( auto &v : hw_state_values_ ) v = 0.0; + + // joint2 upper limit is 1.5, range = 3.0, tolerance = 3% -> 0.09 extra + // Without bypass: command beyond 1.5 should clamp to 1.5 + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 1.55; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + EXPECT_LE( hw_cmd_values_[1], 1.5 ) << "Without bypass, should clamp to upper limit"; + + // Enable bypass + controller_->safety_bypass_active_.store( true ); + + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 1.55; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + + // With bypass (tolerance = 3% of range 3.0 = 0.09), upper limit becomes 1.59 + // 1.55 is within [1.5, 1.59] so it should pass through + EXPECT_GT( hw_cmd_values_[1], 1.5 ) + << "With bypass, commands slightly beyond normal limits should be allowed"; + EXPECT_NEAR( hw_cmd_values_[1], 1.55, 1e-6 ); +} + +// ============================================================================ +// on_activate Validation Tests +// ============================================================================ + +TEST_F( SafetyPositionControllerCollisionTest, ActivateFailsWhenSafetyZoneLessThanPadding ) +{ + // collision_safety_zone (0.01) <= collision_padding (0.05) -> should fail + initWithCollisions( {}, "test_robot_collision.urdf", + { rclcpp::Parameter( "collision_safety_zone", 0.01 ), + rclcpp::Parameter( "collision_padding", 0.05 ) } ); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + + // on_activate should fail validation + rclcpp_lifecycle::State inactive( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + "inactive" ); + auto cb = controller_->on_activate( inactive ); + EXPECT_EQ( cb, controller_interface::CallbackReturn::ERROR ); +} + +TEST_F( SafetyPositionControllerCollisionTest, ActivateFailsWhenSafetyZoneEqualsPadding ) +{ + initWithCollisions( {}, "test_robot_collision.urdf", + { rclcpp::Parameter( "collision_safety_zone", 0.05 ), + rclcpp::Parameter( "collision_padding", 0.05 ) } ); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + + rclcpp_lifecycle::State inactive( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + "inactive" ); + auto cb = controller_->on_activate( inactive ); + EXPECT_EQ( cb, controller_interface::CallbackReturn::ERROR ); +} + +// ============================================================================ +// Safety Bypass Skips Collision But Still Limits Velocity +// ============================================================================ + +TEST_F( SafetyPositionControllerCollisionTest, BypassSkipsCollisionButAllowsRelaxedLimits ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + controller_->safety_bypass_active_.store( true ); + + // Start at a colliding configuration + setStateValue( "joint1", 0.0 ); + setStateValue( "joint2", M_PI ); + setStateValue( "joint3", -M_PI ); + + // Command to the colliding position + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = M_PI; + controller_->reference_interfaces_[2] = -M_PI; + + auto ret = callUpdate(); + EXPECT_EQ( ret, controller_interface::return_type::OK ); + + // With bypass, commands should pass through (no collision blocking, no velocity limiting) + EXPECT_NEAR( hw_cmd_values_[1], M_PI, 1e-6 ) + << "Bypass should allow commands even in colliding configuration"; +} + +// ============================================================================ +// Collision Checker Inf Input Test +// ============================================================================ + +TEST_F( SafetyPositionControllerCollisionTest, InfInputDetectedAsCollision ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + for ( auto &v : hw_state_values_ ) v = 0.0; + + // Command with infinity — should be treated as collision (safe default) + controller_->reference_interfaces_[0] = std::numeric_limits::infinity(); + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + // First need to call update so collision checker sees the inf + // The inf will be clamped by enforce_limits (joint1 has limits [-pi, pi]) + // But the collision checker receives cc_positions which includes the clamped value + // So this tests the controller's overall handling — commands should still be safe + auto ret = callUpdate(); + EXPECT_EQ( ret, controller_interface::return_type::OK ); +} + +// ============================================================================ +// E-stop holds positions across multiple cycles +// ============================================================================ + +TEST_F( SafetyPositionControllerTest, EstopHoldsAcrossMultipleCycles ) +{ + initController(); + configureController(); + setupHardwareInterfaces(); + findMocks(); + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ).Times( ::testing::AnyNumber() ); + activateController(); + + setStateValue( "joint1", 0.5 ); + setStateValue( "joint2", -0.3 ); + setStateValue( "joint3", 1.0 ); + + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + // First update to read positions + callUpdate(); + + sendEstop( true ); + callUpdate(); + + // Commands should be held positions regardless of what reference says + controller_->reference_interfaces_[0] = 999.0; + controller_->reference_interfaces_[1] = 999.0; + controller_->reference_interfaces_[2] = 999.0; + + callUpdate(); + + EXPECT_DOUBLE_EQ( hw_cmd_values_[0], 0.5 ); + EXPECT_DOUBLE_EQ( hw_cmd_values_[1], -0.3 ); + EXPECT_DOUBLE_EQ( hw_cmd_values_[2], 1.0 ); + + // Third cycle still holds + callUpdate(); + EXPECT_DOUBLE_EQ( hw_cmd_values_[0], 0.5 ); +} + +// ============================================================================ +// Collision test: distance-based scaling status fields +// ============================================================================ + +TEST_F( SafetyPositionControllerCollisionTest, StatusReportsDistanceScalingFields ) +{ + initWithCollisions(); + configureWithSrdf(); + setupHardwareInterfaces(); + findMocks(); + + SafetyPositionControllerStatus captured; + EXPECT_CALL( *status_pub_mock_, publish( ::testing::_ ) ) + .WillRepeatedly( [&captured]( const auto &msg ) { captured = msg; } ); + + activateController(); + + // Set last_min_distance_ halfway in safety zone + controller_->last_min_distance_ = 0.025; + + for ( auto &v : hw_state_values_ ) v = 0.0; + controller_->reference_interfaces_[0] = 0.01; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + callUpdate(); + controller_->publish_status(); + + EXPECT_TRUE( captured.collision_check_enabled ); + EXPECT_NEAR( captured.distance_scale, 0.5, 1e-6 ); +} + // ============================================================================ // main // ============================================================================ diff --git a/hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg b/hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg index 33b38b5..c10cd74 100644 --- a/hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg +++ b/hector_ros_controllers_msgs/msg/SafetyPositionControllerStatus.msg @@ -15,3 +15,17 @@ bool estop_engaged # Position limit enforcement bool position_limits_enforced # true if enforce_position_limits param is on + +# Directional collision scaling diagnostics +float64 min_collision_distance # current minimum collision distance [m] +float64 distance_scale # raw distance-based velocity scale [0,1] +float64 effective_scale # after directional override [0,1] +float64 worst_directional_derivative # min dot product across safety-zone pairs (positive = away) +uint32 num_pairs_in_safety_zone # number of collision pairs in the safety zone + +# Manipulability index (Yoshikawa w = sqrt(det(J*J^T)), 0 = singular) +float64 manipulability + +# Active current limits per joint (only meaningful when current_limits_enabled is true) +string[] joint_names # joint names (same order as current_limits) +float64[] current_limits # active current limit per joint [A] (compliant or stiff)