From ee7b60476ddc47c99a4ca8331d9ea87058daeb22 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 4 Sep 2017 18:42:12 +0200
Subject: [PATCH] Add ProblemSolver::createGraspConstraint and
createPregraspConstraint
---
include/hpp/manipulation/problem-solver.hh | 21 ++++++++++++++++
src/problem-solver.cc | 28 ++++++++++++++++++++++
2 files changed, 49 insertions(+)
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 4af6c282..f799860f 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 92d9364e..16339e3f 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)
{
--
GitLab