In the current implementation, there is no possibility to wraparound joint angles. Add this functionality from ROS1 version of the controller. Start at this line.
also in #85 ?