First-Principles Models and Safety-Driven Planning for Soft and Rigid Robots