diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh index 042da6d441cc26adc2ac987a2e6f113e5b342690..98383ef9cf62788b990d4f7c181e43cbbbd7654a 100644 --- a/include/hpp/manipulation/problem-solver.hh +++ b/include/hpp/manipulation/problem-solver.hh @@ -94,12 +94,13 @@ namespace hpp { GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const; /// Create placement constraint - /// \param name name of the first constraint + /// \param name name of the placement constraint, + /// \param triangleName name of the first list of triangles, + /// \param envContactName name of the second list of triangles. /// void createPlacementConstraint (const std::string& name, - const std::string& jointName, - const std::string& triangleName, - const std::string& envContactName); + const std::string& surface1, + const std::string& surface2); /// Reset constraint set and put back the disable collisions /// between gripper and handle diff --git a/src/problem-solver.cc b/src/problem-solver.cc index b15aedd098de5b0c4ecaf038e66e92e49be76e57..85b1151fbf7010be801f52618813d657cd5469a3 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -22,6 +22,8 @@ #include <hpp/model/gripper.hh> +#include <hpp/constraints/static-stability.hh> + #include <hpp/core/roadmap.hh> #include "hpp/manipulation/device.hh" @@ -82,6 +84,45 @@ namespace hpp { return it->second; } + void ProblemSolver::createPlacementConstraint + (const std::string& name, const std::string& surface1, + const std::string& surface2) + { + if (!robot_) throw std::runtime_error ("No robot loaded"); + using constraints::StaticStabilityGravityPtr_t; + using constraints::StaticStabilityGravityComplement; + using constraints::StaticStabilityGravityComplementPtr_t; + std::string complementName (name + "/complement"); + std::pair < StaticStabilityGravityPtr_t, + StaticStabilityGravityComplementPtr_t > constraints + (StaticStabilityGravityComplement::createPair + (name, complementName, robot_)); + JointAndTriangles_t l = robot_->get <JointAndTriangles_t> (surface1); + if (l.empty ()) throw std::runtime_error + ("First list of triangles not found."); + for (JointAndTriangles_t::const_iterator it = l.begin (); + it != l.end(); ++it) { + constraints.first->addObjectTriangle (it->second, it->first); + } + // Search first robot triangles + l = robot_->get <JointAndTriangles_t> (surface2); + if (l.empty ()) { + // and then environment triangles. + l = get <JointAndTriangles_t> (surface2); + if (l.empty ()) throw std::runtime_error + ("Second list of triangles not found."); + } + for (JointAndTriangles_t::const_iterator it = l.begin (); + it != l.end(); ++it) { + constraints.first->addFloorTriangle (it->second, it->first); + } + + addNumericalConstraint (name, constraints.first); + addNumericalConstraint (complementName, constraints.second); + comparisonType (complementName, core::Equality::create ()); + } + + void ProblemSolver::resetConstraints () { if (robot_)