closed_chain_motion_planner