From 3da80dbb0dfff814862dde1449bd5405e5b9f558 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sat, 28 Mar 2020 16:14:53 +0100
Subject: [PATCH] [Edge] Rename methods applyConstraints as
generateTargetConfig
make old methods deprecated.
This will homogeneize with hpp-manipulation-corba naming.
---
include/hpp/manipulation/graph/edge.hh | 131 +++++++++++++++++++++----
src/graph/edge.cc | 80 ++++++++++-----
src/manipulation-planner.cc | 12 +--
3 files changed, 176 insertions(+), 47 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index a29f0d49..37fb47fd 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -71,24 +71,53 @@ 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;
@@ -258,7 +287,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,10 +365,53 @@ 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;
-
+ /// 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 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 ConstraintSetPtr_t buildConfigConstraint();
void buildHistogram ();
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 8d79daf9..f5fce065 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -84,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)
@@ -123,7 +123,7 @@ namespace hpp {
ConfigProjectorPtr_t proj) const
{
GraphPtr_t g = graph_.lock ();
-
+
g->insertNumericalConstraints (proj);
insertNumericalConstraints (proj);
state ()->insertNumericalConstraints (proj);
@@ -371,18 +371,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
+ {
+ 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 = configConstraint ();
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 ()) {
@@ -431,7 +444,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);
@@ -443,8 +456,8 @@ 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;
@@ -471,13 +484,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;
@@ -513,7 +533,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];
}
@@ -574,7 +594,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 ();
@@ -584,24 +611,31 @@ namespace hpp {
}
const Configuration_t& qlevelset = *(distrib ()->configuration ());
- return applyConstraintsWithOffset (qoffset, qlevelset, q);
+ return applyConstraintsWithOffset (qStart, qlevelset, 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 ());
+ qStart = *(nStart->configuration ());
- return applyConstraintsWithOffset (qoffset, qlevelset, q);
+ return applyConstraintsWithOffset (qStart, qlevelset, q);
}
- bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qoffset,
+ bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qStart,
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const
{
// First, set the offset.
@@ -609,7 +643,7 @@ namespace hpp {
const ConfigProjectorPtr_t cp = cs->configProjector ();
assert (cp);
- cp->rightHandSideFromConfig (qoffset);
+ cp->rightHandSideFromConfig (qStart);
for (NumericalConstraints_t::const_iterator it =
paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
@@ -617,7 +651,7 @@ namespace hpp {
}
// 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 ()) {
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index d42f7cd5..5950d3b2 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_)) {
--
GitLab