Finish implementation of manipulation solver
Change constraints handling in manipulationSolver

Use std::map instead of std::vector to store and retrieve
constraints.