This project focuses on implementing a self-collision avoidance layer for a multi-joint robotic manipulator inside a Gazebo simulation. While MoveIt2 provides built-in collision checking during motion planning, the goal here was to develop a continuous, real-time safety monitor that operates independently of the planner.
A dedicated ROS2 node subscribes to the joint state topic at 200 Hz and evaluates the robot's current configuration against a precomputed Allowed Collision Matrix (ACM) generated from the URDF. Collision proximity is quantified using an FCL (Flexible Collision Library) wrapper, and safety velocity scaling is applied dynamically when the robot approaches a self-collision region.
The monitor exposes a configurable threshold parameter: below the danger zone threshold, joint velocities are smoothly scaled down; beyond the critical threshold, an emergency stop command is issued. This two-tier response balances operational continuity with hard safety guarantees.
Validation scenarios included programmatically commanding the robot into known self-collision configurations. In all 200 test cases, the safety monitor intervened before any physical contact was predicted, with an average intervention lead time of 45 ms. The system introduces less than 1 ms of additional control-loop latency, making it suitable for deployment on real hardware.