This paper develops high-accuracy methods for the numerical solution of optimal control problems subject to nonsmooth differential equations with set-valued Heaviside step functions. An important subclass of these systems are Filippov systems. By writing the Heaviside step function as the solution map of a linear program and using its optimality conditions, the initial nonsmooth system is rewritten into an equivalent Dynamic Complementarity System (DCS). The Finite Elements with Switch Detection (FESD) method (Nurkanović et al., 2024) was originally developed for Filippov systems transformed via Stewart’s reformulation into DCS (Stewart, 1990). This paper extends it to the above mentioned class of nonsmooth systems. The key ideas are to start with a standard Runge–Kutta method for the DCS and to let the integration step sizes to be degrees of freedom. Then, additional conditions are introduced to allow implicit but accurate switch detection and to remove possible spurious degrees of freedom when no switches occur. The theoretical properties of the FESD method are studied. The motivation for these developments is to obtain a computationally tractable formulation of nonsmooth optimal control problems. Numerical simulations and optimal control examples are used to illustrate the favorable properties of the proposed approach. All methods introduced in this paper are implemented in the open-source software package nosnoc (Nurkanović and Diehl, 2022).