Distance-Based Velocity Scaling for SafetyPositionController

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(

hector_ros_controllers/include/safety_position_controller/collision_checker.hpp

@@ -7,16 +7,37 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <realtime_tools/realtime_publisher.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <pinocchio/multibody/data.hpp>
 #include <pinocchio/multibody/geometry.hpp>
 #include <pinocchio/multibody/model.hpp>
 
+#include <pinocchio/collision/broadphase-manager.hpp>
+
+#include <Eigen/Core>
+#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
 #include <hpp/fcl/collision.h>
+#include <limits>
+#include <memory>
 #include <unordered_map>
 
-// #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<double>::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<PairInfo> 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<std::string, double> &joint_positions );
+  CollisionResult checkCollision( const std::unordered_map<std::string, double> &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<double> &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<realtime_tools::RealtimePublisher<visualization_msgs::msg::MarkerArray>> rt_markers_pub_;
 
   std::unordered_map<std::string, pinocchio::JointIndex> name_to_id_;
 
+  // Per-pair directional derivatives for visualization (set by controller via setDirectionalInfo)
+  std::vector<double> 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<hpp::fcl::DynamicAABBTreeCollisionManager>;
+  std::unique_ptr<BroadPhaseManager> 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
 };
 

hector_ros_controllers/include/safety_position_controller/safety_position_controller.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <atomic>
 #include <controller_interface/chainable_controller_interface.hpp>
 #include <realtime_tools/realtime_buffer.hpp>
 #include <safety_position_controller/collision_checker.hpp>
@@ -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<double> &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<bool> in_compliant_mode_{ false }; ///< selects compliant vs. stiff current limits
 
   // ---- E-stop ----
   std::atomic<bool> estop_active_{ false };  ///< last requested E-stop state
@@ -221,13 +231,23 @@ class SafetyPositionController final : public controller_interface::ChainableCon
   std::unordered_map<std::string, double> cc_positions_; ///< name→position map for CC
   std::unique_ptr<CollisionChecker> collision_checker_;  ///< optional self-collision checker
   std::string srdf_;                                     ///< SRDF XML (semantic)
-  bool srdf_received_ = false;                           ///< latched SRDF received
+  std::atomic<bool> srdf_received_{ false };             ///< latched SRDF received
+  double last_min_distance_{
+      std::numeric_limits<double>::max() }; ///< previous cycle's min collision distance
+  double last_manipulability_{ 0.0 };       ///< latest Yoshikawa manipulability index
+
+  // ---- Directional collision scaling ----
+  std::vector<int> joint_v_index_; ///< maps controlled joint index → pinocchio velocity-space index
+  std::vector<CollisionResult::PairInfo>
+      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<double>::max() };
 
   // ---- Command/state buffers (aligned with params_.joints) ----
   std::vector<double> cmd_positions_;     ///< post-enforcement commands
   std::vector<double> current_positions_; ///< latest measured positions
-  bool on_hold_{ false };                 ///< non-chained fallback
-  std::vector<double> hold_positions_;    ///< positions to hold when not chained
+  std::vector<double> hold_positions_;    ///< positions to hold during E-stop
 
   // ---- Parameters ----
   std::shared_ptr<ParamListener> param_listener_;
@@ -252,8 +272,9 @@ class SafetyPositionController final : public controller_interface::ChainableCon
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr debug_in_js_pub_;
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr debug_out_js_pub_;
 
-  // Status publisher (latched)
+  // Status publisher (latched) + periodic timer
   rclcpp::Publisher<hector_ros_controllers_msgs::msg::SafetyPositionControllerStatus>::SharedPtr status_pub_;
+  rclcpp::TimerBase::SharedPtr status_timer_;
 
   static constexpr int throttle_logging_msg = 2000; ///< ms; throttle for WARN/ERROR logs
 };

hector_ros_controllers/package.xml

@@ -31,6 +31,7 @@
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_index_cpp</test_depend>
   <test_depend>lifecycle_msgs</test_depend>
   <test_depend>rtest</test_depend>