diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index b4da2987a13f8ab654b66a460024cfa79061af18..e26850b84c8a04efc562a4eed032e8d913a511fb 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -89,6 +89,8 @@ namespace hpp {
const IntervalsContainer_t& passiveDofs() const;
/// Get a reference to the LockedJoints_t
+ /// \deprecated LockedJoint are handled as classical explicit
+ /// constraint
const LockedJoints_t& lockedJoints () const
HPP_MANIPULATION_DEPRECATED;
@@ -117,7 +119,7 @@ namespace hpp {
NumericalConstraints_t numericalConstraints_;
/// Stores the passive dofs for each numerical constraints.
IntervalsContainer_t passiveDofs_;
- /// List of LockedJoint constraints: to be removed
+ /// List of LockedJoint constraints: \todo to be removed
const LockedJoints_t lockedJoints_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
diff --git a/include/hpp/manipulation/graph/state-selector.hh b/include/hpp/manipulation/graph/state-selector.hh
index d4b00890ce82b3e419889107a4bfabcf7a3ce3d9..e726ed33b61db968bf12dd0e21087ab343de9d28 100644
--- a/include/hpp/manipulation/graph/state-selector.hh
+++ b/include/hpp/manipulation/graph/state-selector.hh
@@ -27,7 +27,7 @@ namespace hpp {
namespace graph {
/// This class is used to get the state of a configuration. States have to
/// be ordered because a configuration can be in several states.
- class HPP_MANIPULATION_DLLAPI StateSelector : public GraphComponent
+ class HPP_MANIPULATION_DLLAPI StateSelector
{
public:
virtual ~StateSelector () {};
@@ -35,6 +35,11 @@ namespace hpp {
/// Create a new StateSelector.
static StateSelectorPtr_t create(const std::string& name);
+ const std::string& name() const
+ {
+ return name_;
+ }
+
/// Create an empty state
StatePtr_t createState (const std::string& name, bool waypoint = false,
const int w = 0);
@@ -54,29 +59,21 @@ namespace hpp {
/// Select randomly an outgoing edge of the given state.
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
- /// Should never be called.
- void addNumericalConstraint (const constraints::ImplicitPtr_t& /* function */,
- const segments_t& /* passiveDofs */ = segments_t ())
- {
- throw std::logic_error ("StateSelector component does not have constraints.");
- }
-
- /// Should never be called.
- void addLockedJointConstraint
- (const constraints::LockedJoint& /* constraint */)
- {
- throw std::logic_error ("StateSelector component does not have constraints.");
- }
-
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+ /// Set the parent graph.
+ void parentGraph(const GraphWkPtr_t& parent);
+
+ /// Set the parent graph.
+ GraphPtr_t parentGraph() const;
+
protected:
/// Initialization of the object.
void init (const StateSelectorPtr_t& weak);
/// Constructor
- StateSelector (const std::string& name) : GraphComponent (name)
+ StateSelector (const std::string& name) : name_ (name)
{}
/// Print the object in a stream.
@@ -88,12 +85,21 @@ namespace hpp {
WeighedStates_t orderedStates_;
States_t waypoints_;
- virtual void initialize () { isInit_ = true; };
-
private:
+ /// Name of the component.
+ std::string name_;
+ /// A weak pointer to the parent graph.
+ GraphWkPtr_t graph_;
/// Weak pointer to itself.
StateSelectorPtr_t wkPtr_;
+
+ friend std::ostream& operator<< (std::ostream& os, const StateSelector& ss);
}; // Class StateSelector
+
+ inline std::ostream& operator<< (std::ostream& os, const StateSelector& ss)
+ {
+ return ss.print(os);
+ }
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/problem.hh b/include/hpp/manipulation/problem.hh
index b827071c65c56b911ce476ae5c965ab5cb4e3691..ab624ac1842e9e467a19fce33714d82cf236e443 100644
--- a/include/hpp/manipulation/problem.hh
+++ b/include/hpp/manipulation/problem.hh
@@ -57,12 +57,7 @@ namespace hpp {
void pathValidation (const PathValidationPtr_t& pathValidation);
/// Get the steering method as a SteeringMethod
- SteeringMethodPtr_t steeringMethod () const;
-
- void steeringMethod (core::SteeringMethodPtr_t sm)
- {
- Parent::steeringMethod (sm);
- }
+ SteeringMethodPtr_t manipulationSteeringMethod () const;
/// Build a new path validation
/// \note Current obstacles are added to the created object.
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index e2acc46dd70cde95011c3871a286746a0efb0a63..ffda441963529454c9dead496a8906796400865e 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -279,7 +279,7 @@ namespace hpp {
// Build steering method
const ProblemPtr_t& problem (g->problem());
- steeringMethod_ = problem->steeringMethod()->innerSteeringMethod()->copy();
+ steeringMethod_ = problem->manipulationSteeringMethod()->innerSteeringMethod()->copy();
steeringMethod_->constraints (constraint);
// Build path validation and relative motion matrix
// TODO this path validation will not contain obstacles added after
diff --git a/src/graph/guided-state-selector.cc b/src/graph/guided-state-selector.cc
index 78bad90323861ab17efe14380304bc8c6de3294f..334504c95108c16a7ce974c880026344c96bfae7 100644
--- a/src/graph/guided-state-selector.cc
+++ b/src/graph/guided-state-selector.cc
@@ -138,12 +138,7 @@ namespace hpp {
std::ostream& GuidedStateSelector::print (std::ostream& os) const
{
- os << "|-- ";
- GraphComponent::print (os) << std::endl;
- for (WeighedStates_t::const_iterator it = orderedStates_.begin();
- orderedStates_.end() != it; ++it)
- os << it->first << " " << *it->second;
- return os;
+ return StateSelector::print (os);
}
} // namespace graph
} // namespace manipulation
diff --git a/src/graph/state-selector.cc b/src/graph/state-selector.cc
index 657199de108eb64c1ad9ce5f8557c4b7174ead94..6c0e3239cfafdfe0c98437cce580a514713d3ddf 100644
--- a/src/graph/state-selector.cc
+++ b/src/graph/state-selector.cc
@@ -35,7 +35,6 @@ namespace hpp {
void StateSelector::init (const StateSelectorPtr_t& weak)
{
- GraphComponent::init (weak);
wkPtr_ = weak;
}
@@ -129,13 +128,24 @@ namespace hpp {
std::ostream& StateSelector::print (std::ostream& os) const
{
- os << "|-- ";
- GraphComponent::print (os) << std::endl;
for (WeighedStates_t::const_iterator it = orderedStates_.begin();
orderedStates_.end() != it; ++it)
os << it->first << " " << *it->second;
return os;
}
+
+ GraphPtr_t StateSelector::parentGraph() const
+ {
+ return graph_.lock ();
+ }
+
+ void StateSelector::parentGraph(const GraphWkPtr_t& parent)
+ {
+ graph_ = parent;
+ GraphPtr_t g = graph_.lock();
+ assert(g);
+ }
+
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph/validation.cc b/src/graph/validation.cc
index 3cad101963db3a2583a8ec6a1b3c5ed72953b588..562e307ac0042b4116710a611bb276ecd13bbaee 100644
--- a/src/graph/validation.cc
+++ b/src/graph/validation.cc
@@ -22,6 +22,8 @@
#include <hpp/util/indent.hh>
+#include <hpp/constraints/differentiable-function.hh>
+
#include <hpp/core/collision-validation.hh>
#include <hpp/core/configuration-shooter.hh>
#include <hpp/core/relative-motion.hh>
@@ -35,6 +37,17 @@
namespace hpp {
namespace manipulation {
namespace graph {
+ bool stateAIncludedInStateB (const StatePtr_t& A, const StatePtr_t& B)
+ {
+ const NumericalConstraints_t& Ancs = A->numericalConstraints();
+ const NumericalConstraints_t& Bncs = B->numericalConstraints();
+ for (NumericalConstraints_t::const_iterator _nc = Ancs.begin();
+ _nc != Ancs.end(); ++_nc)
+ if (std::find (Bncs.begin(), Bncs.end(), *_nc) == Bncs.end())
+ return false;
+ return true;
+ }
+
std::ostream& Validation::print (std::ostream& os) const
{
for (std::size_t i = 0; i < warnings_.size(); ++i) {
@@ -64,11 +77,21 @@ namespace hpp {
bool Validation::validateState (const StatePtr_t& state)
{
- std::ostringstream oss;
- oss << incindent;
bool success = true;
- // 1. try to generate a configuration in this state.
+ // 1. Check that all the constraint are not parameterizable.
+ const NumericalConstraints_t& ncs = state->numericalConstraints();
+ for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
+ _nc != ncs.end(); ++_nc)
+ if ((*_nc)->parameterSize() > 0) {
+ std::ostringstream oss;
+ oss << incindent << "Constraint " << (*_nc)->function().name() <<
+ " has a varying right hand side while constraints of a state must"
+ " have a constant one.";
+ addError (state, oss.str());
+ }
+
+ // 2. try to generate a configuration in this state.
bool projOk;
Configuration_t q;
std::size_t i, Nrand = 100;
@@ -79,17 +102,21 @@ namespace hpp {
if (projOk) break;
}
if (!projOk) {
- oss << "Failed to apply the constraints to " << Nrand << "random configurations.";
+ std::ostringstream oss;
+ oss << incindent << "Failed to apply the constraints to " << Nrand <<
+ "random configurations.";
addError (state, oss.str());
return false;
}
if (4 * i > 3 * Nrand) {
- oss << "Success rate of constraint projection is " << i / Nrand << '.';
+ std::ostringstream oss;
+ oss << incindent << "Success rate of constraint projection is " <<
+ i / Nrand << '.';
addWarning (state, oss.str());
oss.clear();
}
- // 2. check the collision pairs which will be disabled because of the
+ // 3. check the collision pairs which will be disabled because of the
// constraint.
core::CollisionValidationPtr_t colValidation (
core::CollisionValidation::create (problem_->robot()));
@@ -128,7 +155,9 @@ namespace hpp {
bool colOk = colValidation->validate (q, colReport);
if (!colOk) {
- oss << "The following collision pairs will always collide." << incendl << *colReport << decindent;
+ std::ostringstream oss;
+ oss << incindent << "The following collision pairs will always "
+ "collide." << incendl << *colReport << decindent;
addError (state, oss.str());
success = false;
}
@@ -146,10 +175,25 @@ namespace hpp {
if (!graph) return false;
bool success = true;
- States_t states = graph->stateSelector()->getStates();
for (std::size_t i = 1; i < graph->nbComponents(); ++i)
if (!validate(graph->get(i).lock())) success = false;
+ // Check that no state is included in a state which has a higher priority.
+ States_t states = graph->stateSelector()->getStates();
+ for (States_t::const_iterator _state = states.begin();
+ _state != states.end(); ++_state) {
+ for (States_t::const_iterator _stateHO = states.begin();
+ _stateHO != _state; ++_stateHO) {
+ if (stateAIncludedInStateB (*_state, *_stateHO)) {
+ std::ostringstream oss;
+ oss << "State " << (*_state)->name() << " is included in state "
+ << (*_stateHO)->name() << " but the latter has a higher priority.";
+ addError (*_state, oss.str());
+ success = false;
+ }
+ }
+ }
+
return success;
}
} // namespace graph
diff --git a/src/problem.cc b/src/problem.cc
index 8d98825315e5983c93a61bb9a2647153529c6a56..002e0b0a468ec00acb51612d95b98279f0a6d097 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -84,7 +84,7 @@ namespace hpp {
return pv;
}
- SteeringMethodPtr_t Problem::steeringMethod () const
+ SteeringMethodPtr_t Problem::manipulationSteeringMethod () const
{
return HPP_DYNAMIC_PTR_CAST (SteeringMethod,
Parent::steeringMethod());