AVX2-Accelerated Collision Detection for Real-Time MPC in Humanoid Robotics

This project aims to resolve the computational bottleneck in self-collision detection within the OCS2 framework for humanoid robotics. The current implementation in ocs2_self_collision operates on a pair-by-pair basis, iterating through collision pairs and calling PinocchioGeometryInterface::computeDistances() followed by Jacobian and penalty term calculations. This sequential approach is not SIMD-friendly by default and fails to utilize the parallel processing power of modern CPUs. To address this, I propose replacing the current computeDistances()-based distance computation with a high-performance implementation inspired by SIMD-amenable structures. A primary focus will be exploring / integrating SIMD-friendly data structures, such as Collision-Affording Point Trees (CAPT), to represent the storage and computation of collisions. This transition aims to transform the collision pipeline into a vectorized workflow that can be efficiently accelerated using AVX2 intrinsics. The primary deliverables include the clean integration of this accelerated engine into the ocs2_self_collision pipeline, accompanied by rigorous correctness checks against Pinocchio/FCL baselines. Furthermore, I will provide a comprehensive end-to-end latency evaluation using ocs2_mobile_manipulator to demonstrate the reduction in average and maximum compute cycle times. This work will significantly enhance the real-time performance of MPC-based humanoid control in complex environments.