  Collisions are common in many dynamical systems with real applications. They
can be formulated as hybrid dynamical systems with discontinuities
automatically triggered when states transverse certain manifolds. We present an
algorithm for the optimal control problem of such hybrid dynamical systems
based on solving the equations derived from the hybrid minimum principle (HMP).
The algorithm is an iterative scheme following the spirit of the method of
successive approximations (MSA), and it is robust to undesired collisions
observed in the initial guesses. We analyze the discontinuities in the system
and propose a stable collision condition, which is crucial for the convergence
of iterative algorithms in systems experiencing collisions. Subsequently, we
establish a convergence theorem demonstrating linear convergence for the MSA
algorithm when collisions are present. We also address several numerical
challenges introduced by the discontinuities. The algorithm is tested on disc
collision problems whose optimal solutions exhibit one or multiple collisions.
Linear convergence in terms of iteration steps and asymptotic first-order
accuracy in terms of time discretization are observed when the algorithm is
implemented with the forward-Euler scheme. The numerical results demonstrate
that the proposed algorithm has better accuracy and convergence than direct
methods based on gradient descent. Furthermore, the algorithm is also simpler,
more accurate, and more stable than a deep reinforcement learning method.
