From 409e1aa65452e86312b204d97dc30d95b1c3afaf Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 15 Apr 2020 13:58:23 +0200
Subject: [PATCH] [WaypointEdge] Specialize initialization.
Set error threshold for each internal edge to the value of the edge divided
by the number of internal edges.
---
include/hpp/manipulation/graph/edge.hh | 3 +++
src/graph/edge.cc | 17 +++++++++++++++++
2 files changed, 20 insertions(+)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 90d8cb13..5ea948be 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -58,6 +58,7 @@ namespace hpp {
/// configuration in the start state by an admissible path.
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent
{
+ friend class WaypointEdge;
public:
typedef core::RelativeMotion RelativeMotion;
@@ -363,6 +364,8 @@ namespace hpp {
void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
const StateWkPtr_t& to);
+ /// Initialize each of the internal edges
+ virtual void initialize ();
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index f7adef5f..548a26cc 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -447,6 +447,22 @@ namespace hpp {
wkPtr_ = weak;
}
+ void WaypointEdge::initialize ()
+ {
+ Edge::initialize();
+ // Set error threshold of internal edge to error threshold of
+ // waypoint edge divided by number of edges.
+ assert(targetConstraint()->configProjector());
+ value_type eps(targetConstraint()->configProjector()
+ ->errorThreshold()/(value_type)edges_.size());
+ for(Edges_t::iterator it(edges_.begin()); it != edges_.end(); ++it){
+ (*it)->initialize();
+ assert ((*it)->targetConstraint());
+ assert ((*it)->targetConstraint()->configProjector());
+ (*it)->targetConstraint()->configProjector()->errorThreshold(eps);
+ }
+ }
+
bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
/// TODO: This is not correct
@@ -533,6 +549,7 @@ namespace hpp {
states_.back() = stateTo();
const size_type nbDof = graph_.lock ()->robot ()->configSize ();
configs_ = matrix_t (nbDof, number + 2);
+ invalidate();
}
void WaypointEdge::setWaypoint (const std::size_t index,
--
GitLab