diff --git a/NEWS b/NEWS
index 280c90ae22628c9d4a5050e0b017351683ec46c3..77f774fa0a967e640a2ac7143441b77568307378 100644
--- a/NEWS
+++ b/NEWS
@@ -1,6 +1,12 @@
2014/12/03 - Release 1.1
* Add projector for paths.
+2014/12/03 - Release 1.0.2
+ * Fix bug when projection using WaypointEdge
+
+2014/12/03 - Release 1.0.1
+ * Fix bug in WaypointEdge
+
2014/11/18 - Release 1.0
* Manipulation planner based on RRT.
* A graph of constraint.
diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index f1b42d017004736ce179932247030b8d451378ad..38db5b7221a49537b0f211330d771e1f6f17fb16 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -10,5 +10,5 @@ HTML_EXTRA_FILES = @CMAKE_SOURCE_DIR@/doc/ObjectManipulation_MasterThesis_Joseph
ALIASES += Link{1}="\ref \1"
ALIASES += Link{2}="\ref \1 \"\2\""
-ALIASES += LHPP{2}="\Link{hpp::\1::\2, \2}"
-ALIASES += LModel{1}="\LHPP{model, \1}"
+ALIASES += LHPP{2}="\Link{hpp::\1::\2,\2}"
+ALIASES += LModel{1}="\LHPP{model,\1}"
diff --git a/doc/main.hh b/doc/main.hh
index 750032a10e5554934b5e6fafe8c637dfb7312539..b502ca4287a97ba9d23542111e5d6d73fc466115 100644
--- a/doc/main.hh
+++ b/doc/main.hh
@@ -29,7 +29,7 @@ namespace hpp {
and Edge::node(), to retrive this Node.
Edge also contains two sets of \LHPP{core,Constraint}:
\li Edge::configConstraint() returns a \LHPP{core,ConstraintSet} used to generate a configuration lying in Edge::to()
- and respecting the offset (previsously set using hpp::core::Constraint::offsetFromConfig),
+ and respecting the rightHandSide (previsously set using hpp::core::ConfigProjector::leftHandSideFromConfig),
\li Edge::pathConstraint() returns a \LHPP{core,ConstraintSet} to be inserted in \LHPP{core,Path} represented
by this Edge.
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 81189fd88c04f92669bfa0700b23418ab18a29aa..f55650214104eabc1b152c0f9a5cbcd47d33148c 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -87,8 +87,8 @@ namespace hpp {
typedef std::map <JointConstPtr_t, JointPtr_t> JointMap_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
- typedef core::LockedDof LockedDof;
- typedef core::LockedDofPtr_t LockedDofPtr_t;
+ typedef core::LockedJoint LockedJoint;
+ typedef core::LockedJointPtr_t LockedJointPtr_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
typedef core::ConstraintSet ConstraintSet;
@@ -101,7 +101,7 @@ namespace hpp {
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
- typedef std::map <std::string, LockedDofPtr_t> LockedDofConstraintMap_t;
+ typedef std::map <std::string, LockedJointPtr_t> LockedDofConstraintMap_t;
typedef fcl::TriangleP Triangle;
typedef std::list <Triangle> TriangleList;
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 9edfaf82fde503beab3398ef0708b2242ac45186..7eea322ac597578b8bc803255c205ec307c05b53 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -202,7 +202,7 @@ namespace hpp {
typedef std::pair < EdgePtr_t, NodePtr_t > Waypoint;
Waypoint waypoint_;
- mutable Configuration_t config_;
+ mutable Configuration_t config_, result_;
}; // class WaypointEdge
/// Edge that find intersection of level set.
@@ -222,9 +222,9 @@ namespace hpp {
LeafHistogramPtr_t histogram () const;
- void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const EquationTypePtr_t ineq);
+ void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq);
- void insertConfigConstraint (const LockedDofPtr_t lockedDof);
+ void insertConfigConstraint (const LockedJointPtr_t lockedDof);
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -249,8 +249,8 @@ namespace hpp {
/// Extra DifferentiableFunctions_t
DifferentiableFunctions_t extraNumericalFunctions_;
- /// Extra LockedDofs_t
- LockedDofs_t extraLockedDofs_;
+ /// Extra LockedJoints_t
+ LockedJoints_t extraLockedDofs_;
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index 9b4ee224c452a0984fc5ac3e586b6c76c767cd75..925e1c2668308732c913aab49115a85f7884a8de 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -46,18 +46,18 @@ namespace hpp {
typedef hpp::core::Constraint Constraint;
typedef hpp::core::ConstraintPtr_t ConstraintPtr_t;
- typedef hpp::core::LockedDof LockedDof;
- typedef hpp::core::LockedDofPtr_t LockedDofPtr_t;
+ typedef hpp::core::LockedJoint LockedJoint;
+ typedef hpp::core::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ConfigProjector ConfigProjector;
typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
typedef hpp::core::ConstraintSet ConstraintSet;
typedef hpp::core::ConstraintSetPtr_t ConstraintSetPtr_t;
typedef hpp::core::Equality Equality;
- typedef hpp::core::EquationTypePtr_t EquationTypePtr_t;
+ typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
- typedef std::pair < DifferentiableFunctionPtr_t, EquationTypePtr_t > DiffFuncAndIneqPair_t;
+ typedef std::pair < DifferentiableFunctionPtr_t, ComparisonTypePtr_t > DiffFuncAndIneqPair_t;
typedef std::list < DiffFuncAndIneqPair_t > DifferentiableFunctions_t;
- typedef std::list < LockedDofPtr_t > LockedDofs_t;
+ typedef std::list < LockedJointPtr_t > LockedJoints_t;
class Histogram;
class NodeHistogram;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 74f66eeb82f865a3edacfb5d1d372a6ddaab830c..2d7806a1b4e1aa13f0d541f037814303995a23fe 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -52,24 +52,25 @@ namespace hpp {
virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function) __attribute__ ((deprecated));
/// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const EquationTypePtr_t& ineq);
+ virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq);
- /// Add core::LockedDof constraint to the component.
- virtual void addLockedDofConstraint (const LockedDofPtr_t& constraint);
+ /// Add core::LockedJoint constraint to the component.
+ virtual void addLockedJointConstraint
+ (const LockedJointPtr_t& constraint);
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one DifferentiableFunctionPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
- /// Insert the LockedDof constraints in a ConstraintSet
- /// \return true is at least one LockedDofPtr_t was inserted.
+ /// Insert the LockedJoint constraints in a ConstraintSet
+ /// \return true is at least one LockedJointPtr_t was inserted.
bool insertLockedDofs (ConstraintSetPtr_t cs) const;
/// Get a reference to the DifferentiableFunctions_t
const DifferentiableFunctions_t& numericalConstraints() const;
- /// Get a reference to the LockedDofs_t
- const LockedDofs_t& lockedDofConstraints () const;
+ /// Get a reference to the LockedJoints_t
+ const LockedJoints_t& lockedDofConstraints () const;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
@@ -86,8 +87,8 @@ namespace hpp {
/// Stores the numerical constraints.
DifferentiableFunctions_t numericalConstraints_;
- /// List of LockedDof constraints
- LockedDofs_t lockedDofConstraints_;
+ /// List of LockedJoint constraints
+ LockedJoints_t lockedDofConstraints_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index 2a644e5958bb4482642d361cce245ab84d4a694c..8d854855f3ae4b48a8b0664b7b84087a61a79313 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -49,7 +49,8 @@ namespace hpp {
}
/// Should never be called.
- void addLockedDofConstraint (const core::LockedDof& /* constraint */)
+ void addLockedJointConstraint
+ (const core::LockedJoint& /* constraint */)
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
}
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index c8e984851e2315cc115c2e2ac6272b4caecf908d..55d41874ab9c33c58326ddd90d5f518ce3c6b13e 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -19,7 +19,7 @@
# include <boost/function.hpp>
-#include <hpp/core/locked-dof.hh>
+#include <hpp/core/locked-joint.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
@@ -90,7 +90,7 @@ namespace hpp {
}
/// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const EquationTypePtr_t& ineq)
+ virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
{
numericalConstraintsForPath_.push_back (DiffFuncAndIneqPair_t(function,ineq));
}
@@ -101,7 +101,7 @@ namespace hpp {
{
for (DifferentiableFunctions_t::const_iterator it = numericalConstraintsForPath_.begin();
it != numericalConstraintsForPath_.end(); it++)
- proj->addConstraint (it->first, it->second);
+ proj->addFunction (it->first, it->second);
return !numericalConstraintsForPath_.empty ();
}
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 01e0b3da5f74434e093f6d421ba36342fb17c32f..84936c27562eded4818bc9efc1cc17f94f6167a2 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -21,6 +21,7 @@
# include <map>
# include <hpp/core/problem-solver.hh>
# include <hpp/model/device.hh>
+# include "hpp/manipulation/deprecated.hh"
# include "hpp/manipulation/object.hh"
# include "hpp/manipulation/robot.hh"
# include "hpp/manipulation/fwd.hh"
@@ -117,28 +118,32 @@ namespace hpp {
/// return NULL if no grasp named graspName
GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
- /// Add a LockedDof constraint to the map
+ /// Add a LockedJoint constraint to the map
/// \param name key of the constraint as stored in an internal map.
/// \param lockedDof the constraint to add.
- void addLockedDofConstraint (const std::string& name,
- const LockedDofPtr_t& lockedDof)
+ void addLockedJointConstraint (const std::string& name,
+ const LockedJointPtr_t& lockedDof)
{
lockedDofConstraintMap_ [name] = lockedDof;
}
- /// Get a LockedDof constraint by name
+ /// Get a LockedJoint constraint by name
/// \param name key of the constraint as stored in an internal map.
- LockedDofPtr_t lockedDofConstraint (const std::string& name) const;
+ LockedJointPtr_t lockedDofConstraint (const std::string& name) const;
/// Reset constraint set and put back the disable collisions
/// between gripper and handle
virtual void resetConstraints ();
- /// Add differentialFunction to the config projector
- /// Build the config projector if not constructed
- virtual void addConstraintToConfigProjector(
- const std::string& constraintName,
- const DifferentiableFunctionPtr_t& constraint);
+ /// Add differential function to the config projector
+ /// \param constraintName Name given to config projector if created by
+ /// this method.
+ /// \param functionName name of the function as stored in internal map.
+ /// Build the config projector if not yet constructed.
+ /// If constraint is a graps, deactivate collision between gripper and
+ /// object.
+ virtual void addFunctionToConfigProjector
+ (const std::string& constraintName, const std::string& functionName);
/// Build a composite robot from several robots and objects
/// \param robotName Name of the composite robot,
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 4c807a62101d83fc89afc24d7c9dfb05b7f68c6a..0757f090ac7dea096e94755b79517c92d2e22640 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -151,7 +151,7 @@ namespace hpp {
bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
{
ConstraintSetPtr_t constraints = pathConstraint ();
- constraints->offsetFromConfig(q1);
+ constraints->configProjector ()->rightHandSideFromConfig(q1);
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
@@ -165,22 +165,24 @@ namespace hpp {
return applyConstraints (*(nnear->configuration ()), q);
}
- bool Edge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
+ bool Edge::applyConstraints (ConfigurationIn_t qoffset,
+ ConfigurationOut_t q) const
{
ConstraintSetPtr_t c = configConstraint ();
- c->offsetFromConfig (qoffset);
ConfigProjectorPtr_t proj = c->configProjector ();
+ proj->rightHandSideFromConfig (qoffset);
if (c->apply (q)) {
return true;
}
- if (proj) {
- ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
- if (ss.nbFailure () > ss.nbSuccess ()) {
- hppDout (warning, c->name () << " fails often." << std::endl << ss);
- } else {
- hppDout (warning, c->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
- }
- }
+ assert (proj);
+ ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
+ if (ss.nbFailure () > ss.nbSuccess ()) {
+ hppDout (warning, c->name () << " fails often." << std::endl << ss);
+ } else {
+ hppDout (warning, c->name () << " succeeds at rate "
+ << (double)(ss.nbSuccess ()) / ss.numberOfObservations ()
+ << ".");
+ }
return false;
}
@@ -202,9 +204,9 @@ namespace hpp {
{
assert (waypoint_.first);
core::PathPtr_t pathToWaypoint;
- // TO DO: Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
+ // Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
// already satisfies the constraints.
- config_ = q2;
+ if (!result_.isApprox (q2)) config_ = q2;
if (!waypoint_.first->applyConstraints (q1, config_))
return false;
if (!waypoint_.first->build (pathToWaypoint, q1, config_, d))
@@ -228,10 +230,12 @@ namespace hpp {
bool WaypointEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
{
assert (waypoint_.first);
- if (!waypoint_.first->applyConstraints (qoffset, q))
- return false;
config_ = q;
- return Edge::applyConstraints (config_, q);
+ if (!waypoint_.first->applyConstraints (qoffset, config_))
+ return false;
+ bool success = Edge::applyConstraints (config_, q);
+ result_ = q;
+ return success;
}
void WaypointEdge::createWaypoint (const unsigned d, const std::string& bname)
@@ -247,15 +251,16 @@ namespace hpp {
edge->isInNodeFrom (isInNodeFrom ());
} else {
WaypointEdgePtr_t we = WaypointEdge::create (graph_, from (), node);
- edge->isInNodeFrom (isInNodeFrom ());
we->createWaypoint (d-1, bname);
edge = we;
+ edge->isInNodeFrom (isInNodeFrom ());
}
ss.str (std::string ()); ss.clear ();
ss << bname << "_e" << d;
edge->name (ss.str ());
waypoint_ = Waypoint (edge, node);
config_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
+ result_ = Configuration_t(graph_.lock ()->robot ()->configSize ());
}
NodePtr_t WaypointEdge::node () const
@@ -322,47 +327,48 @@ namespace hpp {
bool LevelSetEdge::applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const
{
+#if 0
// First, get an offset from the histogram that is not in the same connected component.
statistics::DiscreteDistribution < core::NodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
const Configuration_t& levelsetTarget = *(distrib ()->configuration ()),
q_offset = *(n_offset->configuration ());
// Then, set the offset.
ConstraintSetPtr_t cs = extraConfigConstraint ();
- cs->offsetFromConfig (q_offset);
-
const ConfigProjectorPtr_t cp = cs->configProjector ();
- if (cp) {
- vector_t offset = cp->offsetFromConfig (q_offset);
- size_t row = 0, nbRows = 0;
- for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
- it != extraNumericalFunctions_.end (); ++it) {
- const core::DifferentiableFunction& f = *(it->first);
- nbRows = f.outputSize ();
- vector_t value = vector_t::Zero (nbRows);
- if (f.isParametric ()) {
- f (value, levelsetTarget);
- }
- offset.segment (row, nbRows) = value;
- row += nbRows;
- }
- cp->offset (offset);
- }
- for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
- it != extraLockedDofs_.end (); ++it) {
- (*it)->offsetFromConfig (levelsetTarget);
+ assert (cp);
+ vector_t offset = cp->rightHandSideFromConfig (q_offset);
+ size_t row = 0, nbRows = 0;
+ for (DifferentiableFunctions_t::const_iterator it =
+ extraNumericalFunctions_.begin ();
+ it != extraNumericalFunctions_.end (); ++it) {
+ const core::DifferentiableFunction& f = *(it->first);
+ nbRows = f.outputSize ();
+ vector_t value = vector_t::Zero (nbRows);
+ // TODO: fix this function
+ if (f.isParametric ()) {
+ f (value, levelsetTarget);
+ }
+ offset.segment (row, nbRows) = value;
+ row += nbRows;
+ }
+ cp->rightHandSide (offset);
+ for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
+ it != extraLockedDofs_.end (); ++it) {
+ (*it)->valueFromFromConfig (levelsetTarget);
}
// Eventually, do the projection.
if (cs->apply (q))
return true;
- if (cp) {
- ::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
- if (ss.nbFailure () > ss.nbSuccess ()) {
- hppDout (warning, cs->name () << " fails often." << std::endl << ss);
- } else {
- hppDout (warning, cs->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
- }
- }
+ ::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
+ if (ss.nbFailure () > ss.nbSuccess ()) {
+ hppDout (warning, cs->name () << " fails often." << std::endl << ss);
+ } else {
+ hppDout (warning, cs->name () << " succeeds at rate "
+ << (double)(ss.nbSuccess ()) / ss.numberOfObservations ()
+ << ".");
+ }
+#endif
return false;
}
@@ -392,12 +398,12 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
it != extraNumericalFunctions_.end (); ++it) {
- proj->addConstraint (it->first);
+ proj->addFunction (it->first);
}
constraint->addConstraint (proj);
}
- for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
+ for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
it != extraLockedDofs_.end (); ++it)
constraint->addConstraint (*it);
@@ -422,7 +428,7 @@ namespace hpp {
bool hasDiffFunc = g->insertNumericalConstraints (proj);
for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
it != extraNumericalFunctions_.end (); ++it) {
- proj->addConstraint (it->first, it->second);
+ proj->addFunction (it->first, it->second);
}
hasDiffFunc = !extraNumericalFunctions_.empty () || hasDiffFunc;
hasDiffFunc = insertNumericalConstraints (proj) || hasDiffFunc;
@@ -431,7 +437,7 @@ namespace hpp {
constraint->addConstraint (proj);
g->insertLockedDofs (constraint);
- for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
+ for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
it != extraLockedDofs_.end (); ++it) {
constraint->addConstraint (*it);
}
@@ -442,12 +448,12 @@ namespace hpp {
return extraConstraints_->get ();
}
- void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const EquationTypePtr_t ineq)
+ void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
{
extraNumericalFunctions_.push_back (DiffFuncAndIneqPair_t (function, ineq));
}
- void LevelSetEdge::insertConfigConstraint (const LockedDofPtr_t lockedDof)
+ void LevelSetEdge::insertConfigConstraint (const LockedJointPtr_t lockedDof)
{
extraLockedDofs_.push_back (lockedDof);
}
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index acc48779274cd18ba206ccdc206a04481ece386d..f9085f7131e1df511e9393e10afdac64833da746 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -18,7 +18,7 @@
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
-#include <hpp/core/locked-dof.hh>
+#include <hpp/core/locked-joint.hh>
namespace hpp {
namespace manipulation {
@@ -66,12 +66,13 @@ namespace hpp {
assert (false);
}
- void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const EquationTypePtr_t& ineq)
+ void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
{
numericalConstraints_.push_back(DiffFuncAndIneqPair_t(function,ineq));
}
- void GraphComponent::addLockedDofConstraint (const LockedDofPtr_t& constraint)
+ void GraphComponent::addLockedJointConstraint
+ (const LockedJointPtr_t& constraint)
{
lockedDofConstraints_.push_back (constraint);
}
@@ -80,13 +81,13 @@ namespace hpp {
{
for (DifferentiableFunctions_t::const_iterator it = numericalConstraints_.begin();
it != numericalConstraints_.end(); ++it)
- proj->addConstraint (it->first, it->second);
+ proj->addFunction (it->first, it->second);
return !numericalConstraints_.empty ();
}
bool GraphComponent::insertLockedDofs (ConstraintSetPtr_t cs) const
{
- for (LockedDofs_t::const_iterator it = lockedDofConstraints_.begin();
+ for (LockedJoints_t::const_iterator it = lockedDofConstraints_.begin();
it != lockedDofConstraints_.end(); ++it)
cs->addConstraint (*it);
return !lockedDofConstraints_.empty ();
@@ -97,7 +98,7 @@ namespace hpp {
return numericalConstraints_;
}
- const LockedDofs_t& GraphComponent::lockedDofConstraints () const
+ const LockedJoints_t& GraphComponent::lockedDofConstraints () const
{
return lockedDofConstraints_;
}
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 88568ded71ddccc30561d206f4f0f755341bd5b4..841381a1eb45d98402edb10dddff879463d666cc 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -153,7 +153,14 @@ namespace hpp {
void LeafHistogram::add (const core::NodePtr_t& n)
{
- iterator it = insert (LeafBin (constraint_->offsetFromConfig (*n->configuration ())));
+ iterator it;
+ if (constraint_->configProjector ()) {
+ it = insert
+ (LeafBin (constraint_->configProjector ()->rightHandSideFromConfig
+ (*n->configuration ())));
+ } else {
+ it = insert (LeafBin (vector_t (0)));
+ }
it->push_back (n);
if (numberOfObservations()%10 == 0) {
hppDout (info, *this);
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 721f0d138ec4efd6a23384a6847f4c871672bb25..12f1f1547756fcb08c65f768e5f03238138dd823 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -106,6 +106,9 @@ namespace hpp {
// Select next node in the constraint graph.
const ConfigurationPtr_t q_near = n_near->configuration ();
graph::NodePtr_t node = graph->getNode (*q_near);
+ if (node->neighbors ().totalWeight () == 0) {
+ return false;
+ }
graph::EdgePtr_t edge = graph->chooseEdge (node);
qProj_ = *q_rand;
if (!edge->applyConstraints (n_near, qProj_)) {
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 3df761215c5f21dafc11c21972535d85bfe3f5fe..b0576ad20831a1a8bfb006ee3e3ae3997720ed16 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -107,12 +107,12 @@ namespace hpp {
return constraintGraph_;
}
- LockedDofPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
+ LockedJointPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
{
LockedDofConstraintMap_t::const_iterator it =
lockedDofConstraintMap_.find (name);
if (it == lockedDofConstraintMap_.end ()) {
- throw std::runtime_error ("No LockedDof constraint with this name");
+ throw std::runtime_error ("No LockedJoint constraint with this name");
}
return it->second;
}
@@ -150,15 +150,16 @@ namespace hpp {
}
}
- void ProblemSolver::addConstraintToConfigProjector (
- const std::string& constraintName,
- const DifferentiableFunctionPtr_t& constraint)
+ void ProblemSolver::addFunctionToConfigProjector
+ (const std::string& constraintName, const std::string& functionName)
{
- core::ProblemSolver::addConstraintToConfigProjector(constraintName,
- constraint);
- if ( grasp(constraint) ) {
- GripperPtr_t gripper = grasp(constraint)->first;
- HandlePtr_t handle = grasp(constraint)->second;
+ core::ProblemSolver::addFunctionToConfigProjector (constraintName,
+ functionName);
+ DifferentiableFunctionPtr_t constraint =
+ numericalConstraint (functionName);
+ if (GraspPtr_t g = grasp (constraint)) {
+ GripperPtr_t gripper = g->first;
+ HandlePtr_t handle = g->second;
JointPtr_t joint1 = handle->joint();
model::JointVector_t joints = gripper->getDisabledCollisions();
for (model::JointVector_t::iterator itJoint = joints.begin() ;
diff --git a/src/robot.cc b/src/robot.cc
index ffc8484fb37f930cfba2d8df41d17ba177ea802d..923823b401be10bcc5fdb3f9b8fe0f4d32e9257c 100644
--- a/src/robot.cc
+++ b/src/robot.cc
@@ -193,8 +193,9 @@ namespace hpp {
// Put an anchor joint as root joint
model::ObjectFactory factory;
Transform3f pos; pos.setIdentity ();
- rootJoint (factory.createJointAnchor (pos));
- rootJoint ()->name (name () + "/root");
+ JointPtr_t root = factory.createJointAnchor (pos);
+ root->name (name () + "/root");
+ rootJoint (root);
size_type rankInConfiguration = 0;
size_type rankInVelocity = 0;
// Copy robot kinematic chains