Merge pull request #10 from tu-darmstadt-ros-pkg/feature/safety_position_controller

Feature/safety position controller

Author: Excelsius314

README.md

@@ -3,9 +3,16 @@
 Simple controllers for the [ros2_control](https://control.ros.org/jazzy/index.html) framework.
 
 ## Controllers
-1. Safety Forward Controller: A simple forward controller that resets the command value to zero if no command is received within a specified timeout period.
-2. Self Collision Avoidance Controller: A controller that prevents self-collision by checking the robot's state and if a nearby collision is detected, it makes sure to prevent the robot from moving into that state.
+
+1. Safety Forward Controller: A simple forward controller that resets the command value to zero if no command is
+   received within a specified timeout period.
+2. Self Collision Avoidance Controller: A controller that prevents self-collision by checking the robot's state and if a
+   nearby collision is detected, it makes sure to prevent the robot from moving into that state.
+3. Safety Position Controller: Unwraps joint values for continuous joint such that the command position is in (-inf, inf)
+   and closest to the current value. Normal joint positions are clamped to their limits.
 
 # hector_controller_spawner
-A lightweight ROS2 node that boots an entire *ros2_control* setup in a single shot, managing hardware interfaces and controllers efficiently.
+
+A lightweight ROS2 node that boots an entire *ros2_control* setup in a single shot, managing hardware interfaces and
+controllers efficiently.
 See [README.md](hector_controller_spawner/README.md) for details.

hector_ros_controllers/CMakeLists.txt

@@ -24,17 +24,22 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     generate_parameter_library
     hardware_interface
     pluginlib
+    hpp-fcl
+    pinocchio
     rclcpp
     rclcpp_lifecycle
     realtime_tools
     std_msgs
+    std_srvs
     fcl
     ad_kinematics)
 
 foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${dependency} REQUIRED)
 endforeach()
 
+set(DEFS PINOCCHIO_WITH_HPP_FCL PINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR
+         PINOCCHIO_URDFDOM_USE_STD_SHARED_PTR)
 # Add parameter_libraries here
 
 generate_parameter_library(
@@ -48,14 +53,18 @@ generate_parameter_library(
 generate_parameter_library(safety_forward_controller_parameters
                            params/safety_forward_controller_parameters.yaml)
 
+generate_parameter_library(safety_position_controller_parameters
+                           params/safety_position_controller_parameters.yaml)
 # Add controllers here Define Controllers as Shared Libraries
 
 add_library(
   controllers SHARED
   src/velocity_to_position_controllers_base.cpp
   src/safety_forward_controller.cpp
   src/velocity_to_position_command_controller.cpp
-  src/self_collision_avoidance_controller.cpp)
+  src/self_collision_avoidance_controller.cpp
+  src/safety_position_controller.cpp
+  src/collision_checker.cpp)
 
 target_compile_features(controllers PUBLIC cxx_std_17)
 
@@ -65,26 +74,27 @@ target_include_directories(
 
 target_link_libraries(
   controllers
-  PUBLIC fcl velocity_to_position_command_controller_parameters
+  PUBLIC fcl
+         velocity_to_position_command_controller_parameters
          self_collision_avoidance_controller_parameters
-         safety_forward_controller_parameters)
+         safety_forward_controller_parameters
+         safety_position_controller_parameters)
 
 target_link_libraries(controllers PUBLIC ad_kinematics::ad_kinematics)
 ament_target_dependencies(controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-# Causes the visibility macros to use dllexport rather than dllimport, which is
-# appropriate when building the dll but not consuming it.
-target_compile_definitions(
-  controllers PRIVATE "VELOCITY_TO_POSITION_COMMAND_CONTROLLER_BUILDING_DLL")
+target_compile_definitions(controllers PUBLIC ${DEFS})
 
 pluginlib_export_plugin_description_file(controller_interface
                                          hector_ros_controller_plugin.xml)
 
 install(DIRECTORY include/ DESTINATION include/)
 install(
-  TARGETS controllers velocity_to_position_command_controller_parameters
+  TARGETS controllers
+          velocity_to_position_command_controller_parameters
           self_collision_avoidance_controller_parameters
           safety_forward_controller_parameters
+          safety_position_controller_parameters
   EXPORT export_hector_ros_controllers
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib

hector_ros_controllers/README.md

@@ -0,0 +1,112 @@
+# Hector Ros Controllers
+
+A set of ROS 2 controllers focused on **safety**, **self-collision avoidance**, and **safe position command handling**.
+All controllers are compatible with **ros2_control** and can be inserted as **chain controllers**.
+
+---
+
+## 1. Safety Forward Controller
+
+Forwards incoming commands, but **resets the command to zero** if no message is received within a configured timeout.
+Useful for teleoperation and autonomy if the communication is unreliable or in case of node failures.
+
+---
+
+## 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. Safety Position Controller
+
+A **chainable safety layer** for joint position commands.
+
+### Features
+
+* **Continuous joints unwrapped** to nearest equivalent angle.
+* **Joint limits enforced** directly from URDF.
+* **Target-only self-collision check** (Pinocchio + hpp-fcl).
+* **Block-if-too-far:** prevents large unsafe jumps and makes collision checking meaningful.
+* Optional **compliant / stiff current limiting** per joint.
+
+### Collision Checking Limitation - target-only collision checking
+
+If used **behind a trajectory controller**, target updates are **small steps**, so checking only the target pose is sufficient when using a **small collision padding**.
+
+If the parent controller is a raw position controller (large jumps), **block-if-too-far** ensures targets stay close → prevents missing intermediate collisions.
+
+**Future improvement:** continuous / path collision checking.
+
+
+## Parameters (Safety Position Controller)
+
+| Parameter                          | Type       | Default    | Description                                                                                          |
+| ---------------------------------- | ---------- | ---------- | ---------------------------------------------------------------------------------------------------- |
+| `joints`                           | `string[]` | `[]`       | Names of joints controlled. Must match URDF. *(read-only)*                                           |
+| `unwrap_continuous_joints`         | `bool`     | `true`     | Unwrap continuous joints to maintain continuity in commanded angle.                                  |
+| `enforce_position_limits`          | `bool`     | `true`     | Clamp joint commands to URDF limits.                                                                 |
+| `check_self_collisions`            | `bool`     | `true`     | Run self-collision check on the **target** pose before sending commands.                             |
+| `collision_padding`                | `double`   | `0.0`      | Minimum allowed link-to-link distance [m] — distances ≤ padding count as collision.                  |
+| `collision_cache_epsilon`          | `double`   | `0.000001` | Threshold for reusing last collision check result (skip compute if target change is small).          |
+| `block_if_too_far`                 | `bool`     | `true`     | Blocks commands that are too far from current state. Auto-enabled if `check_self_collisions = true`. |
+| `block_velocity_scaling`           | `double`   | `1.5`      | Scales maximum per-cycle motion: blocked if `                                                        |
+| `debug_visualize_collisions`       | `bool`     | `false`    | Publishes collision debug markers for RViz.                                                          |
+| `set_current_limits`               | `bool`     | `false`    | Enables per-joint current limit control. *(read-only)*                                               |
+| `current_limits.*.compliant_limit` | `double`   | `3.0`      | Current limit in **compliant** mode [A].                                                             |
+| `current_limits.*.stiff_limit`     | `double`   | `5.0`      | Current limit in **stiff** mode [A].                                                                 |
+
+> Note: When using dynamic reconfigure to change the parameters the controller must be deactivated and reactivated for changes to take effect!
+
+
+## Example Configuration
+
+```yaml
+/**:
+  controller_manager:
+    ros__parameters:
+      update_rate: 50
+
+      joint_state_broadcaster:
+        type: joint_state_broadcaster/JointStateBroadcaster
+
+      arm_trajectory_controller:
+        type: joint_trajectory_controller/JointTrajectoryController
+
+      arm_safety_position_controller:
+        type: safety_position_controller/SafetyPositionController
+
+  arm_safety_position_controller:
+    ros__parameters:
+      joints: [arm_joint_1, arm_joint_2, arm_joint_3, arm_joint_4, arm_joint_5, arm_joint_6, arm_joint_7]
+      unwrap_continuous_joints: true
+      enforce_position_limits: true
+      check_self_collisions: true
+      collision_padding: 0.01
+      debug_visualize_collisions: false
+      block_velocity_scaling: 3.0
+      set_current_limits: true
+      current_limits:
+        arm_joint_1: {compliant_limit: 3.0, stiff_limit: 9.0}
+        arm_joint_2: {compliant_limit: 5.0, stiff_limit: 9.0}
+        arm_joint_3: {compliant_limit: 5.0, stiff_limit: 9.0}
+        arm_joint_4: {compliant_limit: 3.0, stiff_limit: 4.9}
+        arm_joint_5: {compliant_limit: 2.5, stiff_limit: 5.5}
+        arm_joint_6: {compliant_limit: 0.5, stiff_limit: 3.5}
+        arm_joint_7: {compliant_limit: 0.5, stiff_limit: 3.5}
+
+  arm_trajectory_controller:
+    ros__parameters:
+      joints: [arm_joint_1, arm_joint_2, arm_joint_3, arm_joint_4, arm_joint_5, arm_joint_6, arm_joint_7]
+      command_joints:
+      - arm_safety_position_controller/arm_joint_1
+      - arm_safety_position_controller/arm_joint_2
+      - arm_safety_position_controller/arm_joint_3
+      - arm_safety_position_controller/arm_joint_4
+      - arm_safety_position_controller/arm_joint_5
+      - arm_safety_position_controller/arm_joint_6
+      - arm_safety_position_controller/arm_joint_7
+      command_interfaces: [position]
+      state_interfaces: [position, velocity]
+```

hector_ros_controllers/hector_ros_controller_plugin.xml

@@ -1,23 +1,31 @@
 <library path="libcontrollers">
-  <class name="velocity_to_position_command_controller/VelocityToPositionCommandController"
-         type="velocity_to_position_command_controller::VelocityToPositionCommandController"
-         base_class_type="controller_interface::ChainableControllerInterface">
-    <description>
-      The velocity to position command controller provides a velocity input and converts it to position commands.
-    </description>
-  </class>
-  <class name="self_collision_avoidance_controller/SelfCollisionAvoidanceController"
-         type="self_collision_avoidance_controller::SelfCollisionAvoidanceController"
-         base_class_type="controller_interface::ChainableControllerInterface">
-    <description>
-      A chainable controller that only forwards joint commands if the resulting configuration is collision free.
-    </description>
-  </class>
-  <class name="safety_forward_controller/SafetyForwardController"
-         type="safety_forward_controller::SafetyForwardController"
-         base_class_type="controller_interface::ControllerInterface">
-    <description>
-    A high-level forward controller ensuring that all command interfaces are stopped after receiving now commands for a specified time, i.e. disconnect to operator station.
-    </description>
-  </class>
+    <class name="velocity_to_position_command_controller/VelocityToPositionCommandController"
+           type="velocity_to_position_command_controller::VelocityToPositionCommandController"
+           base_class_type="controller_interface::ChainableControllerInterface">
+        <description>
+            The velocity to position command controller provides a velocity input and converts it to position commands.
+        </description>
+    </class>
+    <class name="self_collision_avoidance_controller/SelfCollisionAvoidanceController"
+           type="self_collision_avoidance_controller::SelfCollisionAvoidanceController"
+           base_class_type="controller_interface::ChainableControllerInterface">
+        <description>
+            A chainable controller that only forwards joint commands if the resulting configuration is collision free.
+        </description>
+    </class>
+    <class name="safety_forward_controller/SafetyForwardController"
+           type="safety_forward_controller::SafetyForwardController"
+           base_class_type="controller_interface::ControllerInterface">
+        <description>
+            A high-level forward controller ensuring that all command interfaces are stopped after receiving now
+            commands for a specified time, i.e. disconnect to operator station.
+        </description>
+    </class>
+    <class name="safety_position_controller/SafetyPositionController"
+           type="safety_position_controller::SafetyPositionController"
+           base_class_type="controller_interface::ChainableControllerInterface">
+        <description>
+            A chainable position controller ensuring that revolute joints are unwrapped and joint limits are satisfied.
+        </description>
+    </class>
 </library>

hector_ros_controllers/include/safety_position_controller/collision_checker.hpp

@@ -0,0 +1,130 @@
+//
+// Created by aljoscha-schmidt on 10/20/25.
+//
+
+#ifndef COLLISION_CHECKER_HPP
+#define COLLISION_CHECKER_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+#include <pinocchio/multibody/model.hpp>
+
+#include <hpp/fcl/collision.h>
+#include <unordered_map>
+
+// #define SAFETY_CC_ENABLE_TIMING
+
+/// Self-collision checker using Pinocchio + hpp-fcl; optional RViz debug markers.
+class CollisionChecker
+{
+public:
+  /**
+   * @brief Ctor.
+   * @param node lifecycle node (pub/log/time)
+   * @param collision_padding min allowed distance [m]
+   * @param collision_cache_epsilon cache threshold on max(q - q_last) [rad/m] -> reuse last distances
+   * @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,
+                             bool pub_debug_geometry = false );
+
+  /**
+   * @brief Init from URDF/SRDF XML.
+   * - builds Model/Geometry(COLLISION)
+   * - prunes pairs via SRDF + controlled_joints ancestry
+   * @param urdf_xml URDF string
+   * @param srdf_xml SRDF string (may be empty)
+   * @param controlled_joints names used to keep relevant pairs (empty → keep all)
+   * @param free_flyer root as free-flyer
+   * @return true on success
+   */
+  bool initFromXml( const std::string &urdf_xml, const std::string &srdf_xml,
+                    const std::vector<std::string> &controlled_joints, bool free_flyer = false );
+
+  /**
+   * @brief Joint names excluding "universe".
+   * @return names in model order
+   */
+  std::vector<std::string> getJointNames() const;
+
+  /**
+   * @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
+   */
+  bool checkCollision( const std::unordered_map<std::string, double> &joint_positions );
+
+  /**
+   * @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
+   */
+  bool checkCollisionQ( const Eigen::VectorXd &q );
+
+  /**
+   * @brief Set collision padding [m].
+   * @param collision_padding new threshold
+   */
+  void updateCollisionPadding( double collision_padding );
+
+  /**
+   * @brief Toggle RViz debug publishing.
+   * @param pub_debug_geometry on/off
+   */
+  void updateDoDebugVisualization( bool pub_debug_geometry );
+
+  /**
+   * @brief Set cache epsilon
+   * @param epsilon new threshold
+   */
+  void updateCollisionCacheEpsilon( double epsilon );
+
+private:
+  /**
+   * @brief Publish geometry and nearest-point LINE_LIST markers.
+   * - red if part of a pair with distance ≤ 0
+   */
+  void publishMarkers() const;
+
+  /**
+   * @brief Keep only pairs attached to controlled joints (and ancestors).
+   * @param controlled_joints names defining relevance
+   */
+  void filterCollisionPairs( const std::vector<std::string> &controlled_joints );
+
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markers_pub_;
+
+  pinocchio::Model model_;
+  pinocchio::Data data_;
+  pinocchio::GeometryModel geom_model_;
+  pinocchio::GeometryData geom_data_;
+
+  Eigen::VectorXd q_default_;
+  Eigen::VectorXd q_last_;
+  bool last_collision_state_{ false };
+
+  double collision_padding_{ 0.0 };
+  double collision_cache_epsilon_{ 1e-4 };
+  bool pub_debug_geometry_{ false };
+
+  std::unordered_map<std::string, pinocchio::JointIndex> name_to_id_;
+
+#ifdef SAFETY_CC_ENABLE_TIMING
+  double sum_timings_{ 0.0 };
+  int n_timings_{ 0 };
+#endif
+};
+
+#endif // COLLISION_CHECKER_HPP