[MarcioCamposJr]

Description

This PR implements coil-to-coil collision detection and avoidance directly in the robot controller.
InVesalius sends the registrations and tracker poses of two coils. Each robot controller identifies its associated coil through coil_idx. This supports both two-robot operation and a setup with one connected robot and a second manually operated coil.

Main changes

• Calculates the minimum distance between two coils locally in the robot controller.
• Represents each coil as an oriented bounding box estimated from its registered fiducials.
• Adds configurable bounding-box expansion:
  • 1.5 mm on each lateral face;
  • 3 mm on the front and back faces.
• Calculates the separation vector using the closest points between the coil surfaces rather than the raw tracker positions.
• Selects the correct repulsion direction according to the coil associated with each robot.
• Implements three collision-avoidance stages:
  • APPROACH: early preventive response;
  • WORKING: stronger repulsion;
  • STOP: emergency stop.
• Automatically releases a latched collision stop after the coils return to a safe distance.
• Stops automatic motion when collision measurements become stale.
• Estimates relative closing speed from the geometric centers of the coil boxes.
• Dynamically increases the safety clearance according to the closing speed.
• Constrains the repulsion vector so that it cannot move the robot toward the patient’s head.
• Uses the current head pose received with the coil tracker data.
• Stops the robot when no direction can safely separate the coils without moving toward the head.
• Applies collision avoidance through a common supervisor compatible with:
  • directly_PID;
  • directly_upward;
  • radially_outward.
• Preserves the tool orientation during a repulsion movement.
• Does not perform automatic repulsion when no automatic objective is active, preventing conflicts with free-drive mode.

Configuration

Collision distances, repulsion strength, smoothing, measurement timeout, reaction time, safe deceleration, and maximum closing speed can be adjusted through COLLISION_AVOIDANCE_CONFIG.
The estimated coil box expansion can be adjusted through COIL_COLLISION_GEOMETRY_CONFIG.
The current values are initial parameters and should be calibrated and validated on the physical system.

Current limitations

• Exactly two coils are supported.
• Both coils must remain visible for distance updates.
• The head marker must also be visible while repulsion is active.
• If the required coil-separation direction conflicts with head avoidance and no safe alternative exists, the robot is stopped.
• Support for three or more coils is outside the scope of this PR.