Motion Planning for Cooperative Robotic Systems Performing Contact Operations