diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh index 4af6c2829182f82808dbe87a17b29a565a190508..f799860ff66dbec2ec0f35192ce0180da5be0d47 100644 --- a/include/hpp/manipulation/problem-solver.hh +++ b/include/hpp/manipulation/problem-solver.hh @@ -100,6 +100,27 @@ namespace hpp { const value_type& width, const value_type& margin = 1e-4); + /// Create the grasp constraint and its complement + /// \param name name of the grasp constraint, + /// \param gripper gripper's name + /// \param handle handle's name + /// + /// Two constraints are created: + /// - "name" corresponds to the grasp constraint. + /// - "name/complement" corresponds to the complement. + void createGraspConstraint (const std::string& name, + const std::string& gripper, + const std::string& handle); + + /// Create pre-grasp constraint + /// \param name name of the grasp constraint, + /// \param gripper gripper's name + /// \param handle handle's name + /// + void createPreGraspConstraint (const std::string& name, + const std::string& gripper, + const std::string& handle); + virtual void pathValidationType (const std::string& type, const value_type& tolerance); diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 92d9364e59a37d5439d52117ce2e15942bb6decd..16339e3f84e9f90eed9bc276807466c788558a70 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -288,6 +288,34 @@ namespace hpp { addNumericalConstraint (name, NumericalConstraint::create (cvxShape)); } + void ProblemSolver::createGraspConstraint + (const std::string& name, const std::string& gripper, + const std::string& handle) + { + GripperPtr_t g = robot_->get <GripperPtr_t> (gripper); + if (!g) throw std::runtime_error ("No gripper with name " + gripper + "."); + HandlePtr_t h = robot_->get <HandlePtr_t> (handle); + if (!h) throw std::runtime_error ("No handle with name " + handle + "."); + NumericalConstraintPtr_t constraint (h->createGrasp (g)); + NumericalConstraintPtr_t complement (h->createGraspComplement (g)); + addNumericalConstraint (name, constraint); + addNumericalConstraint (name + "/complement", complement); + } + + void ProblemSolver::createPreGraspConstraint + (const std::string& name, const std::string& gripper, + const std::string& handle) + { + GripperPtr_t g = robot_->get <GripperPtr_t> (gripper); + if (!g) throw std::runtime_error ("No gripper with name " + gripper + "."); + HandlePtr_t h = robot_->get <HandlePtr_t> (handle); + if (!h) throw std::runtime_error ("No handle with name " + handle + "."); + + value_type c = h->clearance () + g->clearance (); + NumericalConstraintPtr_t constraint = h->createPreGrasp (g, c); + addNumericalConstraint (name, constraint); + } + void ProblemSolver::pathValidationType (const std::string& type, const value_type& tolerance) {