Real-time Detailed Self-collision Avoidance in Whole-body Model Predictive Control

This paper proposes a novel self-collision avoidance (SCA) scheme for whole-body model predictive control (WB-MPC). Since WB-MPC deals with a large-scale optimization problem, which becomes larger as the target robot is with larger degrees of freedom, the derivatives of the dynamics and cost functions should be computed as fast as possible. As SCA using detailed collision bodies is computationally expensive, it is challenging to embed SCA in WB-MPC with such a requirement. One way to solve this open issue is to approximate the robot model with primitive shapes, but this method accumulates modeling errors. To make derivative calculations of a detailed collision model fast in real-time, we develop a frame-distance predictor using a deep neural network (DNN). The DNN-based frame-distance predictor estimates the minimum distance between all body frames based on the whole-body joint angles. The minimum distance estimated can be embedded into WB-MPC as one of the cost functions in a differentiable manner. As a result, the WB-MPC with the DNN-based frame-distance predictor is able to plan long-term self-collision-aware motions in real-time. The proposed method is evaluated using a dual-arm robot both in simulation and real environment. The results show that the proposed method achieves more accurate tracking motions while satisfying SCA than the case with the approximated collision model.