Skip to content
Snippets Groups Projects
Commit 73994c6e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Enhance helpers

parent fc063274
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/hpp/manipulation/graph/helper.hh
+ 76
− 59
View file @ 73994c6e
...@@ -27,75 +27,92 @@ ...@@ -27,75 +27,92 @@
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { namespace graph {
/// \addtogroup constraint_graph namespace helper
/// \{ {
/// \defgroup helper Helpers to build the graph of constraints
struct NumericalConstraintsAndPassiveDofs { /// \addtogroup helper
NumericalConstraints_t nc; /// \{
IntervalsContainer_t pdof;
NumericalConstraintsAndPassiveDofs merge struct NumericalConstraintsAndPassiveDofs {
(const NumericalConstraintsAndPassiveDofs& other) { NumericalConstraints_t nc;
NumericalConstraintsAndPassiveDofs ret; IntervalsContainer_t pdof;
// ret.nc.reserve (nc.size() + other.nc.size()); NumericalConstraintsAndPassiveDofs merge
ret.pdof.reserve (pdof.size() + other.pdof.size()); (const NumericalConstraintsAndPassiveDofs& other) {
NumericalConstraintsAndPassiveDofs ret;
std::copy (nc.begin(), nc.end(), ret.nc.begin()); // ret.nc.reserve (nc.size() + other.nc.size());
std::copy (other.nc.begin(), other.nc.end(), ret.nc.begin()); ret.pdof.reserve (pdof.size() + other.pdof.size());
std::copy (pdof.begin(), pdof.end(), ret.pdof.begin()); std::copy (nc.begin(), nc.end(), ret.nc.begin());
std::copy (other.pdof.begin(), other.pdof.end(), ret.pdof.begin()); std::copy (other.nc.begin(), other.nc.end(), ret.nc.begin());
return ret;
std::copy (pdof.begin(), pdof.end(), ret.pdof.begin());
std::copy (other.pdof.begin(), other.pdof.end(), ret.pdof.begin());
return ret;
}
template <bool forPath> void addToComp (GraphComponentPtr_t comp) const;
template <bool param> void specifyFoliation (LevelSetEdgePtr_t lse) const;
};
struct FoliatedManifold {
// Manifold definition
NumericalConstraintsAndPassiveDofs nc;
LockedJoints_t lj;
NumericalConstraintsAndPassiveDofs nc_path;
// Foliation definition
NumericalConstraintsAndPassiveDofs nc_fol;
LockedJoints_t lj_fol;
FoliatedManifold merge (const FoliatedManifold& other) {
FoliatedManifold both;
both.nc = nc.merge (other.nc);
both.nc_path = nc_path.merge (other.nc_path);
std::copy (lj.begin (), lj.end (), both.lj.end ());
std::copy (other.lj.begin (), other.lj.end (), both.lj.end ());
return both;
} }
template <bool forPath> void addToComp (GraphComponentPtr_t comp) const; void addToNode (NodePtr_t comp) const;
void addToEdge (EdgePtr_t comp) const;
template <bool param> void specifyFoliation (LevelSetEdgePtr_t lse) const; void specifyFoliation (LevelSetEdgePtr_t lse) const;
};
struct FoliatedManifold {
// Manifold definition
NumericalConstraintsAndPassiveDofs nc;
LockedJoints_t lj;
NumericalConstraintsAndPassiveDofs nc_path;
// Foliation definition
NumericalConstraintsAndPassiveDofs nc_fol;
LockedJoints_t lj_fol;
FoliatedManifold merge (const FoliatedManifold& other) {
FoliatedManifold both;
both.nc = nc.merge (other.nc);
both.nc_path = nc_path.merge (other.nc_path);
std::copy (lj.begin (), lj.end (), both.lj.end ());
std::copy (other.lj.begin (), other.lj.end (), both.lj.end ());
return both;
}
void addToNode (NodePtr_t comp) const; bool isFoliated () const {
void addToEdge (EdgePtr_t comp) const; return lj_fol.empty () && nc_fol.nc.empty ();
void specifyFoliation (LevelSetEdgePtr_t lse) const; }
};
bool isFoliated () const {
return lj_fol.empty () && nc_fol.nc.empty (); typedef std::pair <EdgePtr_t, EdgePtr_t> EdgePair_t;
}
}; enum GraspingCase {
NoGrasp = 0,
class HPP_MANIPULATION_DLLAPI Helper GraspOnly = 1 << 0,
{ WithPreGrasp = 1 << 1
public: };
typedef std::pair <WaypointEdgePtr_t, WaypointEdgePtr_t> WaypointEdgePair_t; enum PlacementCase {
NoPlace = 1 << 2,
WaypointEdgePair_t createWaypoints ( PlaceOnly = 1 << 3,
WithPrePlace = 1 << 4
};
/// Create edges according to the case.
/// gCase is a logical OR combination of GraspingCase and PlacementCase
///
/// When an argument is not relevant, use the default constructor
/// of FoliatedManifold
template < int gCase >
EdgePair_t createEdges (
const std::string& forwName, const std::string& backName, const std::string& forwName, const std::string& backName,
const NodePtr_t& from, const NodePtr_t& to, const NodePtr_t& from, const NodePtr_t& to,
const size_type& wForw, const size_type& wBack, const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp, const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold& preplace, const FoliatedManifold& place, const FoliatedManifold& preplace,
const bool levelSetPlace, const bool levelSetGrasp, const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef = FoliatedManifold () const FoliatedManifold& submanifoldDef = FoliatedManifold ()
); );
}; /// \}
/// \} } // namespace helper
} // namespace graph } // namespace graph
} // namespace manipulation } // namespace manipulation
} // namespace hpp } // namespace hpp
......
src/graph/helper.cc
+ 401
− 168
View file @ 73994c6e
...@@ -18,184 +18,417 @@ ...@@ -18,184 +18,417 @@
#include <hpp/util/debug.hh> #include <hpp/util/debug.hh>
#include "hpp/manipulation/graph/node.hh" #include <hpp/manipulation/graph/node.hh>
#include "hpp/manipulation/graph/edge.hh" #include <hpp/manipulation/graph/edge.hh>
#include "hpp/manipulation/graph/node-selector.hh" #include <hpp/manipulation/graph/node-selector.hh>
namespace hpp { namespace hpp {
namespace manipulation { namespace manipulation {
namespace graph { namespace graph {
template <bool forPath> namespace helper {
void NumericalConstraintsAndPassiveDofs::addToComp template <bool forPath>
(GraphComponentPtr_t comp) const void NumericalConstraintsAndPassiveDofs::addToComp
{ (GraphComponentPtr_t comp) const
if (nc.empty ()) return; {
NodePtr_t n; if (nc.empty ()) return;
if (forPath) { NodePtr_t n;
n = HPP_DYNAMIC_PTR_CAST (Node, comp); if (forPath) {
if (!n) throw std::logic_error ("Wrong type: expect a Node"); n = HPP_DYNAMIC_PTR_CAST (Node, comp);
if (!n) throw std::logic_error ("Wrong type: expect a Node");
}
NumericalConstraints_t::const_iterator it;
IntervalsContainer_t::const_iterator itpdof = pdof.begin ();
for (it = nc.begin (); it != nc.end (); ++it) {
if (forPath) n->addNumericalConstraintForPath (*it, *itpdof);
else comp->addNumericalConstraint (*it, *itpdof);
++itpdof;
}
assert (itpdof == pdof.end ());
} }
NumericalConstraints_t::const_iterator it;
IntervalsContainer_t::const_iterator itpdof = pdof.begin (); template <bool param>
for (it = nc.begin (); it != nc.end (); ++it) { void NumericalConstraintsAndPassiveDofs::specifyFoliation
if (forPath) n->addNumericalConstraintForPath (*it, *itpdof); (LevelSetEdgePtr_t lse) const
else comp->addNumericalConstraint (*it, *itpdof); {
++itpdof; NumericalConstraints_t::const_iterator it;
IntervalsContainer_t::const_iterator itpdof = pdof.begin ();
for (it = nc.begin (); it != nc.end (); ++it) {
if (param) lse->insertParamConstraint (*it, *itpdof);
else lse->insertConditionConstraint (*it, *itpdof);
++itpdof;
}
assert (itpdof == pdof.end ());
}
void FoliatedManifold::addToNode (NodePtr_t comp) const
{
nc.addToComp <false> (comp);
for (LockedJoints_t::const_iterator it = lj.begin ();
it != lj.end (); ++it)
comp->addLockedJointConstraint (*it);
nc_path.addToComp <true> (comp);
}
void FoliatedManifold::addToEdge (EdgePtr_t comp) const
{
nc_fol.addToComp <false> (comp);
for (LockedJoints_t::const_iterator it = lj_fol.begin ();
it != lj_fol.end (); ++it)
comp->addLockedJointConstraint (*it);
} }
assert (itpdof == pdof.end ());
} void FoliatedManifold::specifyFoliation (LevelSetEdgePtr_t lse) const
{
template <bool param> nc.specifyFoliation <false> (lse);
void NumericalConstraintsAndPassiveDofs::specifyFoliation for (LockedJoints_t::const_iterator it = lj.begin ();
(LevelSetEdgePtr_t lse) const it != lj.end (); ++it)
{ lse->insertConditionConstraint (*it);
NumericalConstraints_t::const_iterator it;
IntervalsContainer_t::const_iterator itpdof = pdof.begin (); nc_fol.specifyFoliation <true> (lse);
for (it = nc.begin (); it != nc.end (); ++it) { for (LockedJoints_t::const_iterator it = lj_fol.begin ();
if (param) lse->insertParamConstraint (*it, *itpdof); it != lj_fol.end (); ++it)
else lse->insertConditionConstraint (*it, *itpdof); lse->insertParamConstraint (*it);
++itpdof;
} }
assert (itpdof == pdof.end ());
} template <> EdgePair_t
createEdges <WithPreGrasp | WithPrePlace> (
void FoliatedManifold::addToNode (NodePtr_t comp) const const std::string& forwName, const std::string& backName,
{ const NodePtr_t& from, const NodePtr_t& to,
nc.addToComp <false> (comp); const size_type& wForw, const size_type& wBack,
for (LockedJoints_t::const_iterator it = lj.begin (); const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
it != lj.end (); ++it) const FoliatedManifold& place, const FoliatedManifold& preplace,
comp->addLockedJointConstraint (*it); const bool levelSetGrasp, const bool levelSetPlace,
nc_path.addToComp <true> (comp); const FoliatedManifold& submanifoldDef)
} {
// Create the edges
void FoliatedManifold::addToEdge (EdgePtr_t comp) const WaypointEdgePtr_t weForw = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
{ from->linkTo (forwName, to, wForw, WaypointEdge::create)),
nc_fol.addToComp <false> (comp);
for (LockedJoints_t::const_iterator it = lj_fol.begin (); weBack = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
it != lj_fol.end (); ++it) to-> linkTo (backName, from, wBack, WaypointEdge::create));
comp->addLockedJointConstraint (*it);
} weForw->nbWaypoints (3);
weBack->nbWaypoints (3);
void FoliatedManifold::specifyFoliation (LevelSetEdgePtr_t lse) const
{ std::string name = forwName;
nc.specifyFoliation <false> (lse); NodeSelectorPtr_t ns = weForw->parentGraph ()->nodeSelector ();
for (LockedJoints_t::const_iterator it = lj.begin (); NodePtr_t n0 = from,
it != lj.end (); ++it) n1 = ns->createNode (name + "_pregrasp", true),
lse->insertConditionConstraint (*it); n2 = ns->createNode (name + "_intersec", true),
n3 = ns->createNode (name + "_preplace", true),
nc_fol.specifyFoliation <true> (lse); n4 = to;
for (LockedJoints_t::const_iterator it = lj_fol.begin ();
it != lj_fol.end (); ++it) EdgePtr_t e01 = n0->linkTo (name + "_e01", n1, -1, Edge::create),
lse->insertParamConstraint (*it); e12 = n1->linkTo (name + "_e12", n2, -1, Edge::create),
} e23 = n2->linkTo (name + "_e23", n3, -1, Edge::create),
e34 = weForw;
Helper::WaypointEdgePair_t Helper::createWaypoints ( LevelSetEdgePtr_t e12_ls;
const std::string& forwName, const std::string& backName, if (levelSetGrasp)
const NodePtr_t& from, const NodePtr_t& to, e12_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
const size_type& wForw, const size_type& wBack, n1->linkTo (name + "_e12_ls", n2, -1, LevelSetEdge::create));
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold& preplace, // Set the edges properties
const bool levelSetPlace, const bool levelSetGrasp, e01->node (n0);
const FoliatedManifold& submanifoldDef) e12->node (n0); e12->setShort (true);
{ e23->node (n4); e23->setShort (true);
// Create the edges e34->node (n4);
WaypointEdgePtr_t weForw = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
from->linkTo (forwName, to, wForw, WaypointEdge::create)), // set the nodes constraints
// From and to are not populated automatically to avoid duplicates.
weBack = HPP_DYNAMIC_PTR_CAST (WaypointEdge, place.addToNode (n1);
to-> linkTo (backName, from, wBack, WaypointEdge::create)); pregrasp.addToNode (n1);
submanifoldDef.addToNode (n1);
weForw->nbWaypoints (3); place.addToNode (n2);
weBack->nbWaypoints (3); grasp.addToNode (n2);
submanifoldDef.addToNode (n2);
std::string name = forwName; preplace.addToNode (n3);
NodeSelectorPtr_t ns = weForw->parentGraph ()->nodeSelector (); grasp.addToNode (n3);
NodePtr_t n0 = from, submanifoldDef.addToNode (n3);
n1 = ns->createNode (name + "_pregrasp", true),
n2 = ns->createNode (name + "_intersec", true), // Set the edges constraints
n3 = ns->createNode (name + "_preplace", true), place.addToEdge (e01);
n4 = to; submanifoldDef.addToEdge (e01);
place.addToEdge (e12);
EdgePtr_t e01 = n0->linkTo (name + "_e01", n1, -1, Edge::create), submanifoldDef.addToEdge (e12);
e12 = n1->linkTo (name + "_e12", n2, -1, grasp.addToEdge (e23);
(levelSetGrasp?(Node::EdgeFactory)LevelSetEdge::create:Edge::create)), submanifoldDef.addToEdge (e23);
e23 = n2->linkTo (name + "_e23", n3, -1, Edge::create), grasp.addToEdge (e34);
e34 = weForw; submanifoldDef.addToEdge (e34);
// Set the edges properties // Set the waypoints
e01->node (n0); weForw->setWaypoint (0, e01, n1);
e12->node (n0); e12->setShort (true); weForw->setWaypoint (1, (levelSetGrasp)?e12_ls:e12, n2);
e23->node (n4); e23->setShort (true); weForw->setWaypoint (2, e23, n3);
e34->node (n4);
// Populate bacward edge
// set the nodes constraints EdgePtr_t e43 = n4->linkTo (name + "_e43", n3, -1, Edge::create),
// From and to are not populated automatically to avoid duplicates. e32 = n3->linkTo (name + "_e32", n2, -1, Edge::create),
place.addToNode (n1); e21 = n2->linkTo (name + "_e21", n1, -1, Edge::create),
pregrasp.addToNode (n1); e10 = weBack;
submanifoldDef.addToNode (n1); LevelSetEdgePtr_t e32_ls;
place.addToNode (n2); if (levelSetPlace)
grasp.addToNode (n2); e32_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
submanifoldDef.addToNode (n2); n3->linkTo (name + "_e32_ls", n2, -1, LevelSetEdge::create));
preplace.addToNode (n3);
grasp.addToNode (n3); e43->node (n4);
submanifoldDef.addToNode (n3); e32->node (n4); e32->setShort (true);
e21->node (n0); e21->setShort (true);
// Set the edges constraints e10->node (n0);
place.addToEdge (e01);
submanifoldDef.addToEdge (e01); place.addToEdge (e10);
place.addToEdge (e12); submanifoldDef.addToEdge (e10);
submanifoldDef.addToEdge (e12); place.addToEdge (e21);
grasp.addToEdge (e23); submanifoldDef.addToEdge (e21);
submanifoldDef.addToEdge (e23); grasp.addToEdge (e32);
grasp.addToEdge (e34); submanifoldDef.addToEdge (e32);
submanifoldDef.addToEdge (e34); grasp.addToEdge (e43);
submanifoldDef.addToEdge (e43);
// Set the waypoints
weForw->setWaypoint (0, e01, n1); weBack->setWaypoint (0, e43, n3);
weForw->setWaypoint (1, e12, n2); weForw->setWaypoint (1, (levelSetGrasp)?e32_ls:e32, n2);
weForw->setWaypoint (2, e23, n3); weBack->setWaypoint (2, e21, n1);
// Populate bacward edge if (levelSetPlace) {
EdgePtr_t e43 = n4->linkTo (name + "_e43", n3, -1, Edge::create), if (!place.isFoliated ()) {
e32 = n3->linkTo (name + "_e12", n2, -1, hppDout (warning, "You asked for a LevelSetEdge for placement, "
(levelSetPlace?(Node::EdgeFactory)LevelSetEdge::create:Edge::create)), "but did not specify the target foliation. "
e21 = n2->linkTo (name + "_e23", n1, -1, Edge::create), "It will have no effect");
e10 = weBack; }
e32_ls->node (n4);
e43->node (n4); e32_ls->setShort (true);
e32->node (n4); e32->setShort (true); grasp.addToEdge (e32_ls);
e21->node (n0); e21->setShort (true); place.specifyFoliation (e32_ls);
e10->node (n0); submanifoldDef.addToEdge (e32_ls);
}
place.addToEdge (e10); if (levelSetGrasp) {
submanifoldDef.addToEdge (e10); if (!grasp.isFoliated ()) {
place.addToEdge (e21); hppDout (warning, "You asked for a LevelSetEdge for grasping, "
submanifoldDef.addToEdge (e21); "but did not specify the target foliation. "
grasp.addToEdge (e32); "It will have no effect");
submanifoldDef.addToEdge (e32); }
grasp.addToEdge (e43); e12_ls->node (n0);
submanifoldDef.addToEdge (e43); e12_ls->setShort (true);
place.addToEdge (e12_ls);
weBack->setWaypoint (0, e43, n3); grasp.specifyFoliation (e12_ls);
weBack->setWaypoint (1, e32, n2); submanifoldDef.addToEdge (e12_ls);
weBack->setWaypoint (2, e21, n1); }
if (levelSetPlace && !place.isFoliated ()) { return std::make_pair (weForw, weBack);
hppDout (warning, "You asked for a LevelSetEdge for placement, " }
"but did not specify the target foliation. "
"It will have no effect"); template <> EdgePair_t
} else { createEdges <GraspOnly | PlaceOnly>(
place.specifyFoliation (HPP_DYNAMIC_PTR_CAST (LevelSetEdge, e32)); const std::string& forwName, const std::string& backName,
const NodePtr_t& from, const NodePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& ,
const FoliatedManifold& place, const FoliatedManifold& ,
const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef)
{
// Create the edges
WaypointEdgePtr_t weForw = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
from->linkTo (forwName, to, wForw, WaypointEdge::create)),
weBack = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
to-> linkTo (backName, from, wBack, WaypointEdge::create));
weForw->nbWaypoints (1);
weBack->nbWaypoints (1);
std::string name = forwName;
NodeSelectorPtr_t ns = weForw->parentGraph ()->nodeSelector ();
NodePtr_t n0 = from,
n1 = ns->createNode (name + "_intersec", true),
n2 = to;
EdgePtr_t e01 = n0->linkTo (name + "_e01", n1, -1, Edge::create),
e12 = weForw;
LevelSetEdgePtr_t e01_ls;
if (levelSetGrasp)
e01_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
n0->linkTo (name + "_e01_ls", n1, -1, LevelSetEdge::create));
// Set the edges properties
e01->node (n0);
e12->node (n1);
// set the nodes constraints
// From and to are not populated automatically to avoid duplicates.
place.addToNode (n1);
grasp.addToNode (n1);
submanifoldDef.addToNode (n1);
// Set the edges constraints
place.addToEdge (e01);
submanifoldDef.addToEdge (e01);
grasp.addToEdge (e12);
submanifoldDef.addToEdge (e12);
// Set the waypoints
weForw->setWaypoint (0, (levelSetGrasp)?e01_ls:e01, n1);
// Populate bacward edge
EdgePtr_t e21 = n2->linkTo (name + "_e21", n1, -1, Edge::create),
e10 = weBack;
LevelSetEdgePtr_t e21_ls;
if (levelSetPlace)
e21_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
n2->linkTo (name + "_e21_ls", n1, -1, LevelSetEdge::create));
e21->node (n2);
e10->node (n0);
place.addToEdge (e10);
submanifoldDef.addToEdge (e10);
grasp.addToEdge (e21);
submanifoldDef.addToEdge (e21);
weForw->setWaypoint (0, (levelSetGrasp)?e21_ls:e21, n1);
if (levelSetPlace) {
if (!place.isFoliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for placement, "
"but did not specify the target foliation. "
"It will have no effect");
}
e21_ls->node (n2);
e21_ls->setShort (true);
grasp.addToEdge (e21_ls);
place.specifyFoliation (e21_ls);
submanifoldDef.addToEdge (e21_ls);
}
if (levelSetGrasp) {
if (!grasp.isFoliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for grasping, "
"but did not specify the target foliation. "
"It will have no effect");
}
e01_ls->node (n0);
e01_ls->setShort (true);
place.addToEdge (e01_ls);
grasp.specifyFoliation (e01_ls);
submanifoldDef.addToEdge (e01_ls);
}
return std::make_pair (weForw, weBack);
} }
if (levelSetGrasp && !grasp.isFoliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for grasping, " template <> EdgePair_t
"but did not specify the target foliation. " createEdges <WithPreGrasp | NoPlace>(
"It will have no effect"); const std::string& forwName, const std::string& backName,
grasp.specifyFoliation (HPP_DYNAMIC_PTR_CAST (LevelSetEdge, e12)); const NodePtr_t& from, const NodePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold&, const FoliatedManifold&,
const bool levelSetGrasp, const bool,
const FoliatedManifold& submanifoldDef)
{
// Create the edges
WaypointEdgePtr_t weForw = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
from->linkTo (forwName, to, wForw, WaypointEdge::create)),
weBack = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
to-> linkTo (backName, from, wBack, WaypointEdge::create));
weForw->nbWaypoints (1);
weBack->nbWaypoints (1);
std::string name = forwName;
NodeSelectorPtr_t ns = weForw->parentGraph ()->nodeSelector ();
NodePtr_t n0 = from,
n1 = ns->createNode (name + "_pregrasp", true),
n2 = to;
EdgePtr_t e01 = n0->linkTo (name + "_e01", n1, -1, Edge::create),
e12 = weForw;
LevelSetEdgePtr_t e12_ls;
if (levelSetGrasp)
e12_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
n1->linkTo (name + "_e12_ls", n2, -1, LevelSetEdge::create));
// Set the edges properties
e01->node (n0);
e12->node (n0); e12->setShort (true);
// set the nodes constraints
// From and to are not populated automatically to avoid duplicates.
pregrasp.addToNode (n1);
submanifoldDef.addToNode (n1);
// Set the edges constraints
submanifoldDef.addToEdge (e01);
submanifoldDef.addToEdge (e12);
// Set the waypoints
weForw->setWaypoint (0, e01, n1);
// weForw->setWaypoint (1, (levelSetGrasp)?e12_ls:e12, n2);
// Populate bacward edge
EdgePtr_t e21 = n2->linkTo (name + "_e21", n1, -1, Edge::create),
e10 = weBack;
e21->node (n0); e21->setShort (true);
e10->node (n0);
submanifoldDef.addToEdge (e10);
submanifoldDef.addToEdge (e21);
weForw->setWaypoint (0, e21, n1);
if (levelSetGrasp) {
hppDout (error, "You specified a foliated grasp with no placement. "
"This is currently unsupported.");
if (!grasp.isFoliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for grasping, "
"but did not specify the target foliation. "
"It will have no effect");
}
e12_ls->node (n0);
e12_ls->setShort (true);
grasp.specifyFoliation (e12_ls);
submanifoldDef.addToEdge (e12_ls);
}
return std::make_pair (weForw, weBack);
} }
return std::make_pair (weForw, weBack);
} template <> EdgePair_t
createEdges <GraspOnly | NoPlace>(
const std::string& forwName, const std::string& backName,
const NodePtr_t& from, const NodePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold&,
const FoliatedManifold&, const FoliatedManifold&,
const bool levelSetGrasp, const bool,
const FoliatedManifold& submanifoldDef)
{
// Create the edges
EdgePtr_t eForw;
if (levelSetGrasp)
eForw = from->linkTo (forwName, to, wForw, LevelSetEdge::create);
else eForw = from->linkTo (forwName, to, wForw, Edge::create);
EdgePtr_t eBack = to-> linkTo (backName, from, wBack, Edge::create);
std::string name = forwName;
eForw->node (from);
submanifoldDef.addToEdge (eForw);
eBack->node (from);
submanifoldDef.addToEdge (eBack);
if (levelSetGrasp) {
if (!grasp.isFoliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for grasping, "
"but did not specify the target foliation. "
"It will have no effect");
}
grasp.specifyFoliation (HPP_DYNAMIC_PTR_CAST (LevelSetEdge, eForw));
}
return std::make_pair (eForw, eBack);
}
} // namespace graph } // namespace graph
} // namespace manipulation } // namespace manipulation
} // namespace hpp } // namespace hpp
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment