Constraint Manifold Optimization for Robotic Inference and Planning With Constraints