diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh index d3e77af46d7b215b7d3fffe4a05531a91ef5a7e7..db2e558f77382fc3f5d27bfee3d45881df916283 100644 --- a/include/hpp/manipulation/problem-solver.hh +++ b/include/hpp/manipulation/problem-solver.hh @@ -94,10 +94,12 @@ namespace hpp { /// \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. + /// \param margin see hpp::constraints::QPStaticStability constructor /// void createPlacementConstraint (const std::string& name, const std::string& surface1, - const std::string& surface2); + const std::string& surface2, + const value_type& margin = 1e-4); /// 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 5356b2ce287f226b2ca3891859923b6a23d73dce..359e0a07b9edc28c904901ea71d6f483549b4e25 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -137,7 +137,7 @@ namespace hpp { void ProblemSolver::createPlacementConstraint (const std::string& name, const std::string& surface1, - const std::string& surface2) + const std::string& surface2, const value_type& margin) { if (!robot_) throw std::runtime_error ("No robot loaded"); using constraints::ConvexShape; @@ -169,6 +169,8 @@ namespace hpp { constraints.first->addFloor (ConvexShape (it->second, it->first)); } + constraints.first->setNormalMargin (margin); + addNumericalConstraint (name, NumericalConstraint::create (constraints.first)); addNumericalConstraint (complementName, NumericalConstraint::create