diff --git a/NEWS b/NEWS
index 0c133eb5bbe48a3eb5fc2ec43c6e023977eecc5f..0a8dde97929b493080f0e8684e706d76b8e35e87 100644
--- a/NEWS
+++ b/NEWS
@@ -1,4 +1,26 @@
-*- outline -*-
+New in
+* In graph::steeringMethod, if q1 == q2, the steering method calls the problem
+ inner steering method. This avoids a failure if no loop transition has been
+ set on the state containing q1.
+* In class Edge,
+ - different security margins for different pairs of links can
+ be set for collision checking,
+ - rename some methods for homogeneity with python bindings
+ - applyConstraints -> generateTargetConfig
+ - configConstraint -> targetConstraint,
+ - from -> stateFrom, to -> stateTo.
+* In class LevelSetEdge,
+ - write documentation,
+ - rename method applyConstraintsWithOffset into generateTargetConfigOnLeaf,
+ - add read access to condition and parameterization constraints.
+* In class GraphComponent,
+ - add cost
+* Add end-effector-tracjectory plugin.
+* In class ManipulationPlanner,
+ - call the problem PathValidation instead of the edge one.
+
+New in 4.8.0
* Rewrite steering method CrossStateOptimization.
* In graph component classes (State, Edge, Graph) locked joints are handled as other numerical constraints.
- decouple waypoint computations.
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index 2d1578be3c59d3f9509f581df72d77001ef03c81..9ca64de3f434a2e125f080fe33b7d42657e5d5f8 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -1,6 +1,5 @@
INPUT = @CMAKE_SOURCE_DIR@/doc \
@CMAKE_SOURCE_DIR@/include \
- @CMAKE_SOURCE_DIR@/src/steering-method/cross-state-optimization/function.cc \
@CMAKE_BINARY_DIR@/doc
HTML_EXTRA_FILES = @CMAKE_SOURCE_DIR@/doc/ObjectManipulation_MasterThesis_JosephMirabel.pdf
diff --git a/doc/constraint-graph.png b/doc/constraint-graph.png
new file mode 100644
index 0000000000000000000000000000000000000000..c28a8da7e529fc2e9ccc490756027554127f1c53
Binary files /dev/null and b/doc/constraint-graph.png differ
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index ac42495677b7d58acdf042e5877b782d092796bf..90d8cb135b3cd71c85f625e0723fb9434f753906 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -71,42 +71,59 @@ namespace hpp {
const StateWkPtr_t& from,
const StateWkPtr_t& to);
- /// Apply edge constraint
+ /// Generate a reachable configuration in the target state
///
- /// \param nnear node containing the configuration defining the right
+ /// \param nStart node containing the configuration defining the right
/// hand side of the edge constraint,
- /// \param[in,out] q configuration to which the edge constraint is
- /// applied.
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear
+ /// \deprecated Use generateTargetConfig instead.
+ virtual bool applyConstraints (core::NodePtr_t nStart,
+ ConfigurationOut_t q) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Generate a reachable configuration in the target state
///
- /// \sa hpp::core::ConfigProjector::rightHandSideFromConfig
- virtual bool applyConstraints (core::NodePtr_t nnear, ConfigurationOut_t q) const;
- /// Apply edge constraint
+ /// \param qStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear.
+ /// \deprecated Use generateTargetConfig instead.
+ virtual bool applyConstraints (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Generate a reachable configuration in the target state
///
- /// \param qoffset configuration defining the right hand side of the
- /// edge constraint,
- /// \param[in,out] q configuration to which the edge constraint is
- /// applied.
+ /// \param nStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nStart.
+ virtual bool generateTargetConfig(core::NodePtr_t nStart,
+ ConfigurationOut_t q) const;
+
+ /// Generate a reachable configuration in the target state
///
- /// \sa hpp::core::ConfigProjector::rightHandSideFromConfig
- virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
+ /// \param qStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear.
+ virtual bool generateTargetConfig (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const;
virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
ConfigurationIn_t q2) const;
- /// Get the destination
- /// \deprecated from and to method are renamed stateFrom and stateTo.
- /// method from cannot be binded in python since "from"
- /// is a keyword in python.
- StatePtr_t to () const HPP_MANIPULATION_DEPRECATED;
-
- /// Get the origin
- /// \deprecated from and to method are renamed stateFrom and stateTo.
- /// method from cannot be binded in python since "from"
- /// is a keyword in python.
- StatePtr_t from () const HPP_MANIPULATION_DEPRECATED;
-
/// Get the destination
StatePtr_t stateTo () const;
@@ -174,7 +191,12 @@ namespace hpp {
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
/// Constraint of the destination state and of the path
- ConstraintSetPtr_t configConstraint() const;
+ /// \deprecated Use targetConstraint instead
+ ConstraintSetPtr_t configConstraint() const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Constraint of the destination state and of the path
+ ConstraintSetPtr_t targetConstraint() const;
void setShort (bool isShort) {
isShort_ = isShort;
@@ -195,8 +217,13 @@ namespace hpp {
/// Constructor
Edge (const std::string& name);
- virtual ConstraintSetPtr_t buildConfigConstraint();
+ virtual ConstraintSetPtr_t buildConfigConstraint()
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Build path and target state constraint set.
+ virtual ConstraintSetPtr_t buildTargetConstraint();
+ /// Build path constraints
virtual ConstraintSetPtr_t buildPathConstraint();
virtual void initialize ();
@@ -212,7 +239,7 @@ namespace hpp {
/// Constraint ensuring that a q_proj will be in to_ and in the
/// same leaf of to_ as the configuration used for initialization.
- ConstraintSetPtr_t configConstraints_;
+ ConstraintSetPtr_t targetConstraints_;
/// The two ends of the transition.
StateWkPtr_t from_, to_;
@@ -236,21 +263,34 @@ namespace hpp {
friend class Graph;
}; // class Edge
- /// Edge with waypoint.
- /// Waypoints are handled recursively, i.e.\ class WaypointEdge contains only a
- /// State and an Edge, the second Edge being itself.
- /// In this package, the State in a WaypointEdge is semantically different from other State
- /// because it does not correspond to a state with different manipulation rules. It has
- /// the same rules as another State (either Edge::from() or Edge::to()).
+ /// Edge with intermediate waypoint states.
+ ///
+ /// This class implements a transition from one state to another state
+ /// with intermediate states in-between. This feature is particularly
+ /// interesting when manipulating objects. Between a state where a gripper
+ /// does not grasp an object and a state where the same gripper grasps
+ /// the object, it is useful to add an intermediate state where the
+ /// gripper is close to the object.
///
- /// Semantically, a waypoint State is fully part of the WaypointEdge. When a corresponding path
- /// reaches it, no planning is required to know what to do next. To the contrary, when a path reaches
- /// Edge::from() or Edge::to(), there may be several possibilities.
+ /// Waypoints are handled recursively, i.e.\ class WaypointEdge
+ /// contains only a State and an Edge, the second Edge being
+ /// itself. In this package, the State in a WaypointEdge is
+ /// semantically different from other State because it does not
+ /// correspond to a state with different manipulation rules. It
+ /// has the same rules as another State (either Edge::stateFrom() or
+ /// Edge::stateTo()).
+ ///
+ /// Semantically, a waypoint State is fully part of the WaypointEdge.
+ /// When a corresponding path reaches it, no planning is required to know
+ /// what to do next. To the contrary, when a path reaches
+ /// Edge::stateFrom() or Edge::stateTo(), there may be several
+ /// possibilities.
///
/// \note
- /// Implementation details: let's say, between the two states \f$N_f\f$ and \f$N_t\f$,
- /// two waypoints are required:
- /// \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1 \xrightarrow{e_2} N_t\f$.
+ /// Implementation details: let's say, between the two states \f$N_f\f$
+ /// and \f$N_t\f$, two waypoints are required:
+ /// \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1
+ /// \xrightarrow{e_2} N_t\f$.
/// The WaypointEdge contains:
/// \li from: \f$N_f\f$,
/// \li to: \f$N_t\f$,
@@ -270,7 +310,30 @@ namespace hpp {
virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
- virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
+
+ /// Generate a reachable configuration in the target state
+ ///
+ /// \param qStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear.
+ /// deprecated Used generateTargetConfig instead.
+ virtual bool applyConstraints (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Generate a reachable configuration in the target state
+ ///
+ /// \param qStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear.
+ virtual bool generateTargetConfig (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const;
/// Return the index-th edge.
const EdgePtr_t& waypoint (const std::size_t index) const;
@@ -313,7 +376,78 @@ namespace hpp {
WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
- /// Edge that find intersection of level set.
+ /// Edge that handles crossed foliations
+ ///
+ /// Let us consider the following simple constraint graph
+ /// corresponding to a robot grasping an object with one gripper.
+ ///
+ /// \image html constraint-graph.png "Simple constraint graph corresponding to a robot grasping an object."
+ ///
+ /// In order to disambiguate, we assume here that
+ /// \li transition <b>Grasp object</b> is in \b Placement state,
+ /// \li transition <b>Release object</b> is in \b Grasp state.
+ ///
+ /// If state \b Placement is defined by the object lying on a planar
+ /// polygonal surface, then
+ /// \li state \b Placement,
+ /// \li transition \b Transit, and
+ /// \li transition <b>Grasp object</b>
+ ///
+ /// are all constrained in a foliated manifold parameterized by the
+ /// position of the box on the surface.
+ ///
+ /// Likewise, if the object is cylindrical the grasp may have a degree
+ /// of freedom corresponding to the angle around z-axis of the gripper
+ /// with respect to the object. See classes
+ /// \link hpp::manipulation::Handle Handle\endlink and
+ /// \link hpp::pinocchio::Gripper Gripper\endlink for details.
+ /// In this latter case,
+ /// \li state \b Grasp,
+ /// \li transition \b Transfer, and
+ /// \li transition <b>Release object</b>
+ ///
+ /// are all constrained in a foliated manifold parameterized by the
+ /// angle around z-axis of the gripper with respect to the object.
+ ///
+ /// Let us denote
+ /// \li \c grasp the numerical constraint defining state \b Grasp,
+ /// \li \c placement the numerical constraint defining state \b Placement,
+ /// \li \c grasp_comp the parameterized constraint defining a leaf
+ /// of \c Transfer (the angle between the gripper and the
+ /// object),
+ /// \li \c placement_comp the parameterized constraint defining a leaf
+ /// of \b Placement (the position of the object on the contact
+ /// surface).
+ ///
+ /// As explained in <a
+ /// href="https://hal.archives-ouvertes.fr/hal-01358767">this
+ /// paper </a>, we are in the crossed foliation case and manipulation RRT
+ /// will never be able to connect trees expanding in different leaves of
+ /// the foliation.
+ ///
+ /// This class solves this issue in the following way by creating an
+ /// instance of LevelSetEdge between \b Placement and \b Grasp.
+ ///
+ /// When extending a configuration \f$\mathbf{q}_{start}\f$ in state
+ /// \b Placement, this transition will produce a target configuration
+ /// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig)
+ /// \endlink as follows.
+ ///
+ /// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge
+ /// histogram (see method \link LevelSetEdge::histogram histogram\endlink)
+ /// \li compute right hand side of \c grasp_comp with
+ /// \f$\mathbf{q}_{rand}\f$,
+ /// \li compute right hand side of \c placement_comp with
+ /// \f$\mathbf{q}_{start}\f$,
+ /// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp)
+ /// using input configuration \f$\mathbf{q}\f$. Note that the
+ /// parent method Edge::generateTargetConfig does the same without
+ /// adding \c grasp_comp.
+ ///
+ /// The constraints parameterizing the target state foliation
+ /// (\c graps_comp in our example) are passed to class instances
+ /// using method \link LevelSetEdge::insertParamConstraint
+ /// insertParamConstraint\endlink.
class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge
{
public:
@@ -325,27 +459,115 @@ namespace hpp {
const GraphWkPtr_t& graph, const StateWkPtr_t& from,
const StateWkPtr_t& to);
- virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
-
- virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
-
- virtual ConstraintSetPtr_t buildConfigConstraint();
-
+ /// Generate a reachable configuration in the target state
+ ///
+ /// \param nStart node containing the configuration defining the right
+ /// hand side of the edge constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear
+ /// \deprecated Use generateTargetConfig instead.
+ virtual bool applyConstraints (core::NodePtr_t nStart,
+ ConfigurationOut_t q) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Generate a reachable configuration in the target state
+ ///
+ /// \param qStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear.
+ /// \deprecated Use generateTargetConfig instead.
+ virtual bool applyConstraints (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Generate a reachable configuration in the target state
+ ///
+ /// \param nStart node containing the configuration defining the right
+ /// hand side of the edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nStart.
+ virtual bool generateTargetConfig(core::NodePtr_t nStart,
+ ConfigurationOut_t q) const;
+
+ /// Generate a reachable configuration in the target state
+ ///
+ /// \param qStart configuration defining the right hand side of the
+ /// edge path constraint,
+ /// \param[in,out] q input configuration used to initialize the
+ /// numerical solver and output configuration lying
+ /// in the target state and reachable along the edge
+ /// from nnear.
+ virtual bool generateTargetConfig (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const;
+
+ /// Generate a reachable configuration in leaf of target state
+ /// \param qStart configuration defining the right hand side of the
+ /// edge path constraint,
+ /// \param qLeaf configuration used to set the right hand side of
+ /// the target state foliation. See method
+ /// \link LevelSetEdge::insertParamConstraint
+ /// insertParamConstraint\endlink.
+ bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
+ ConfigurationIn_t qLeaf,
+ ConfigurationOut_t q) const;
+
+ /// \deprecated Use buildTargetConstraint instead
+ virtual ConstraintSetPtr_t buildConfigConstraint()
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Build path and target state constraints
+ virtual ConstraintSetPtr_t buildTargetConstraint();
+
+ /// Build the histogram
+ /// \sa LevelSetEdge::histogram.
void buildHistogram ();
+ /// Return pointer on histogram of the edge
+ ///
+ /// The edge histogram is a container of configurations defined by
+ /// a set of constraints called the <b>condition constraints</b>
+ /// that a configuration should satisfy to be inserted in the
+ /// histogram.
+ ///
+ /// The histogram is passed to the Roadmap via the graph (method
+ /// Graph::insertHistogram). The roadmap then populates the histogram
+ /// with all new configurations satisfying the condition constraints.
+ ///
+ /// The condition constraints should therefore be the constraints of
+ /// the target state of the level set edge.
+ ///
+ /// \sa LevelSetEdge::conditionConstraints
+ /// LevelSetEdge::insertConditionConstraint
LeafHistogramPtr_t histogram () const;
/// \name Foliation definition
/// \{
- /// Insert a numerical constraint that parametrizes the foliation
+ /// Insert a constraints parameterizing the target state foliation
+ /// \param nm the numerical constraint,
+ /// \param passiveDofs the passive degrees of freedom of the
+ /// constraint.
void insertParamConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
- /// Insert a numerical constraint that defines the foliation
+ /// Get constraints parameterizing the target state foliation
+ const NumericalConstraints_t& paramConstraints() const;
+
+ /// Insert a condition constraint
+ /// \sa LevelSetEdge::histogram
void insertConditionConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
+ /// Get constraints parameterizing the target state foliation
+ /// \sa LevelSetEdge::histogram
+ const NumericalConstraints_t& conditionConstraints() const;
/// \}
/// Print the object in a stream.
@@ -367,9 +589,6 @@ namespace hpp {
virtual void initialize ();
private:
- bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
- ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;
-
// Parametrizer
// NumericalConstraints_t
NumericalConstraints_t paramNumericalConstraints_;
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index ff07201995951b2b16f7018bb2487f7af084d023..af5840675609ddf46ee14769a61c22ef944fd72f 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -122,10 +122,18 @@ namespace hpp {
/// \return The initialized projector.
ConstraintSetPtr_t configConstraint (const StatePtr_t& state) const;
- /// Constraint to project onto the same leaf as config.
- /// \param edges a list of edges defining the foliation.
- /// \return The constraint.
- ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const;
+ /// Constraints of path and target state of an edge
+ /// \deprecated Use tagetConstraint instead.
+ ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const
+ HPP_MANIPULATION_DEPRECATED;
+
+ /// Constraints a configuration in target state should satisfy
+ /// \param edge a transition
+ /// \return The set of constraints a configuration lying in the
+ /// target state of the edge should satisfy. This set
+ /// includes the paths constraints of the edge.
+ /// \sa Edge::targetConstraint.
+ ConstraintSetPtr_t targetConstraint (const EdgePtr_t& edge) const;
/// Get error of a config with respect to a state constraint
///
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index f08aa4bc8442e088fb8cd3f7ef285631bf65798b..412e82fa7df1c90de75dba8222c73b1e29966e29 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -41,7 +41,7 @@ namespace hpp {
Edge::Edge (const std::string& name) :
GraphComponent (name), isShort_ (false),
pathConstraints_ (),
- configConstraints_ (),
+ targetConstraints_ (),
steeringMethod_ (),
securityMargins_ (),
pathValidation_ ()
@@ -50,16 +50,6 @@ namespace hpp {
Edge::~Edge ()
{}
- StatePtr_t Edge::to () const
- {
- return to_.lock();
- }
-
- StatePtr_t Edge::from () const
- {
- return from_.lock();
- }
-
StatePtr_t Edge::stateTo () const
{
return to_.lock();
@@ -94,7 +84,7 @@ namespace hpp {
// 01 | ? | ? | 0 | 0
// 11 | ? | 1 | * | 0
// 10 | ? | 1 | 1 | 1
- //
+ //
/// true if reverse
if ( (!src_contains_q0 && !src_contains_q1)
|| (!dst_contains_q0 && !dst_contains_q1)
@@ -133,7 +123,7 @@ namespace hpp {
ConfigProjectorPtr_t proj) const
{
GraphPtr_t g = graph_.lock ();
-
+
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
state ()->insertNumericalConstraints (proj);
@@ -175,7 +165,7 @@ namespace hpp {
void Edge::initialize ()
{
- configConstraints_ = buildConfigConstraint ();
+ targetConstraints_ = buildTargetConstraint ();
pathConstraints_ = buildPathConstraint ();
isInit_ = true;
}
@@ -201,13 +191,19 @@ namespace hpp {
return os;
}
+ ConstraintSetPtr_t Edge::targetConstraint() const
+ {
+ throwIfNotInitialized ();
+ return targetConstraints_;
+ }
+
ConstraintSetPtr_t Edge::configConstraint() const
{
throwIfNotInitialized ();
- return configConstraints_;
+ return targetConstraints_;
}
- // Merge constraints of several graph components into a config projectors
+ // Merge constraints of several graph components into a config projector
// Replace constraints and complement by combination of both when
// necessary.
static void mergeConstraintsIntoConfigProjector
@@ -274,6 +270,11 @@ namespace hpp {
}
ConstraintSetPtr_t Edge::buildConfigConstraint()
+ {
+ return buildTargetConstraint();
+ }
+
+ ConstraintSetPtr_t Edge::buildTargetConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -288,6 +289,11 @@ namespace hpp {
if (state () != stateTo ()) {
components.push_back (state ());
}
+ // Copy constraints from
+ // - graph,
+ // - this edge,
+ // - the destination state,
+ // - the state in which the transition lies if different
mergeConstraintsIntoConfigProjector (proj, components, parentGraph ());
constraint->addConstraint (proj);
@@ -381,18 +387,31 @@ namespace hpp {
}
}
- bool Edge::applyConstraints (core::NodePtr_t nnear, ConfigurationOut_t q) const
+ bool Edge::applyConstraints (core::NodePtr_t nStart,
+ ConfigurationOut_t q) const
{
- return applyConstraints (*(nnear->configuration ()), q);
+ return generateTargetConfig (*(nStart->configuration ()), q);
}
bool Edge::applyConstraints (ConfigurationIn_t qoffset,
ConfigurationOut_t q) const
{
- ConstraintSetPtr_t c = configConstraint ();
+ return generateTargetConfig(qoffset, q);
+ }
+
+ bool Edge::generateTargetConfig (core::NodePtr_t nStart,
+ ConfigurationOut_t q) const
+ {
+ return generateTargetConfig (*(nStart->configuration ()), q);
+ }
+
+ bool Edge::generateTargetConfig (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
+ {
+ ConstraintSetPtr_t c = targetConstraint();
ConfigProjectorPtr_t proj = c->configProjector ();
- proj->rightHandSideFromConfig (qoffset);
- if (isShort_) q = qoffset;
+ proj->rightHandSideFromConfig (qStart);
+ if (isShort_) q = qStart;
if (c->apply (q)) return true;
const ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
if (ss.nbFailure () > ss.nbSuccess ()) {
@@ -441,7 +460,7 @@ namespace hpp {
core::PathVectorPtr_t pv = core::PathVector::create
(graph_.lock ()->robot ()->configSize (),
graph_.lock ()->robot ()->numberDof ());
- // Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
+ // Many times, this will be called rigth after WaypointEdge::generateTargetConfig so config_
// already satisfies the constraints.
size_type n = edges_.size();
assert (configs_.cols() == n + 1);
@@ -453,17 +472,17 @@ namespace hpp {
for (size_type i = 0; i < n; ++i) {
if (i < (n-1) && !useCache) configs_.col (i+1) = q2;
- if (i < (n-1) && !edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
- hppDout (info, "Waypoint edge " << name() << ": applyConstraints failed at waypoint " << i << "."
+ if (i < (n-1) && !edges_[i]->generateTargetConfig (configs_.col(i), configs_.col (i+1))) {
+ hppDout (info, "Waypoint edge " << name() << ": generateTargetConfig failed at waypoint " << i << "."
<< "\nUse cache: " << useCache
);
lastSucceeded_ = false;
return false;
}
- assert (configConstraint ());
- assert (configConstraint ()->configProjector ());
+ assert (targetConstraint());
+ assert (targetConstraint()->configProjector ());
value_type eps
- (configConstraint ()->configProjector ()->errorThreshold ());
+ (targetConstraint()->configProjector ()->errorThreshold ());
if ((configs_.col(i) - configs_.col (i+1)).squaredNorm () > eps*eps) {
if (!edges_[i]->build (p, configs_.col(i), configs_.col (i+1))) {
hppDout (info, "Waypoint edge " << name()
@@ -481,13 +500,20 @@ namespace hpp {
return true;
}
- bool WaypointEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
+ bool WaypointEdge::applyConstraints (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
+ {
+ return generateTargetConfig(qStart,q);
+ }
+
+ bool WaypointEdge::generateTargetConfig (ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
{
assert (configs_.cols() == size_type(edges_.size() + 1));
- configs_.col(0) = qoffset;
+ configs_.col(0) = qStart;
for (std::size_t i = 0; i < edges_.size (); ++i) {
configs_.col (i+1) = q;
- if (!edges_[i]->applyConstraints (configs_.col(i), configs_.col (i+1))) {
+ if (!edges_[i]->generateTargetConfig (configs_.col(i), configs_.col (i+1))) {
q = configs_.col(i+1);
lastSucceeded_ = false;
return false;
@@ -523,7 +549,7 @@ namespace hpp {
const EdgePtr_t& WaypointEdge::waypoint (const std::size_t index) const
{
- assert (index < edges_.size());
+ assert (index < edges_.size());
return edges_[index];
}
@@ -584,7 +610,14 @@ namespace hpp {
}
}
- bool LevelSetEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
+ bool LevelSetEdge::applyConstraints(ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
+ {
+ return generateTargetConfig(qStart, q);
+ }
+
+ bool LevelSetEdge::generateTargetConfig(ConfigurationIn_t qStart,
+ ConfigurationOut_t q) const
{
// First, get an offset from the histogram
statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistrib ();
@@ -592,42 +625,53 @@ namespace hpp {
hppDout (warning, "Edge " << name() << ": Distrib is empty");
return false;
}
- const Configuration_t& qlevelset = *(distrib ()->configuration ());
+ const Configuration_t& qLeaf = *(distrib ()->configuration ());
- return applyConstraintsWithOffset (qoffset, qlevelset, q);
+ return generateTargetConfigOnLeaf (qStart, qLeaf, q);
+ }
+
+ bool LevelSetEdge::applyConstraints(core::NodePtr_t nStart,
+ ConfigurationOut_t q) const
+ {
+ return generateTargetConfig(nStart, q);
}
- bool LevelSetEdge::applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const
+ bool LevelSetEdge::generateTargetConfig(core::NodePtr_t nStart,
+ ConfigurationOut_t q) const
{
- // First, get an offset from the histogram that is not in the same connected component.
- statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
+ // First, get an offset from the histogram that is not in the same connected component.
+ statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (nStart->connectedComponent ());
if (distrib.size () == 0) {
hppDout (warning, "Edge " << name() << ": Distrib is empty");
return false;
}
- const Configuration_t& qlevelset = *(distrib ()->configuration ()),
- qoffset = *(n_offset->configuration ());
+ const Configuration_t& qLeaf = *(distrib ()->configuration ()),
+ qStart = *(nStart->configuration ());
- return applyConstraintsWithOffset (qoffset, qlevelset, q);
+ return generateTargetConfigOnLeaf (qStart, qLeaf, q);
}
- bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qoffset,
- ConfigurationIn_t qlevelset, ConfigurationOut_t q) const
+ bool LevelSetEdge::generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
+ ConfigurationIn_t qLeaf,
+ ConfigurationOut_t q) const
{
// First, set the offset.
- ConstraintSetPtr_t cs = configConstraint ();
+ ConstraintSetPtr_t cs = targetConstraint();
const ConfigProjectorPtr_t cp = cs->configProjector ();
assert (cp);
- cp->rightHandSideFromConfig (qoffset);
+ // Set right hand side of edge constraints with qStart
+ cp->rightHandSideFromConfig (qStart);
+ // Set right hand side of constraints parameterizing the target state
+ // foliation with qLeaf.
for (NumericalConstraints_t::const_iterator it =
paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
- cp->rightHandSideFromConfig (*it, qlevelset);
+ cp->rightHandSideFromConfig (*it, qLeaf);
}
// Eventually, do the projection.
- if (isShort_) q = qoffset;
+ if (isShort_) q = qStart;
if (cs->apply (q)) return true;
::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
if (ss.nbFailure () > ss.nbSuccess ()) {
@@ -730,6 +774,11 @@ namespace hpp {
}
ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint()
+ {
+ return buildTargetConstraint();
+ }
+
+ ConstraintSetPtr_t LevelSetEdge::buildTargetConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -738,6 +787,13 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+ // Copy constraints from
+ // - graph,
+ // - param numerical constraints
+ // - this edge,
+ // - the destination state,
+ // - the state in which the transition lies if different
+
g->insertNumericalConstraints (proj);
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
@@ -767,6 +823,11 @@ namespace hpp {
paramPassiveDofs_.push_back (passiveDofs);
}
+ const NumericalConstraints_t& LevelSetEdge::paramConstraints() const
+ {
+ return paramNumericalConstraints_;
+ }
+
void LevelSetEdge::insertConditionConstraint
(const constraints::ImplicitPtr_t& nm,
const segments_t& passiveDofs)
@@ -776,6 +837,11 @@ namespace hpp {
condPassiveDofs_.push_back (passiveDofs);
}
+ const NumericalConstraints_t& LevelSetEdge::conditionConstraints() const
+ {
+ return condNumericalConstraints_;
+ }
+
LevelSetEdge::LevelSetEdge
(const std::string& name) :
Edge (name)
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 3d5ba3b7ccc858ce0d3afc3339d42b0116cb3360..3725672c605ce5b73b10b969a32dc5dd9d87ddf1 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -132,7 +132,7 @@ namespace hpp {
Edges_t edges;
for (Neighbors_t::const_iterator it = from->neighbors ().begin ();
it != from->neighbors ().end (); ++it) {
- if (it->second->to () == to)
+ if (it->second->stateTo () == to)
edges.push_back (it->second);
}
return edges;
@@ -205,7 +205,7 @@ namespace hpp {
(ConfigurationIn_t leafConfig, ConfigurationIn_t config,
const EdgePtr_t& edge, vector_t& error) const
{
- ConstraintSetPtr_t cs (configConstraint (edge));
+ ConstraintSetPtr_t cs (targetConstraint (edge));
ConfigProjectorPtr_t cp (cs->configProjector ());
if (cp) cp->rightHandSideFromConfig (leafConfig);
return cs->isSatisfied (config, error);
@@ -223,7 +223,12 @@ namespace hpp {
ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge) const
{
- return edge->configConstraint ();
+ return edge->targetConstraint ();
+ }
+
+ ConstraintSetPtr_t Graph::targetConstraint (const EdgePtr_t& edge) const
+ {
+ return edge->targetConstraint ();
}
ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge) const
diff --git a/src/graph/guided-state-selector.cc b/src/graph/guided-state-selector.cc
index 334504c95108c16a7ce974c880026344c96bfae7..1ccfec093f592717ec7a706f29a0e1b848eb2611 100644
--- a/src/graph/guided-state-selector.cc
+++ b/src/graph/guided-state-selector.cc
@@ -92,13 +92,13 @@ namespace hpp {
const Neighbors_t& n = state->neighbors();
/// You stay in the same state
for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
- if (it->second->to () == state)
+ if (it->second->stateTo () == state)
nn.insert (it->second, it->first);
/// Go from state it1 to state
// The path will be build from state. So we must find an edge from
// state to it1, that will be reversely
for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
- if (it->second->to () == *it1)
+ if (it->second->stateTo () == *it1)
nn.insert (it->second, it->first);
} else {
States_t::const_iterator it1 = stateList_.begin ();
@@ -118,7 +118,8 @@ namespace hpp {
for (Neighbors_t::const_iterator it = n.begin (); it != n.end (); ++it)
/// You stay in the same state
/// or go from state to state it1
- if (it->second->to () == state || it->second->to () == *it1)
+ if (it->second->stateTo () == state ||
+ it->second->stateTo () == *it1)
nn.insert (it->second, it->first);
}
if (nn.size () > 0 && nn.totalWeight() > 0)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index d42f7cd5a36c8b07b82f318462e9ccdb3521b015..5950d3b2359cbd0c48e42301fd536237d4fcf421 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -55,7 +55,7 @@ namespace hpp {
HPP_DEFINE_TIMECOUNTER(tryConnectToRoadmap);
/// extend steps
HPP_DEFINE_TIMECOUNTER(chooseEdge);
- HPP_DEFINE_TIMECOUNTER(applyConstraints);
+ HPP_DEFINE_TIMECOUNTER(generateTargetConfig);
HPP_DEFINE_TIMECOUNTER(buildPath);
HPP_DEFINE_TIMECOUNTER(projectPath);
HPP_DEFINE_TIMECOUNTER(validatePath);
@@ -241,7 +241,7 @@ namespace hpp {
HPP_DISPLAY_TIMECOUNTER(nearestNeighbor);
HPP_DISPLAY_TIMECOUNTER(delayedEdges);
HPP_DISPLAY_TIMECOUNTER(chooseEdge);
- HPP_DISPLAY_TIMECOUNTER(applyConstraints);
+ HPP_DISPLAY_TIMECOUNTER(generateTargetConfig);
HPP_DISPLAY_TIMECOUNTER(buildPath);
HPP_DISPLAY_TIMECOUNTER(projectPath);
HPP_DISPLAY_TIMECOUNTER(validatePath);
@@ -265,10 +265,10 @@ namespace hpp {
return false;
}
qProj_ = *q_rand;
- HPP_START_TIMECOUNTER (applyConstraints);
+ HPP_START_TIMECOUNTER (generateTargetConfig);
SuccessStatistics& es = edgeStat (edge);
- if (!edge->applyConstraints (n_near, qProj_)) {
- HPP_STOP_TIMECOUNTER (applyConstraints);
+ if (!edge->generateTargetConfig(n_near, qProj_)) {
+ HPP_STOP_TIMECOUNTER (generateTargetConfig);
es.addFailure (reasons_[FAILURE]);
es.addFailure (reasons_[PROJECTION]);
return false;
@@ -278,7 +278,7 @@ namespace hpp {
es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
return false;
}
- HPP_STOP_TIMECOUNTER (applyConstraints);
+ HPP_STOP_TIMECOUNTER (generateTargetConfig);
core::PathPtr_t path;
HPP_START_TIMECOUNTER (buildPath);
if (!edge->build (path, *q_near, qProj_)) {
diff --git a/src/path-optimization/enforce-transition-semantic.cc b/src/path-optimization/enforce-transition-semantic.cc
index 14221df930b6f0b5d9c75be71d69e7177f67d750..675298a93065868201fff1f85084697703a1dddd 100644
--- a/src/path-optimization/enforce-transition-semantic.cc
+++ b/src/path-optimization/enforce-transition-semantic.cc
@@ -40,12 +40,12 @@ namespace hpp {
Edges_t edges;
for (graph::Neighbors_t::const_iterator it = from->neighbors ().begin ();
it != from->neighbors ().end (); ++it) {
- if (it->second->to () == to)
+ if (it->second->stateTo () == to)
edges.push_back (it->second);
}
for (Edges_t::const_iterator it = from->hiddenNeighbors ().begin ();
it != from->hiddenNeighbors ().end (); ++it) {
- if ((*it)->to () == to)
+ if ((*it)->stateTo () == to)
edges.push_back (*it);
}
return edges;
@@ -70,8 +70,8 @@ namespace hpp {
}
Configuration_t q0 = current->initial();
Configuration_t q1 = current->end ();
- StatePtr_t src = c->edge()->from();
- StatePtr_t dst = c->edge()->to ();
+ StatePtr_t src = c->edge()->stateFrom();
+ StatePtr_t dst = c->edge()->stateTo ();
if (src == dst) continue;
bool q0_in_src = src->contains(q0);
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
index 50bfb16e6fae78011e2fa1ccea030c7bebb86581..0e6cf918bee25fa8722df59532dfdd0c7a68332b 100644
--- a/src/path-optimization/spline-gradient-based.cc
+++ b/src/path-optimization/spline-gradient-based.cc
@@ -91,8 +91,8 @@ namespace hpp {
// The path should always go through the start and end states of the
// transition.
assert(!HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, transition));
- graph::StatePtr_t from = transition->from();
- graph::StatePtr_t to = transition->to();
+ graph::StatePtr_t from = transition->stateFrom();
+ graph::StatePtr_t to = transition->stateTo();
graph::StatePtr_t from2 = from, to2 = to;
Configuration_t q0 = path->initial (),
@@ -120,7 +120,8 @@ namespace hpp {
if (use_direct) {
// Nominal case
if (transition->state() != to) {
- constrainEndIntoState (path, i, splines[i], transition->to(), lc);
+ constrainEndIntoState (path, i, splines[i], transition->stateTo(),
+ lc);
}
stateOfStart = to;
} else if (use_reverse) {
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 4216f5edd3e2dd5824e448415985d5ec50d657e6..6e3afcfd10bd0eb68609d7dce677b02d0463e2db 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -91,7 +91,7 @@ namespace hpp {
std::size_t i; // index in parent state_with_depths_t
inline state_with_depth () : s(), e(), l(0), i (0) {}
inline state_with_depth (EdgePtr_t _e, std::size_t _l, std::size_t _i)
- : s(_e->from()), e(_e), l(_l), i (_i) {}
+ : s(_e->stateFrom()), e(_e), l(_l), i (_i) {}
};
typedef std::vector<state_with_depth> state_with_depths_t;
typedef std::map<StatePtr_t,state_with_depths_t> StateMap_t;
@@ -131,7 +131,7 @@ namespace hpp {
// Insert state to if necessary
StateMap_t::iterator next = parent1.insert (
StateMap_t::value_type(
- transition->to(), state_with_depths_t ()
+ transition->stateTo(), state_with_depths_t ()
)).first;
next->second.push_back (
@@ -233,7 +233,7 @@ namespace hpp {
d.addParent (_state, transition)
);
- done = done || (transition->to() == d.s2);
+ done = done || (transition->stateTo() == d.s2);
}
if (done) break;
}
@@ -475,7 +475,7 @@ namespace hpp {
graph::GraphPtr_t cg (problem_.constraintGraph ());
// Fill solvers with graph, node and edge constraints
for (std::size_t j = 0; j < d.N; ++j) {
- graph::StatePtr_t state (transitions [(std::size_t)j]->to ());
+ graph::StatePtr_t state (transitions [(std::size_t)j]->stateTo ());
// initialize solver with state constraints
d.solvers [(std::size_t)j] = state->configConstraint ()->
configProjector ()->solver ();