Optimal Control for Articulated Soft Robots