feat: Integrate Rotational CCD using compas-forge for motion planning

[moaminmo90]

Summary

This Pull Request addresses the TODO regarding sweep_collision_fn inside motion planning loops (specifically in pybullet_plan_motion.py and pybullet_plan_cartesian_motion.py). By integrating the Rust-backed compas-forge engine, we introduce Rotational Continuous Collision Detection (CCD) with temporal sub-stepping. This approach aims to mitigate the "tunneling effect" during continuous rotary joint movements while maintaining computational efficiency.

Technical Implementation & Optimizations

To optimize performance inside the planning loops, the implementation avoids repeated mesh reconstruction by utilizing a caching mechanism:

1. Mesh Registration (pybullet_set_robot_cell.py): Collision meshes for robot links, attached tools, and stationary rigid obstacles are registered once inside compas-forge's thread-safe memory registry during set_robot_cell.
2. Pose Tracking (pybullet_set_robot_cell_state.py): Global frames of tools and rigid bodies are updated during cell state transitions and cached in client._forge_object_poses.
3. Collision Evaluation (pybullet_plan_cartesian_motion.py): Within the Cartesian path planning loop, the planner queries compas_forge.check_swept_collision_cached using string IDs and calculated start/end poses.

Preliminary Benchmarks

Initial evaluations were conducted on a standard UR5 robot cell setup with a stationary obstacle:

• A single swept-collision query (including SLERP/LERP pose interpolation, 10 piecewise-linear sub-steps, and GJK/EPA continuous solver) executes in approximately 0.2 ms on a Windows AMD64 environment.
• This efficiency allows continuous collision validation within the planning loop with low computational overhead to the DFS solver.

Backward Compatibility & Fallback

If compas-forge is not installed in the local environment, the planner gracefully falls back to the default discrete collision checking, ensuring no breaking changes to existing COMPAS core APIs.

We welcome your feedback and review on this integration for compas_fab!