// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#include <hpp/manipulation/graph/helper.hh>
#include <tr1/unordered_map>
#include <tr1/unordered_set>
#include <iterator>
#include <array>
#include <boost/regex.hpp>
#include <pinocchio/multibody/model.hpp>
#include <hpp/util/debug.hh>
#include <hpp/pinocchio/gripper.hh>
#include <pinocchio/multibody/model.hpp>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/manipulation/handle.hh>
#include <hpp/manipulation/graph/state.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/state-selector.hh>
#include <hpp/manipulation/graph/guided-state-selector.hh>
#include <hpp/manipulation/problem-solver.hh>
#define CASE_TO_STRING(var, value) ( (var & value) ? std::string(#value) : std::string() )
namespace hpp {
namespace manipulation {
namespace graph {
namespace helper {
typedef constraints::Implicit Implicit;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
template <bool forPath>
void addToComp
(const NumericalConstraints_t& nc, GraphComponentPtr_t comp)
{
if (nc.empty ()) return;
StatePtr_t n;
if (forPath) {
n = HPP_DYNAMIC_PTR_CAST (State, comp);
if (!n) throw std::logic_error ("Wrong type: expect a State");
}
for (const auto& c : nc)
if (c) {
if (forPath) n->addNumericalConstraintForPath (c);
else comp->addNumericalConstraint (c);
}
}
template <bool param>
void specifyFoliation
(const NumericalConstraints_t& nc, LevelSetEdgePtr_t lse)
{
for (const auto& c : nc)
if (c) {
if (param) lse->insertParamConstraint (c);
else lse->insertConditionConstraint (c);
}
}
void FoliatedManifold::addToState (StatePtr_t comp) const
{
addToComp<false>(nc, comp);
addToComp<false>(nc_path, comp);
}
void FoliatedManifold::addToEdge (EdgePtr_t comp) const
{
addToComp <false> (nc_fol, comp);
}
void FoliatedManifold::specifyFoliation (LevelSetEdgePtr_t lse) const
{
for (const auto& c : nc)
lse->insertConditionConstraint (c);
for (const auto& c : nc_fol)
lse->insertConditionConstraint (c);
}
namespace {
template <int gCase>
struct CaseTraits {
static const bool pregrasp = (gCase & WithPreGrasp);
static const bool preplace = (gCase & WithPrePlace);
static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));
static const bool valid =
( (gCase & WithPreGrasp) || (gCase & GraspOnly) || (gCase & NoGrasp) )
&& ( (gCase & WithPrePlace) || (gCase & PlaceOnly) || (gCase & NoPlace) )
&& !((gCase & NoGrasp) && (gCase & NoPlace));
static const std::size_t nbWaypoints = (pregrasp?1:0) + (intersec?1:0) + (preplace?1:0);
static const std::size_t Nstates = 2 + nbWaypoints;
static const std::size_t Nedges = 1 + nbWaypoints;
// static const std::size_t iNpregrasp = pregrasp?1 + 1:nbWaypoints;
// static const std::size_t iNpreplace = pregrasp?1 + 1:nbWaypoints;
typedef std::array <StatePtr_t, Nstates> StateArray;
typedef std::array <EdgePtr_t, Nedges> EdgeArray;
static inline const StatePtr_t& Npregrasp (const StateArray& n) { assert (pregrasp); return n[1]; }
static inline const StatePtr_t& Nintersec (const StateArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; }
static inline const StatePtr_t& Npreplace (const StateArray& n) { assert (preplace); return n[1 + (pregrasp?1:0) + (intersec?1:0)]; }
static inline std::string caseToString ()
{
return CASE_TO_STRING (gCase, NoGrasp)
+ CASE_TO_STRING (gCase, GraspOnly)
+ CASE_TO_STRING (gCase, WithPreGrasp)
+ " - "
+ CASE_TO_STRING (gCase, NoPlace)
+ CASE_TO_STRING (gCase, PlaceOnly)
+ CASE_TO_STRING (gCase, WithPrePlace);
}
static inline EdgePtr_t makeWE (
const std::string& name,
const StatePtr_t& from, const StatePtr_t& to,
const size_type& w)
{
if (Nedges > 1) {
WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
(from->linkTo (name, to, w, WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
return we;
} else return from->linkTo (name, to, w, Edge::create);
}
static inline StateArray makeWaypoints (
const StatePtr_t& from, const StatePtr_t& to,
const std::string& name)
{
StateSelectorPtr_t ns = from->parentGraph ()->stateSelector ();
StateArray states;
std::size_t r = 0;
states[r] = from; ++r;
if (pregrasp) {
states[r] = ns->createState (name + "_pregrasp", true); ++r;
}
if (intersec) {
states[r] = ns->createState (name + "_intersec", true); ++r;
}
if (preplace) {
states[r] = ns->createState (name + "_preplace", true); ++r;
}
states[r] = to;
return states;
}
static inline EdgePtr_t makeLSEgrasp (const std::string& name,
const StateArray& n, const EdgeArray& e,
const size_type w, LevelSetEdgePtr_t& gls)
{
if (Nedges > 1) {
const std::size_t T = (pregrasp?1:0) + (intersec?1:0);
WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
(n.front()->linkTo (name + "_ls", n.back(), w,
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
gls = linkWaypoint <LevelSetEdge> (n, T-1, T, name, "ls");
for (std::size_t i = 0; i < Nedges; ++i)
we->setWaypoint (i, e[i], n[i+1]);
we->setWaypoint (T-1, gls, n[T]);
gls->state (n.front());
gls->setShort (pregrasp);
return we;
} else {
assert (gCase == (GraspOnly | NoPlace)
&& "Cannot implement a LevelSetEdge for grasping");
gls = static_pointer_cast <LevelSetEdge>
(n.front()->linkTo (name + "_ls", n.back(), w,
LevelSetEdge::create));
return gls;
}
}
static inline EdgePtr_t makeLSEplace (const std::string& name,
const StateArray& n, const EdgeArray& e,
const size_type w, LevelSetEdgePtr_t& pls)
{
if (Nedges > 1) {
const std::size_t T = (pregrasp?1:0) + (intersec?1:0);
WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
(n.back()->linkTo (name + "_ls", n.front(), w,
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
pls = linkWaypoint <LevelSetEdge> (n, T+1, T, name, "ls");
// for (std::size_t i = Nedges - 1; i != 0; --i)
for (std::size_t k = 0; k < Nedges; ++k) {
std::size_t i = Nedges - 1 - k;
we->setWaypoint (Nedges - 1 - i, e[i], n[i]);
}
we->setWaypoint (Nedges - 1 - T, pls, n[T]);
pls->state (n.back ());
pls->setShort (preplace);
return we;
} else {
assert (gCase == (NoGrasp | PlaceOnly)
&& "Cannot implement a LevelSetEdge for placement");
pls = static_pointer_cast <LevelSetEdge>
(n.back()->linkTo (name + "_ls", n.front(), w,
LevelSetEdge::create));
return pls;
}
}
template <typename EdgeType>
static inline shared_ptr<EdgeType> linkWaypoint (
const StateArray& states,
const std::size_t& iF, const std::size_t& iT,
const std::string& prefix,
const std::string& suffix = "")
{
std::stringstream ss;
ss << prefix << "_" << iF << iT;
if (suffix.length () > 0) ss << "_" << suffix;
return static_pointer_cast <EdgeType>
(states[iF]->linkTo (ss.str(), states[iT], -1, EdgeType::create));
}
template <bool forward>
static inline EdgeArray linkWaypoints (
const StateArray& states, const EdgePtr_t& edge,
const std::string& name)
{
EdgeArray e;
WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge);
if (forward)
for (std::size_t i = 0; i < Nedges; ++i) {
e[i] = linkWaypoint <Edge> (states, i, i + 1, name);
we->setWaypoint (i, e[i], states[i+1]);
}
else
// for (std::size_t i = Nedges - 1; i != 0; --i) {
for (std::size_t k = 0; k < Nedges; ++k) {
std::size_t i = Nedges - 1 - k;
e[i] = linkWaypoint <Edge> (states, i + 1, i, name);
we->setWaypoint (Nedges - 1 - i, e[i], states[i]);
}
return e;
}
static inline void setStateConstraints (const StateArray& n,
const FoliatedManifold& g, const FoliatedManifold& pg,
const FoliatedManifold& p, const FoliatedManifold& pp,
const FoliatedManifold& m)
{
// From and to are not populated automatically
// to avoid duplicates.
if (pregrasp) {
p .addToState (Npregrasp (n));
pg.addToState (Npregrasp (n));
m .addToState (Npregrasp (n));
}
if (intersec) {
p .addToState (Nintersec (n));
g .addToState (Nintersec (n));
m .addToState (Nintersec (n));
}
if (preplace) {
pp.addToState (Npreplace (n));
g .addToState (Npreplace (n));
m .addToState (Npreplace (n));
}
}
static inline void setEdgeConstraints (const EdgeArray& e,
const FoliatedManifold& g, const FoliatedManifold& p,
const FoliatedManifold& m)
{
// The border B
const std::size_t B = (pregrasp?1:0) + (intersec?1:0);
for (std::size_t i = 0; i < B ; ++i) p.addToEdge (e[i]);
for (std::size_t i = B; i < Nedges; ++i) g.addToEdge (e[i]);
for (std::size_t i = 0; i < Nedges; ++i) m.addToEdge (e[i]);
}
template <bool forward>
static inline void setEdgeProp
(const EdgeArray& e, const StateArray& n)
{
/// Last is short
const std::size_t K = (forward?1:0);
for (std::size_t i = K; i < Nedges - 1 + K; ++i)
e[i]->setShort (true);
// The border B
std::size_t B;
if ((gCase & NoGrasp)) // There is no grasp
B = 0;
else // There is a grasp
B = 1 + (pregrasp?1:0);
for (std::size_t i = 0; i < B ; ++i) e[i]->state (n[0]);
for (std::size_t i = B; i < Nedges; ++i) e[i]->state (n[Nstates-1]);
}
};
}
template <int gCase> Edges_t createEdges (
const std::string& forwName, const std::string& backName,
const StatePtr_t& from, const StatePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold& preplace,
const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef)
{
typedef CaseTraits<gCase> T;
hppDout (info, "Creating edges " << forwName << " and " << backName
<< "\ncase is " << T::caseToString ());
assert (T::valid && "Not a valid case.");
typedef typename T::StateArray StateArray;
typedef typename T::EdgeArray EdgeArray;
// Create the edges
EdgePtr_t weForw = T::makeWE (forwName, from, to, wForw),
weBack = T::makeWE (backName, to, from, wBack),
weForwLs, weBackLs;
std::string name = forwName;
StateArray n = T::makeWaypoints (from, to, name);
EdgeArray eF = T::template linkWaypoints <true> (n, weForw, name);
// Set the states constraints
// Note that submanifold is not taken into account for states
// because the edges constraints will enforce configuration to stay
// in a leaf, and so in the manifold itself.
T::setStateConstraints (n, grasp, pregrasp, place, preplace,
submanifoldDef);
// Set the edges properties
T::template setEdgeProp <true> (eF, n);
// Set the edges constraints
T::setEdgeConstraints (eF, grasp, place, submanifoldDef);
LevelSetEdgePtr_t gls;
if (levelSetGrasp)
weForwLs = T::makeLSEgrasp (name, n, eF, 10*wForw, gls);
// Populate bacward edge
name = backName;
EdgeArray eB = T::template linkWaypoints <false> (n, weBack, name);
T::template setEdgeProp <false> (eB, n);
T::setEdgeConstraints (eB, grasp, place, submanifoldDef);
LevelSetEdgePtr_t pls;
if (levelSetPlace)
weBackLs = T::makeLSEplace (name, n, eB, 10*wBack, pls);
Edges_t ret { weForw, weBack };
if (levelSetPlace) {
if (!place.foliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for placement, "
"but did not specify the target foliation. "
"It will have no effect");
}
grasp.addToEdge (pls);
place.specifyFoliation (pls);
submanifoldDef.addToEdge (pls);
pls->buildHistogram ();
place.addToEdge (weBackLs);
submanifoldDef.addToEdge (weBackLs);
ret.push_back (weBackLs);
}
if (levelSetGrasp) {
if (!grasp.foliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for grasping, "
"but did not specify the target foliation. "
"It will have no effect");
}
place.addToEdge (gls);
grasp.specifyFoliation (gls);
submanifoldDef.addToEdge (gls);
gls->buildHistogram ();
grasp.addToEdge (weForwLs);
submanifoldDef.addToEdge (weForwLs);
ret.push_back (weForwLs);
}
return ret;
}
EdgePtr_t createLoopEdge (
const std::string& loopName,
const StatePtr_t& state,
const size_type& w,
const bool levelSet,
const FoliatedManifold& submanifoldDef)
{
// Create the edges
EdgePtr_t loop;
if (levelSet)
loop = state->linkTo (loopName, state, w, LevelSetEdge::create);
else loop = state->linkTo (loopName, state, w, Edge::create);
loop->state (state);
submanifoldDef.addToEdge (loop);
if (levelSet) {
if (!submanifoldDef.foliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for looping, "
"but did not specify the target foliation. "
"It will have no effect");
}
LevelSetEdgePtr_t ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge, loop);
submanifoldDef.specifyFoliation (ls);
ls->buildHistogram ();
}
return loop;
}
void graspManifold (
const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
ImplicitPtr_t gc = handle->createGrasp (gripper, "");
grasp.nc.push_back (gc);
grasp.nc_path.push_back (gc);
ImplicitPtr_t gcc = handle->createGraspComplement
(gripper, "");
if (gcc->function ().outputSize () > 0)
grasp.nc_fol.push_back (gcc);
const value_type c = handle->clearance () + gripper->clearance ();
ImplicitPtr_t pgc = handle->createPreGrasp (gripper, c, "");
pregrasp.nc.push_back (pgc);
pregrasp.nc_path.push_back (pgc);
}
void strictPlacementManifold (
const ImplicitPtr_t placement,
const ImplicitPtr_t preplacement,
const ImplicitPtr_t placementComplement,
FoliatedManifold& place, FoliatedManifold& preplace)
{
place.nc.push_back (placement);
place.nc_path.push_back (placement);
if (placementComplement && placementComplement->function().outputSize () > 0)
place.nc_fol.push_back (placementComplement);
preplace.nc.push_back (preplacement);
preplace.nc_path.push_back (preplacement);
}
void relaxedPlacementManifold (
const ImplicitPtr_t placement,
const ImplicitPtr_t preplacement,
const LockedJoints_t objectLocks,
FoliatedManifold& place, FoliatedManifold& preplace)
{
if (placement) {
place.nc.push_back (placement);
// The placement constraints are not required in the path, as long as
// they are satisfied at both ends and the object does not move. The
// former condition is ensured by the placement constraints on both
// ends and the latter is ensure by the LockedJoint constraints.
place.nc_path.push_back (placement);
}
std::copy (objectLocks.begin(), objectLocks.end(), std::back_inserter(place.lj_fol));
if (placement && preplacement) {
preplace.nc.push_back (preplacement);
// preplace.nc_path.push_back (preplacement);
}
}
namespace {
typedef std::size_t index_t;
typedef std::vector <index_t> IndexV_t;
typedef std::list <index_t> IndexL_t;
typedef std::pair <index_t, index_t> Grasp_t;
typedef std::tuple <StatePtr_t, FoliatedManifold> StateAndManifold_t;
//typedef std::vector <index_t, index_t> GraspV_t;
/// GraspV_t corresponds to a unique ID of a permutation.
/// - its size is the number of grippers,
/// - 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 Status {
Accept,
Refuse,
NoMatch,
Undefined
};
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
{
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;
struct Result {
ProblemSolverPtr_t ps;
GraphPtr_t graph;
typedef unsigned long stateid_type;
std::tr1::unordered_map<stateid_type, StateAndManifold_t> states;
typedef std::pair<stateid_type, stateid_type> edgeid_type;
struct edgeid_hash {
std::tr1::hash<edgeid_type::first_type> first;
std::tr1::hash<edgeid_type::second_type> second;
std::size_t operator() (const edgeid_type& eid) const {
return first(eid.first) + second(eid.second);
}
};
std::tr1::unordered_set<edgeid_type, edgeid_hash> edges;
std::vector< std::array<ImplicitPtr_t,3> > graspCs;
index_t nG, nOH;
GraspV_t dims;
const Grippers_t& gs;
const Objects_t& ohs;
std::vector<std::string> handleNames;
CompiledRules_t rules;
CompiledRule::Status defaultAcceptationPolicy;
Result (const ProblemSolverPtr_t problem, const Grippers_t& grippers, const Objects_t& objects, GraphPtr_t g) :
ps (problem), graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects),
defaultAcceptationPolicy (CompiledRule::Refuse)
{
for (const Object_t& o : objects) {
nOH += std::get<1>(o).size();
for (const HandlePtr_t& h : std::get<1>(o))
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);
}
void setRules (const Rules_t& r)
{
for (Rules_t::const_iterator _r = r.begin(); _r != r.end(); ++_r)
rules.push_back (CompiledRule(*this, *_r));
}
bool graspIsAllowed (const GraspV_t& idxOH) const
{
assert (idxOH.size () == nG);
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.");
}
}
return (defaultAcceptationPolicy == CompiledRule::Accept);
}
inline stateid_type stateid (const GraspV_t& iG)
{
stateid_type iGOH = iG[0];
stateid_type res;
for (index_t i = 1; i < nG; ++i) {
res = iGOH + dims[i] * (iG[i]);
if (res < iGOH) {
hppDout (info, "State ID overflowed. There are too many states...");
}
iGOH = res;
// iGOH += dims[i] * (iG[i]);
}
return iGOH;
}
bool hasState (const GraspV_t& iG)
{
return states.count(stateid(iG)) > 0;
}
StateAndManifold_t& operator() (const GraspV_t& iG)
{
return states [stateid(iG)];
}
bool hasEdge (const GraspV_t& g1, const GraspV_t& g2)
{
return edges.count(edgeid_type(stateid(g1), stateid(g2))) > 0;
}
void addEdge (const GraspV_t& g1, const GraspV_t& g2)
{
edges.insert(edgeid_type(stateid(g1), stateid(g2)));
}
inline std::array<ImplicitPtr_t,3>& graspConstraint (
const index_t& iG, const index_t& iOH)
{
std::array<ImplicitPtr_t,3>& gcs =
graspCs [iG * nOH + iOH];
if (!gcs[0]) {
hppDout (info, "Create grasps constraints for ("
<< iG << ", " << iOH << ")");
const GripperPtr_t& g (gs[iG]);
const HandlePtr_t& h (handle (iOH));
const std::string& grasp = g->name() + " grasps " + h->name();
if (!ps->numericalConstraints.has(grasp)) {
ps->createGraspConstraint (grasp, g->name(), h->name());
}
gcs[0] = ps->numericalConstraints.get(grasp);
gcs[1] = ps->numericalConstraints.get(grasp + "/complement");
const std::string& pregrasp = g->name() + " pregrasps " + h->name();
if (!ps->numericalConstraints.has(pregrasp)) {
ps->createPreGraspConstraint (pregrasp, g->name(), h->name());
}
gcs[2] = ps->numericalConstraints.get(pregrasp);
}
return gcs;
}
const Object_t& object (const index_t& iOH) const {
index_t iH = iOH;
for (const Object_t& o : ohs) {
if (iH < std::get<1>(o).size()) return o;
iH -= std::get<1>(o).size();
}
throw std::out_of_range ("Handle index");
}
const HandlePtr_t& handle (const index_t& iOH) const {
index_t iH = iOH;
for (const Object_t& o : ohs) {
if (iH < std::get<1>(o).size()) return std::get<1>(o)[iH];
iH -= std::get<1>(o).size();
}
throw std::out_of_range ("Handle index");
}
/// Check if an object can be placed
bool objectCanBePlaced (const Object_t& o) const
{
// If the object has no joint, then it cannot be placed.
return (std::get<2>(std::get<0>(o)).size() > 0);
}
/// Check is an object is grasped by the GraspV_t
bool isObjectGrasped (const GraspV_t& idxOH,
const Object_t& o) const
{
assert (idxOH.size () == nG);
for (std::size_t i = 0; i < idxOH.size (); ++i)
if (idxOH[i] < nOH) // This grippers grasps an object
if (std::get<2>(o) == std::get<2>(object(idxOH[i])))
return true;
return false;
}
/// Get a state name from a set of grasps
std::string name (const GraspV_t& idxOH, bool abbrev = false) const {
assert (idxOH.size () == nG);
std::stringstream ss;
bool first = true;
std::string sepGOH = (abbrev?"-":" grasps "),
sep = (abbrev?":":" : ");
for (std::size_t i = 0; i < idxOH.size (); ++i) {
if (idxOH[i] < nOH) { // This grippers grasps an object
if (first) first = false; else ss << sep;
if (abbrev) ss << i << sepGOH << idxOH[i];
else
ss << gs[i]->name() << sepGOH << handle (idxOH[i])->name ();
}
}
if (first) return (abbrev?"f":"free");
return ss.str();
}
/// Get an edge name from a set of grasps
std::pair<std::string, std::string> name
(const GraspV_t& gFrom, const GraspV_t& gTo, const index_t iG)
{
const std::string nf (name (gFrom, true)),
nt (name (gTo, true));
std::stringstream ssForw, ssBack;
const char sep[] = " | ";
ssForw << gs[iG]->name() << " > " << handle (gTo[iG])->name () << sep << nf;
ssBack << gs[iG]->name() << " < " << handle (gTo[iG])->name () << sep << nt;
return std::make_pair (ssForw.str(), ssBack.str ());
}
std::string nameLoopEdge (const GraspV_t& gFrom)
{
const std::string nf (name (gFrom, true));
std::stringstream ss;
const char sep[] = " | ";
ss << "Loop" << sep << nf;
return ss.str();
}
void graspManifold (const index_t& iG, const index_t& iOH,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
std::array<ImplicitPtr_t,3>& gcs
= graspConstraint (iG, iOH);
grasp.nc.push_back (gcs[0]);
grasp.nc_path.push_back (gcs[0]);
if (gcs[1]->function ().outputSize () > 0)
grasp.nc_fol.push_back (gcs[1]);
pregrasp.nc.push_back (gcs[2]);
pregrasp.nc_path.push_back (gcs[2]);
}
};
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)
{
StateAndManifold_t& nam = r (g);
if (!std::get<0>(nam)) {
hppDout (info, "Creating state " << r.name (g));
std::get<0>(nam) = r.graph->stateSelector ()->createState
(r.name (g), false, priority);
// Loop over the grippers and create grasping constraints if required
FoliatedManifold unused;
std::set <index_t> idxsOH;
for (index_t i = 0; i < r.nG; ++i) {
if (g[i] < r.nOH) {
idxsOH.insert (g[i]);
r.graspManifold (i, g[i], std::get<1>(nam), unused);
}
}
index_t iOH = 0;
for (const Object_t& o : r.ohs) {
if (!r.objectCanBePlaced(o)) continue;
bool oIsGrasped = false;
// TODO: use the fact that the set is sorted.
// for (const HandlePtr_t& h : std::get<0>(o))
for (index_t i = 0; i < std::get<1>(o).size(); ++i) {
if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
++iOH;
}
if (!oIsGrasped) {
const auto& pc (std::get<0>(o));
relaxedPlacementManifold (std::get<0>(pc),
std::get<1>(pc),
std::get<2>(pc),
std::get<1>(nam), unused);
}
}
std::get<1>(nam).addToState (std::get<0>(nam));
createLoopEdge (r.nameLoopEdge (g),
std::get<0>(nam), 0,
false,
// TODO std::get<1>(nam).foliated(),
std::get<1>(nam));
}
return nam;
}
/// Arguments are such that
/// \li gTo[iG] != gFrom[iG]
/// \li for all i != iG, gTo[iG] == gFrom[iG]
void makeEdge (Result& r,
const GraspV_t& gFrom, const GraspV_t& gTo,
const index_t iG, const int priority)
{
if (r.hasEdge(gFrom, gTo)) {
hppDout (warning, "Prevented creation of duplicated edge\nfrom "
<< r.name (gFrom) << "\nto " << r.name (gTo));
return;
}
const StateAndManifold_t& from = makeState (r, gFrom, priority),
to = makeState (r, gTo, priority+1);
const Object_t& o = r.object (gTo[iG]);
// Detect when grasping an object already grasped.
// or when there is no placement information for it.
bool noPlace = !r.objectCanBePlaced(o)
|| r.isObjectGrasped (gFrom, o);
FoliatedManifold grasp, pregrasp, place, preplace,
submanifold;
r.graspManifold (iG, gTo[iG], grasp, pregrasp);
if (!noPlace) {
const auto& pc (std::get<0>(o));
relaxedPlacementManifold (std::get<0>(pc),
std::get<1>(pc),
std::get<2>(pc),
place, preplace);
}
std::pair<std::string, std::string> names =
r.name (gFrom, gTo, iG);
{
FoliatedManifold unused;
std::set <index_t> idxsOH;
for (index_t i = 0; i < r.nG; ++i) {
if (gFrom[i] < r.nOH) {
idxsOH.insert (gFrom[i]);
r.graspManifold (i, gFrom[i], submanifold, unused);
}
}
index_t iOH = 0;
for (const Object_t& o : r.ohs) {
if (!r.objectCanBePlaced(o)) continue;
bool oIsGrasped = false;
const index_t iOHstart = iOH;
for (; iOH < iOHstart + std::get<1>(o).size(); ++iOH) {
if (iOH == gTo [iG]) {
oIsGrasped = true;
iOH = iOHstart + std::get<1>(o).size();
break;
}
if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
}
if (!oIsGrasped) {
const auto& pc (std::get<0>(o));
relaxedPlacementManifold (std::get<0>(pc),
std::get<1>(pc),
std::get<2>(pc),
submanifold, unused);
}
}
}
if (pregrasp.empty ()) {
if (noPlace)
createEdges <GraspOnly | NoPlace> (
names.first , names.second,
std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
submanifold);
else if (preplace.empty ())
createEdges <GraspOnly | PlaceOnly> (
names.first , names.second,
std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
submanifold);
else {
hppDout (error, "GraspOnly | WithPrePlace not implemeted yet");
/*
createEdges <GraspOnly | WithPrePlace> (
names.first , names.second,
std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
submanifold); // */
}
} else {
if (noPlace)
createEdges <WithPreGrasp | NoPlace> (
names.first , names.second,
std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
submanifold);
else if (preplace.empty ())
createEdges <WithPreGrasp | PlaceOnly> (
names.first , names.second,
std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
submanifold);
else
createEdges <WithPreGrasp | WithPrePlace> (
names.first , names.second,
std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
submanifold);
}
r.addEdge(gFrom, gTo);
}
/// idx are the available grippers
void recurseGrippers (Result& r,
const IndexV_t& idx_g, const IndexV_t& idx_oh,
const GraspV_t& grasps, const int depth)
{
bool curGraspIsAllowed = r.graspIsAllowed(grasps);
if (curGraspIsAllowed) makeState (r, grasps, depth);
if (idx_g.empty () || idx_oh.empty ()) return;
IndexV_t nIdx_g (idx_g.size() - 1);
IndexV_t nIdx_oh (idx_oh.size() - 1);
for (IndexV_t::const_iterator itx_g = idx_g.begin ();
itx_g != idx_g.end (); ++itx_g) {
// Copy all element except itx_g
std::copy (std::next (itx_g), idx_g.end (),
std::copy (idx_g.begin (), itx_g, nIdx_g.begin ())
);
for (IndexV_t::const_iterator itx_oh = idx_oh.begin ();
itx_oh != idx_oh.end (); ++itx_oh) {
// Create the edge for the selected grasp
GraspV_t nGrasps = grasps;
nGrasps [*itx_g] = *itx_oh;
bool nextGraspIsAllowed = r.graspIsAllowed(nGrasps);
if (nextGraspIsAllowed) makeState (r, nGrasps, depth + 1);
if (curGraspIsAllowed && nextGraspIsAllowed)
makeEdge (r, grasps, nGrasps, *itx_g, depth);
// Copy all element except itx_oh
std::copy (std::next (itx_oh), idx_oh.end (),
std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
);
// Do all the possible combination below this new grasp
recurseGrippers (r, nIdx_g, nIdx_oh, nGrasps, depth + 2);
}
}
}
}
void graphBuilder (
const ProblemSolverPtr_t& ps,
const Objects_t& objects,
const Grippers_t& grippers,
GraphPtr_t graph,
const Rules_t& rules)
{
if (!graph) throw std::logic_error ("The graph must be initialized");
StateSelectorPtr_t ns = graph->stateSelector ();
if (!ns) throw std::logic_error ("The graph does not have a StateSelector");
Result r (ps, grippers, objects, graph);
r.setRules (rules);
IndexV_t availG (r.nG), availOH (r.nOH);
for (index_t i = 0; i < r.nG; ++i) availG[i] = i;
for (index_t i = 0; i < r.nOH; ++i) availOH[i] = i;
GraspV_t iG (r.nG, r.nOH);
recurseGrippers (r, availG, availOH, iG, 0);
hppDout (info, "Created a graph with " << r.states.size() << " states "
"and " << r.edges.size() << " edges.");
}
GraphPtr_t graphBuilder (
const ProblemSolverPtr_t& ps,
const std::string& graphName,
const Strings_t& griNames,
const std::vector <ObjectDef_t>& objs,
const Strings_t& envNames,
const std::vector <Rule>& rules,
const value_type& prePlaceWidth)
{
if (ps->graphs.has (graphName))
throw std::invalid_argument ("A graph named " + graphName + " already exists.");
const Device& robot = *(ps->robot ());
const pinocchio::Model& model = robot.model();
Grippers_t grippers (griNames.size());
index_t i = 0;
for (const std::string& gn : griNames) {
grippers[i] = robot.grippers.get (gn);
++i;
}
Objects_t objects (objs.size());
i = 0;
const value_type margin = 1e-3;
bool prePlace = (prePlaceWidth > 0);
for (const ObjectDef_t& od : objs) {
// Create handles
std::get<2>(objects[i]) = i;
std::get<1>(objects[i]).resize (od.handles.size());
Handles_t::iterator it = std::get<1>(objects[i]).begin();
for (const std::string hn : od.handles) {
*it = robot.handles.get (hn);
++it;
}
// Create placement
const std::string placeN = "place_" + od.name;
const std::string preplaceN = "pre" + placeN;
// If user provides constraint "place_objectName",
// then
// use this as placement and use "preplace_objectName" for
// pre-placement if defined.
// else if contact surfaces are defined and selected
// create default placement constraint using the ProblemSolver
// methods createPlacementConstraint and createPrePlacementConstraint
auto& pc (std::get<0>(objects[i]));
if (ps->numericalConstraints.has(placeN)) {
std::get<0>(pc) = ps->numericalConstraints.get (placeN);
if (ps->numericalConstraints.has(preplaceN)) {
std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
}
} else if (!envNames.empty() && !od.shapes.empty ()) {
ps->createPlacementConstraint (placeN,
od.shapes, envNames, margin);
std::get<0>(pc) = ps->numericalConstraints.get (placeN);
if (prePlace) {
ps->createPrePlacementConstraint (preplaceN,
od.shapes, envNames, margin, prePlaceWidth);
std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
}
}
// Create object lock
// Loop over all frames of object, detect joint and create locked
// joint.
assert (robot.robotFrames (od.name).size () != 0);
for (const FrameIndex& f : robot.robotFrames (od.name)) {
if (model.frames[f].type != ::pinocchio::JOINT) continue;
const JointIndex j = model.frames[f].parent;
JointPtr_t oj (Joint::create (ps->robot(), j));
LiegroupSpacePtr_t space (oj->configurationSpace ());
LiegroupElement lge (robot.currentConfiguration()
.segment (oj->rankInConfiguration (),
oj->configSize ()), space);
LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
ps->numericalConstraints.add ("lock_" + oj->name (), lj);
std::get<2>(pc).push_back (lj);
}
++i;
}
GraphPtr_t graph = Graph::create (graphName,
ps->robot(), ps->problem());
ps->graphs.add (graphName, graph);
ps->constraintGraph (graphName);
graph->stateSelector (
GuidedStateSelector::create ("stateSelector",
ps->roadmap ()));
graph->maxIterations (ps->maxIterProjection ());
graph->errorThreshold (ps->errorThreshold ());
graphBuilder (ps, objects, grippers, graph, rules);
return graph;
}
} // namespace helper
} // namespace graph
} // namespace manipulation
} // namespace hpp