From f2a3cf9aab29fa78e1f82e0d613442672e86f11d Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 26 Sep 2016 17:37:10 +0200
Subject: [PATCH] helper::Rule have a list of (gripper, handle)
---
include/hpp/manipulation/graph/helper.hh | 11 +---
src/graph/helper.cc | 81 +++++++++++++-----------
2 files changed, 46 insertions(+), 46 deletions(-)
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index bf83c00..4245ce2 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -99,15 +99,10 @@ namespace hpp {
};
struct Rule {
- std::string gripper_;
- std::string handle_;
+ std::vector<std::string> grippers_;
+ std::vector<std::string> handles_;
bool link_;
- Rule() : gripper_(""), handle_(""), link_(false) {}
- Rule(std::string gripper, std::string handle, bool link) {
- gripper_ = gripper;
- handle_ = handle;
- link_ = link;
- }
+ Rule() : grippers_(), handles_(), link_(false) {}
};
typedef std::vector<Rule> Rules_t;
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index b01a00f..3bc380e 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -522,23 +522,27 @@ namespace hpp {
/// - the values correpond to the index of the handle (0..nbHandle-1), or
/// nbHandle to mean no handle.
typedef std::vector <index_t> GraspV_t;
+ struct Result;
struct CompiledRule {
- enum Result {
+ enum Status {
Accept,
Refuse,
NoMatch,
Undefined
};
- boost::regex gripper, handle;
- bool link;
- CompiledRule (const Rule& r) :
- gripper (r.gripper_), handle (r.handle_), link (r.link_) {}
- Result check (const std::string& g, const std::string& h) const
+ std::vector<boost::regex> handles;
+ Status status;
+ CompiledRule (const Result& res, const Rule& r);
+ Status check (const std::vector<std::string>& names, const GraspV_t& g) const
{
- if (boost::regex_match(g, gripper))
- if (boost::regex_match(h, handle))
- return (link ? Accept : Refuse);
- return NoMatch;
+ const std::size_t nG = g.size();
+ assert(nG == handles.size());
+ for (std::size_t i = 0; i < nG; ++i) {
+ if (handles[i].empty()) continue;
+ if (!boost::regex_match(names[g[i]], handles[i]))
+ return NoMatch;
+ }
+ return status;
}
};
typedef std::vector<CompiledRule> CompiledRules_t;
@@ -561,9 +565,9 @@ namespace hpp {
GraspV_t dims;
const Grippers_t& gs;
const Objects_t& ohs;
+ std::vector<std::string> handleNames;
CompiledRules_t rules;
- CompiledRule::Result defaultAcceptationPolicy;
- mutable Eigen::MatrixXi rulesCache;
+ CompiledRule::Status defaultAcceptationPolicy;
Result (const Grippers_t& grippers, const Objects_t& objects, GraphPtr_t g) :
graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects),
@@ -571,50 +575,36 @@ namespace hpp {
{
BOOST_FOREACH (const Object_t& o, objects) {
nOH += o.get<1>().size();
+ BOOST_FOREACH (const HandlePtr_t& h, o.get<1>()) {
+ handleNames.push_back(h->name());
+ }
}
+ handleNames.push_back("");
dims.resize (nG);
dims[0] = nOH + 1;
for (index_t i = 1; i < nG; ++i)
dims[i] = dims[i-1] * (nOH + 1);
graspCs.resize (nG * nOH);
- rulesCache = Eigen::MatrixXi::Constant(nG, nOH + 1, CompiledRule::Undefined);
}
void setRules (const Rules_t& r)
{
for (Rules_t::const_iterator _r = r.begin(); _r != r.end(); ++_r)
- rules.push_back (CompiledRule(*_r));
+ rules.push_back (CompiledRule(*this, *_r));
}
bool graspIsAllowed (const GraspV_t& idxOH) const
{
assert (idxOH.size () == nG);
- for (std::size_t i = 0; i < nG; ++i) {
- const std::string& g = gs[i]->name(),
- h = (idxOH[i] == nOH) ? "" : handle (idxOH[i])->name ();
- if ((CompiledRule::Result)rulesCache(i, idxOH[i]) == CompiledRule::Undefined) {
- CompiledRule::Result status = (idxOH[i] == nOH ? CompiledRule::Accept : defaultAcceptationPolicy);
- for (std::size_t r = 0; r < rules.size(); ++r) {
- switch (rules[r].check(g,h)) {
- case CompiledRule::Accept:
- status = CompiledRule::Accept;
- break; // Exit loop
- case CompiledRule::Refuse:
- status = CompiledRule::Refuse;
- break; // Exit loop
- case CompiledRule::NoMatch:
- continue; // Check next rule
- default:
- throw std::invalid_argument ("Rules are ill-defined.");
- }
- break;
- }
- rulesCache(i, idxOH[i]) = status;
+ for (std::size_t r = 0; r < rules.size(); ++r) {
+ switch (rules[r].check(handleNames,idxOH)) {
+ case CompiledRule::Accept : return true;
+ case CompiledRule::Refuse : return false;
+ case CompiledRule::NoMatch: continue; // Check next rule
+ default: throw std::invalid_argument ("Rules are ill-defined.");
}
- bool keep = ((CompiledRule::Result)rulesCache(i, idxOH[i]) == CompiledRule::Accept);
- if (!keep) return false;
}
- return true;
+ return (defaultAcceptationPolicy == CompiledRule::Accept);
}
inline stateid_type stateid (const GraspV_t& iG)
@@ -769,6 +759,21 @@ namespace hpp {
}
};
+ CompiledRule::CompiledRule (const Result& res, const Rule& r) :
+ handles(res.nG), status (r.link_ ? Accept : Refuse)
+ {
+ assert(r.grippers_.size() == r.handles_.size());
+ for (std::size_t j = 0; j < r.grippers_.size(); ++j) {
+ boost::regex gripper (r.grippers_[j]);
+ for (std::size_t i = 0; i < res.nG; ++i) {
+ if (boost::regex_match(res.gs[i]->name(), gripper)) {
+ assert(handles[i].empty() && "Two gripper regex match the different gripper names.");
+ handles[i] = r.handles_[j];
+ }
+ }
+ }
+ }
+
const StateAndManifold_t& makeState (Result& r, const GraspV_t& g,
const int priority)
{
--
GitLab