diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 66c5ab1c62597db975eee4e9d0e6579d18363a24..5cd036895f03411c998e6f6dd7f5ef2928c8da47 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -51,7 +51,7 @@ namespace hpp {
/// \return the constraint of relative transformation between the handle and
/// the gripper. The rotation around x is not constrained.
virtual NumericalConstraintPtr_t createGrasp
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createGrasp.
@@ -59,7 +59,7 @@ namespace hpp {
/// \return a relative orientation constraint between the handle and
/// the gripper. Only the rotation around the x-axis is constrained.
virtual NumericalConstraintPtr_t createGraspComplement
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint corresponding to a pregrasping task.
/// \param gripper object containing the gripper information
@@ -69,7 +69,7 @@ namespace hpp {
/// \todo this function is never called. It should follow changes of
/// Handle::createPreGrasp prototype.
virtual NumericalConstraintPtr_t createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const;
+ (const GripperPtr_t& gripper, const value_type& shift, std::string name) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createPreGrasp.
@@ -81,7 +81,7 @@ namespace hpp {
/// \note The translation along x-axis and the rotation around z-axis are constrained.
virtual NumericalConstraintPtr_t createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift,
- const value_type& width) const;
+ const value_type& width, std::string name) const;
virtual std::ostream& print (std::ostream& os) const;
protected:
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 3bc140fdd2f65d657bec1bdcbc8b05dc84cb468a..1c6be4ed7bfd3531cca864ca71c93ff117af9f2d 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -29,32 +29,6 @@
namespace hpp {
namespace manipulation {
namespace graph {
- /// \cond
- /// Cache mechanism that enable const-correctness of member functions.
- template <typename C>
- class HPP_MANIPULATION_LOCAL Cache
- {
- public:
- void set (const C& c)
- {
- c_ = c;
- }
-
- operator bool() const
- {
- return (bool)c_;
- }
-
- const C& get () const
- {
- return c_;
- }
-
- private:
- C c_;
- };
- /// \endcond
-
/// \addtogroup constraint_graph
/// \{
@@ -141,13 +115,13 @@ namespace hpp {
/// Get steering method associated to the edge.
const core::SteeringMethodPtr_t& steeringMethod () const
{
- return steeringMethod_->get();
+ return steeringMethod_;
}
/// Get path validation associated to the edge.
const core::PathValidationPtr_t& pathValidation () const
{
- return pathValidation_->get();
+ return pathValidation_;
}
const RelativeMotion::matrix_type& relativeMotion () const
@@ -190,9 +164,11 @@ namespace hpp {
/// \return The initialized constraint.
ConstraintSetPtr_t pathConstraint() const;
- virtual ConstraintSetPtr_t buildConfigConstraint() const;
+ virtual ConstraintSetPtr_t buildConfigConstraint();
+
+ virtual ConstraintSetPtr_t buildPathConstraint();
- virtual ConstraintSetPtr_t buildPathConstraint() const;
+ virtual void initialize ();
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
@@ -200,16 +176,12 @@ namespace hpp {
bool isShort_;
private:
- typedef Cache < ConstraintSetPtr_t > Constraint_t;
- typedef Cache < core::SteeringMethodPtr_t > SteeringMethod_t;
- typedef Cache < core::PathValidationPtr_t > PathValidation_t;
-
/// See pathConstraint member function.
- Constraint_t* pathConstraints_;
+ ConstraintSetPtr_t pathConstraints_;
/// Constraint ensuring that a q_proj will be in to_ and in the
/// same leaf of to_ as the configuration used for initialization.
- Constraint_t* configConstraints_;
+ ConstraintSetPtr_t configConstraints_;
/// The two ends of the transition.
StateWkPtr_t from_, to_;
@@ -218,11 +190,11 @@ namespace hpp {
StateWkPtr_t state_;
/// Steering method used to create paths associated to the edge
- SteeringMethod_t* steeringMethod_;
+ core::SteeringMethodPtr_t steeringMethod_;
/// Path validation associated to the edge
mutable RelativeMotion::matrix_type relMotion_;
- PathValidation_t* pathValidation_;
+ core::PathValidationPtr_t pathValidation_;
/// Weak pointer to itself.
EdgeWkPtr_t wkPtr_;
@@ -244,18 +216,13 @@ namespace hpp {
/// \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} N_t\f$.
- /// The outmost WaypointEdg contains:
+ /// \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$,
- /// \li constraints: those of edge \f$E\f$,
- /// \li waypoint: \f$(E_1, n_1)\f$.
- ///
- /// where \f$E_1\f$ is an instance of class WaypointEdge containing:
- /// \li from: \f$N_f\f$,
- /// \li to: \f$n_1\f$,
- /// \li constraints: those of edge \f$e_1\f$,
- /// \li waypoint: \f$(e_0, n_0)\f$.
+ /// \li states: \f$(n_0, n_1)\f$
+ /// \li transitions: \f$(e_0, e_1, e_2)\f$
+ /// \li constraints: any calls to the constraints throw,
class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge
{
public:
@@ -265,18 +232,14 @@ namespace hpp {
const GraphWkPtr_t& graph, const StateWkPtr_t& from,
const StateWkPtr_t& to);
- virtual bool direction (const core::PathPtr_t& path) 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;
virtual bool applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const;
- /// Return the inner waypoint.
- /// \param EdgeType is either Edge or WaypointEdge
- template <class EdgeType>
- boost::shared_ptr <EdgeType> waypoint (const std::size_t index) const;
+ /// Return the index-th edge.
+ const EdgePtr_t& waypoint (const std::size_t index) const;
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -286,7 +249,7 @@ namespace hpp {
std::size_t nbWaypoints () const
{
- return waypoints_.size ();
+ return edges_.size () - 1;
}
/// Set waypoint index with wEdge and wTo.
@@ -295,7 +258,8 @@ namespace hpp {
protected:
WaypointEdge (const std::string& name) :
- Edge (name)
+ Edge (name),
+ lastSucceeded_ (false)
{
}
/// Initialization of the object.
@@ -306,14 +270,11 @@ namespace hpp {
virtual std::ostream& print (std::ostream& os) const;
private:
- typedef std::pair < EdgePtr_t, StatePtr_t > Waypoint_t;
- typedef std::vector <Waypoint_t> Waypoints_t;
+ Edges_t edges_;
+ States_t states_;
- Waypoints_t waypoints_;
-
- mutable Configuration_t init_;
mutable matrix_t configs_;
- mutable Configuration_t result_;
+ mutable bool lastSucceeded_;
WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
@@ -334,7 +295,7 @@ namespace hpp {
virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
- virtual ConstraintSetPtr_t buildConfigConstraint() const;
+ virtual ConstraintSetPtr_t buildConfigConstraint();
void buildHistogram ();
@@ -383,6 +344,8 @@ namespace hpp {
/// Populate DrawingAttributes tooltip
virtual void populateTooltip (dot::Tooltip& tp) const;
+ virtual void initialize ();
+
private:
bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index b3425dbab7c6a6c8454910b26ce4c0c51a3193e5..b5479a568fe32cc6948765e3f1330ddaef4772a8 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -41,6 +41,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI GraphComponent
{
public:
+ virtual ~GraphComponent () {};
+
/// Get the component name.
const std::string& name() const;
@@ -102,7 +104,9 @@ namespace hpp {
/// Initialize the component
void init (const GraphComponentWkPtr_t& weak);
- GraphComponent(const std::string& name) : name_ (name), id_(-1)
+ GraphComponent(const std::string& name) : isInit_(false)
+ , name_ (name)
+ , id_(-1)
{}
/// Stores the numerical constraints.
@@ -114,6 +118,13 @@ namespace hpp {
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
+ bool isInit_;
+
+ void throwIfNotInitialized () const
+ {
+ if (!isInit_) throw std::logic_error ("The graph should have been initialized first.");
+ }
+
/// Print the object in a stream.
virtual std::ostream& print (std::ostream& os) const;
friend std::ostream& operator<< (std::ostream&, const GraphComponent&);
@@ -121,6 +132,8 @@ namespace hpp {
/// Populate DrawingAttributes tooltip
virtual void populateTooltip (dot::Tooltip& tp) const;
+ virtual void initialize () = 0;
+
private:
/// Name of the component.
std::string name_;
@@ -128,6 +141,8 @@ namespace hpp {
GraphComponentWkPtr_t wkPtr_;
/// ID of the component (index in components vector).
std::size_t id_;
+
+ friend class Graph;
};
std::ostream& operator<< (std::ostream& os, const GraphComponent& graphComp);
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index b6827f905183818c5282a95e8584bb36f29ea03a..56ba1109eeb24aae8c129f74320ae34e18783aa5 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -107,12 +107,12 @@ namespace hpp {
/// Constraint to project onto the Node.
/// \param state the state on which to project.
/// \return The initialized projector.
- ConstraintSetPtr_t configConstraint (const StatePtr_t& state);
+ 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);
+ ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const;
/// Get error of a config with respect to a state constraint
///
@@ -125,7 +125,7 @@ namespace hpp {
/// constraints.
/// \deprecated use getConfigErrorForState instead
bool getConfigErrorForNode (ConfigurationIn_t config,
- const StatePtr_t& state, vector_t& error)
+ const StatePtr_t& state, vector_t& error) const
HPP_MANIPULATION_DEPRECATED;
/// Get error of a config with respect to a state constraint
@@ -138,7 +138,7 @@ namespace hpp {
/// Call method core::ConstraintSet::isSatisfied for the state
/// constraints.
bool getConfigErrorForState (ConfigurationIn_t config,
- const StatePtr_t& state, vector_t& error);
+ const StatePtr_t& state, vector_t& error) const;
/// Get error of a config with respect to an edge constraint
///
@@ -152,7 +152,7 @@ namespace hpp {
/// input configuration and method core::ConstraintSet::isSatisfied
/// for the edge constraint.
bool getConfigErrorForEdge (ConfigurationIn_t config,
- const EdgePtr_t& edge, vector_t& error);
+ const EdgePtr_t& edge, vector_t& error) const;
/// Get error of a config with respect to an edge foliation leaf
///
@@ -166,12 +166,12 @@ namespace hpp {
/// on the edge constraints.
bool getConfigErrorForEdgeLeaf
(ConfigurationIn_t leafConfig, ConfigurationIn_t config,
- const EdgePtr_t& edge, vector_t& error);
+ const EdgePtr_t& edge, vector_t& error) const;
/// Constraint to project a path.
/// \param edge a list of edges defining the foliation.
/// \return The constraint.
- ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge);
+ ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge) const;
/// Set maximal number of iterations
void maxIterations (size_type iterations);
@@ -214,6 +214,8 @@ namespace hpp {
/// Print the component in DOT language.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
+ virtual void initialize ();
+
protected:
/// Initialization of the object.
void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index 4245ce29e0d27ba5b343c26521162de74a3acb44..ce733e56f1e543b24518cea6a8c20f031f15fe8a 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -185,6 +185,7 @@ namespace hpp {
///
/// \param[in,out] graph must be an initialized empty Graph.
void graphBuilder (
+ const ProblemSolverPtr_t& ps,
const Objects_t& objects,
const Grippers_t& grippers,
GraphPtr_t graph,
diff --git a/include/hpp/manipulation/graph/state-selector.hh b/include/hpp/manipulation/graph/state-selector.hh
index 995df7e065295313a115cf249807c7085e25d04f..d5a88d53f550c4d2101ba2d76584885c37fa3fea 100644
--- a/include/hpp/manipulation/graph/state-selector.hh
+++ b/include/hpp/manipulation/graph/state-selector.hh
@@ -30,6 +30,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI StateSelector : public GraphComponent
{
public:
+ virtual ~StateSelector () {};
+
/// Create a new StateSelector.
static StateSelectorPtr_t create(const std::string& name);
@@ -84,6 +86,8 @@ namespace hpp {
WeighedStates_t orderedStates_;
States_t waypoints_;
+ virtual void initialize () { isInit_ = true; };
+
private:
/// Weak pointer to itself.
StateSelectorPtr_t wkPtr_;
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 2a0c6fb2e12ce87bf5792d292ac700e229d95447..66b54fcfd1d381922bdb252b699b7d471b41943a 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -104,12 +104,17 @@ namespace hpp {
Weight_t getWeight (const EdgePtr_t&edge);
/// Constraint to project onto this state.
- ConstraintSetPtr_t configConstraint() const;
+ ConstraintSetPtr_t configConstraint() const
+ {
+ throwIfNotInitialized ();
+ return configConstraints_;
+ }
/// Add core::NumericalConstraint to the component.
virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
const segments_t& passiveDofs = segments_t ())
{
+ isInit_ = false;
numericalConstraintsForPath_.push_back (nm);
passiveDofsForPath_.push_back (passiveDofs);
}
@@ -118,6 +123,7 @@ namespace hpp {
virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
HPP_MANIPULATION_DEPRECATED
{
+ isInit_ = false;
numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
}
@@ -156,6 +162,8 @@ namespace hpp {
virtual void populateTooltip (dot::Tooltip& tp) const;
+ virtual void initialize ();
+
private:
/// List of possible motions from this state (i.e. the outgoing
/// vertices).
@@ -163,8 +171,7 @@ namespace hpp {
std::vector <EdgePtr_t> hiddenNeighbors_;
/// Set of constraints to be statisfied.
- typedef Cache < ConstraintSetPtr_t > Constraint_t;
- Constraint_t* configConstraints_;
+ ConstraintSetPtr_t configConstraints_;
/// Stores the numerical constraints for path.
NumericalConstraints_t numericalConstraintsForPath_;
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index b58156f18591a1a807ef92f665da85c7e23b3291..6c3a8ba781f4abadb7604805459caf27e7623c4b 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -95,6 +95,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLLOCAL Histogram
{
public:
+ virtual ~Histogram () {};
+
virtual void add (const RoadmapNodePtr_t& node) = 0;
virtual HistogramPtr_t clone () const = 0;
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 7575518c7f6b5cd0fb0a749db86f442c506b27f7..0670cd8178ee88e1b39f14c64b708037350661f3 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -31,6 +31,8 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Handle
{
public:
+ virtual ~Handle () {};
+
/// Create constraint corresponding to a gripper grasping this object
/// \param robot the robot that grasps the handle,
/// \param grasp object containing the grasp information
@@ -98,14 +100,14 @@ namespace hpp {
/// the gripper.
/// \note The 6 DOFs of the relative transformation are constrained.
virtual NumericalConstraintPtr_t createGrasp
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint that acts on the non-constrained axis of the
/// constraint generated by Handle::createGrasp.
/// \param gripper object containing the gripper information
/// \return a constraints that is not doing anything.
virtual NumericalConstraintPtr_t createGraspComplement
- (const GripperPtr_t& gripper) const;
+ (const GripperPtr_t& gripper, std::string name) const;
/// Create constraint corresponding to a pregrasping task.
/// \param gripper object containing the gripper information
@@ -115,12 +117,12 @@ namespace hpp {
/// are constrained. The transformation is shifted along x-axis of
/// value shift.
virtual NumericalConstraintPtr_t createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const;
+ (const GripperPtr_t& gripper, const value_type& shift, std::string name) const;
static NumericalConstraintPtr_t createGrasp
- (const GripperPtr_t& gripper,const HandlePtr_t& handle)
+ (const GripperPtr_t& gripper,const HandlePtr_t& handle, std::string name)
{
- return handle->createGrasp(gripper);
+ return handle->createGrasp(gripper, name);
}
/// Get the clearance
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index ecf3fd161427b01cf2c6c1e1ad376a0dac6cbb82..3dddc4ebaea1675bfbb7c6d1d2511859435a66fe 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -67,6 +67,9 @@ namespace hpp {
/// Get the constraint graph
graph::GraphPtr_t constraintGraph () const;
+
+ /// Should be called before any call on the graph is made.
+ void initConstraintGraph ();
/// \}
/// Create placement constraint
@@ -93,6 +96,27 @@ namespace hpp {
const value_type& width,
const value_type& margin = 1e-4);
+ /// Create the grasp constraint and its complement
+ /// \param name name of the grasp constraint,
+ /// \param gripper gripper's name
+ /// \param handle handle's name
+ ///
+ /// Two constraints are created:
+ /// - "name" corresponds to the grasp constraint.
+ /// - "name/complement" corresponds to the complement.
+ void createGraspConstraint (const std::string& name,
+ const std::string& gripper,
+ const std::string& handle);
+
+ /// Create pre-grasp constraint
+ /// \param name name of the grasp constraint,
+ /// \param gripper gripper's name
+ /// \param handle handle's name
+ ///
+ void createPreGraspConstraint (const std::string& name,
+ const std::string& gripper,
+ const std::string& handle);
+
virtual void pathValidationType (const std::string& type,
const value_type& tolerance);
diff --git a/include/hpp/manipulation/roadmap-node.hh b/include/hpp/manipulation/roadmap-node.hh
index a6e728997a99c7978bc54a66244e59c1f02922d0..439677d58b9343d5fd39a278f58c6981e61070e3 100644
--- a/include/hpp/manipulation/roadmap-node.hh
+++ b/include/hpp/manipulation/roadmap-node.hh
@@ -66,13 +66,13 @@ namespace hpp {
/// \deprecated use graphState instead.
graph::StatePtr_t graphNode () const HPP_MANIPULATION_DEPRECATED
{
- return state_;
+ return state_.lock();
}
/// Getter for the graph::State.
graph::StatePtr_t graphState () const
{
- return state_;
+ return state_.lock();
}
/// Setter for the graph::State.
@@ -97,7 +97,7 @@ namespace hpp {
symbolicCC_ = sc;
}
- const SymbolicComponentPtr_t& symbolicComponent () const
+ SymbolicComponentPtr_t symbolicComponent () const
{
return symbolicCC_;
}
@@ -105,7 +105,7 @@ namespace hpp {
private:
CachingSystem cacheSystem_;
- graph::StatePtr_t state_;
+ graph::StateWkPtr_t state_;
SymbolicComponentPtr_t symbolicCC_;
};
} // namespace manipulation
diff --git a/include/hpp/manipulation/symbolic-component.hh b/include/hpp/manipulation/symbolic-component.hh
index 7da561c7431f17f5087e627cdd35fcea5272d0d5..0648b0818ee86093159773a56cf499587b79de98 100644
--- a/include/hpp/manipulation/symbolic-component.hh
+++ b/include/hpp/manipulation/symbolic-component.hh
@@ -34,7 +34,7 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI SymbolicComponent
{
public:
- typedef std::set<SymbolicComponentPtr_t> SymbolicComponents_t;
+ typedef std::set<SymbolicComponent*> SymbolicComponents_t;
/// return a shared pointer to new instance
static SymbolicComponentPtr_t create (const RoadmapPtr_t& roadmap);
@@ -81,7 +81,7 @@ namespace hpp {
RoadmapNodes_t nodes_;
private:
- RoadmapPtr_t roadmap_;
+ RoadmapWkPtr_t roadmap_;
SymbolicComponents_t to_, from_;
SymbolicComponentWkPtr_t weak_;
}; // class SymbolicComponent
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index 7a263cb2da49c87e0d903ff57ceaa572fbecce8a..0cf828f3a8bbe33395b2e1ce5decce33bf46c3d0 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -33,14 +33,15 @@ namespace hpp {
static const matrix3_t I3 = matrix3_t::Identity();
NumericalConstraintPtr_t AxialHandle::createGrasp
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
+ if (n.empty())
+ n = "Transformation_(1,1,1,1,1,0)_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,1,1,1,1,0)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -48,15 +49,16 @@ namespace hpp {
}
NumericalConstraintPtr_t AxialHandle::createGraspComplement
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (false)(false)(false)(false)(false)
(true);
+ if (n.empty())
+ n = "Transformation_(0,0,0,0,0,1)_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(0,0,0,0,0,1)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -65,16 +67,17 @@ namespace hpp {
}
NumericalConstraintPtr_t AxialHandle::createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const
+ (const GripperPtr_t& gripper, const value_type& shift, std::string n) const
{
using boost::assign::list_of;
std::vector <bool> mask = list_of (true)(true)(true)(true)(true)(false);
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
+ if (n.empty())
+ n = "Transformation_(1,1,1,1,1,0)_" + name () + "_" + gripper->name ();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,1,1,1,1,0)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
transform,
@@ -83,7 +86,7 @@ namespace hpp {
NumericalConstraintPtr_t AxialHandle::createPreGraspComplement
(const GripperPtr_t& gripper, const value_type& shift,
- const value_type& width) const
+ const value_type& width, std::string n) const
{
using boost::assign::list_of;
using core::DoubleInequality;
@@ -91,10 +94,11 @@ namespace hpp {
(false);
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
+ if (n.empty())
+ n = "Transformation_(1,0,0,0,0,0)_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Transformation_(1,0,0,0,0,0)_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
transform, localPosition(), mask),
diff --git a/src/graph-node-optimizer.cc b/src/graph-node-optimizer.cc
index 4ee5886f7a6cc640b2308b5acd1f0d0ff2b872c5..4ddf44bc4985e92fc508f9f9c0e69e142414fa5d 100644
--- a/src/graph-node-optimizer.cc
+++ b/src/graph-node-optimizer.cc
@@ -16,7 +16,7 @@
#include <hpp/manipulation/graph-node-optimizer.hh>
-#include <hpp/core/steering-method-straight.hh>
+#include <hpp/core/steering-method/straight.hh>
namespace hpp {
namespace manipulation {
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index a20080f3c92a2f754f20af666dd283fd03a55096..8c7fdd8863c3d2366641cd996ffc95e834afe943 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -38,19 +38,14 @@ namespace hpp {
namespace graph {
Edge::Edge (const std::string& name) :
GraphComponent (name), isShort_ (false),
- pathConstraints_ (new Constraint_t()),
- configConstraints_ (new Constraint_t()),
- steeringMethod_ (new SteeringMethod_t()),
- pathValidation_ (new PathValidation_t())
+ pathConstraints_ (),
+ configConstraints_ (),
+ steeringMethod_ (),
+ pathValidation_ ()
{}
Edge::~Edge ()
- {
- if (pathConstraints_ ) delete pathConstraints_;
- if (configConstraints_) delete configConstraints_;
- if (steeringMethod_ ) delete steeringMethod_;
- if (pathValidation_ ) delete pathValidation_;
- }
+ {}
StatePtr_t Edge::to () const
{
@@ -90,28 +85,6 @@ namespace hpp {
return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1));
}
- bool WaypointEdge::direction (const core::PathPtr_t& path) const
- {
- Configuration_t q0 = path->initial (),
- q1 = path->end ();
- const bool src_contains_q0 = waypoints_.back().second->contains (q0);
- const bool dst_contains_q0 = to ()->contains (q0);
- const bool src_contains_q1 = waypoints_.back().second->contains (q1);
- const bool dst_contains_q1 = to ()->contains (q1);
-
- /// See Edge::direction for Karnaugh table
- /// true if reverse
- if ( (!src_contains_q0 && !src_contains_q1)
- || (!dst_contains_q0 && !dst_contains_q1)
- || (!src_contains_q0 && !dst_contains_q0))
- HPP_THROW (std::runtime_error,
- "Edge " << name() << " does not seem to have generated path from"
- << pinocchio::displayConfig(q0) << " to "
- << pinocchio::displayConfig(q1)
- );
- return !(src_contains_q0 && (!src_contains_q1 || dst_contains_q1));
- }
-
bool Edge::intersectionConstraint (const EdgePtr_t& other,
ConfigProjectorPtr_t proj) const
{
@@ -159,6 +132,13 @@ namespace hpp {
state_ = to;
}
+ void Edge::initialize ()
+ {
+ configConstraints_ = buildConfigConstraint ();
+ pathConstraints_ = buildPathConstraint ();
+ isInit_ = true;
+ }
+
std::ostream& Edge::print (std::ostream& os) const
{
os << "| | |-- ";
@@ -181,13 +161,11 @@ namespace hpp {
ConstraintSetPtr_t Edge::configConstraint() const
{
- if (!*configConstraints_) {
- configConstraints_->set (buildConfigConstraint ());
- }
- return configConstraints_->get ();
+ throwIfNotInitialized ();
+ return configConstraints_;
}
- ConstraintSetPtr_t Edge::buildConfigConstraint() const
+ ConstraintSetPtr_t Edge::buildConfigConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -216,14 +194,11 @@ namespace hpp {
ConstraintSetPtr_t Edge::pathConstraint() const
{
- if (!*pathConstraints_) {
- ConstraintSetPtr_t pathConstraints (buildPathConstraint ());
- pathConstraints_->set (pathConstraints);
- }
- return pathConstraints_->get ();
+ throwIfNotInitialized ();
+ return pathConstraints_;
}
- ConstraintSetPtr_t Edge::buildPathConstraint() const
+ ConstraintSetPtr_t Edge::buildPathConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -244,16 +219,15 @@ namespace hpp {
// Build steering method
const ProblemPtr_t& problem (g->problem());
- steeringMethod_->set(problem->steeringMethod()
- ->innerSteeringMethod()->copy());
- steeringMethod_->get()->constraints (constraint);
+ steeringMethod_ = problem->steeringMethod()->innerSteeringMethod()->copy();
+ steeringMethod_->constraints (constraint);
// Build path validation and relative motion matrix
// TODO this path validation will not contain obstacles added after
// its creation.
- pathValidation_->set(problem->pathValidationFactory ());
+ pathValidation_ = problem->pathValidationFactory ();
relMotion_ = RelativeMotion::matrix (g->robot());
RelativeMotion::fromConstraint (relMotion_, g->robot(), constraint);
- pathValidation_->get()->filterCollisionPairs (relMotion_);
+ pathValidation_->filterCollisionPairs (relMotion_);
return constraint;
}
@@ -273,12 +247,7 @@ namespace hpp {
const
{
using pinocchio::displayConfig;
- core::SteeringMethodPtr_t sm (steeringMethod_->get());
- if (!sm) {
- buildPathConstraint ();
- }
- sm = (steeringMethod_->get());
- if (!sm) {
+ if (!steeringMethod_) {
std::ostringstream oss;
oss << "No steering method set in edge " << name () << ".";
throw std::runtime_error (oss.str ().c_str ());
@@ -287,7 +256,7 @@ namespace hpp {
constraints->configProjector ()->rightHandSideFromConfig(q1);
if (constraints->isSatisfied (q1)) {
if (constraints->isSatisfied (q2)) {
- path = (*sm) (q1, q2);
+ path = (*steeringMethod_) (q1, q2);
return (bool)path;
} else {
hppDout(info, "q2 = " << displayConfig (q2)
@@ -346,13 +315,16 @@ namespace hpp {
const StateWkPtr_t& to)
{
Edge::init (weak, graph, from, to);
+ nbWaypoints(0);
wkPtr_ = weak;
}
bool WaypointEdge::canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const
{
/// TODO: This is not correct
- return waypoints_.back().first->canConnect (q1, q2) && Edge::canConnect (q1, q2);
+ for (std::size_t i = 0; i < edges_.size (); ++i)
+ if (!edges_[i]->canConnect(q1, q2)) return false;
+ return true;
}
bool WaypointEdge::build (core::PathPtr_t& path, ConfigurationIn_t q1,
@@ -364,101 +336,82 @@ namespace hpp {
graph_.lock ()->robot ()->numberDof ());
// Many times, this will be called rigth after WaypointEdge::applyConstraints so config_
// already satisfies the constraints.
- bool useCache = init_.isApprox (q1) && result_.isApprox (q2);
- if (!useCache) configs_.col (0) = q2;
-
- assert (waypoints_[0].first);
- if (!waypoints_[0].first->applyConstraints (q1, configs_.col (0))) {
- hppDout (info, "Waypoint edge " << name() << ": applyConstraints failed at waypoint 0."
- << "\nUse cache: " << useCache
- );
- return false;
- }
- if (!waypoints_[0].first->build (p, q1, configs_.col (0))) {
- hppDout (info, "Waypoint edge " << name() << ": build failed at waypoint 0."
- << "\nUse cache: " << useCache
- );
- return false;
- }
- pv->appendPath (p);
-
- for (std::size_t i = 1; i < waypoints_.size (); ++i) {
- assert (waypoints_[i].first);
- if (!useCache) configs_.col (i) = q2;
- if (!waypoints_[i].first->applyConstraints (configs_.col(i-1), configs_.col (i))) {
+ size_type n = edges_.size();
+ assert (configs_.cols() == n + 1);
+ bool useCache = lastSucceeded_
+ && configs_.col(0).isApprox (q1)
+ && configs_.col(n).isApprox (q2);
+ configs_.col(0) = q1;
+ configs_.col(n) = q2;
+
+ 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 << "."
<< "\nUse cache: " << useCache
);
+ lastSucceeded_ = false;
return false;
}
- if (!waypoints_[i].first->build (p, configs_.col(i-1), configs_.col (i))) {
+ if (!edges_[i]->build (p, configs_.col(i), configs_.col (i+1))) {
hppDout (info, "Waypoint edge " << name() << ": build failed at waypoint " << i << "."
<< "\nUse cache: " << useCache
);
+ lastSucceeded_ = false;
return false;
}
pv->appendPath (p);
}
- if (!Edge::build (p, configs_.col (configs_.cols()-1), q2))
- return false;
- pv->appendPath (p);
-
path = pv;
+ lastSucceeded_ = true;
return true;
}
bool WaypointEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
{
- assert (waypoints_[0].first);
- configs_.col (0) = q;
- if (!waypoints_[0].first->applyConstraints (qoffset, configs_.col (0))) {
- q = configs_.col(0);
- return false;
- }
- for (std::size_t i = 1; i < waypoints_.size (); ++i) {
- assert (waypoints_[i].first);
- configs_.col (i) = q;
- if (!waypoints_[i].first->applyConstraints (configs_.col(i-1), configs_.col (i))) {
- q = configs_.col(i);
+ assert (configs_.cols() == size_type(edges_.size() + 1));
+ configs_.col(0) = qoffset;
+ 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))) {
+ q = configs_.col(i+1);
+ lastSucceeded_ = false;
return false;
}
}
- bool success = Edge::applyConstraints (configs_.col (configs_.cols()-1), q);
- init_ = qoffset;
- result_ = q;
- return success;
+ q = configs_.col(edges_.size());
+ lastSucceeded_ = true;
+ return true;
}
void WaypointEdge::nbWaypoints (const size_type number)
{
- waypoints_.resize (number);
+ edges_.resize (number + 1);
+ states_.resize (number + 1);
+ states_.back() = to();
const size_type nbDof = graph_.lock ()->robot ()->configSize ();
- configs_ = matrix_t (nbDof, number);
- init_ = Configuration_t (nbDof);
- result_ = Configuration_t (nbDof);
+ configs_ = matrix_t (nbDof, number + 2);
}
void WaypointEdge::setWaypoint (const std::size_t index,
const EdgePtr_t wEdge,
const StatePtr_t wTo)
{
- assert (index < waypoints_.size());
- waypoints_[index] = Waypoint_t (wEdge, wTo);
- }
-
- template <>
- EdgePtr_t WaypointEdge::waypoint <Edge> (const std::size_t index) const
- {
- assert (index < waypoints_.size());
- return waypoints_[index].first;
+ assert (edges_.size() == states_.size());
+ assert (index < edges_.size());
+ if (index == states_.size() - 1) {
+ assert (!wTo || wTo == to());
+ } else {
+ states_[index] = wTo;
+ }
+ edges_[index] = wEdge;
}
- template <>
- WaypointEdgePtr_t WaypointEdge::waypoint <WaypointEdge> (const std::size_t index) const
+ const EdgePtr_t& WaypointEdge::waypoint (const std::size_t index) const
{
- assert (index < waypoints_.size());
- return HPP_DYNAMIC_PTR_CAST (WaypointEdge, waypoints_[index].first);
+ assert (index < edges_.size());
+ return edges_[index];
}
std::ostream& WaypointEdge::print (std::ostream& os) const
@@ -473,29 +426,18 @@ namespace hpp {
{
// First print the waypoint node, then the first edge.
da ["style"]="dashed";
- for (std::size_t i = 0; i < waypoints_.size (); ++i)
- waypoints_[i].second->dotPrint (os, da);
+ for (std::size_t i = 0; i < states_.size () - 1; ++i)
+ states_[i]->dotPrint (os, da);
da ["style"]="solid";
- for (std::size_t i = 0; i < waypoints_.size (); ++i)
- waypoints_[i].first->dotPrint (os, da) << std::endl;
+ for (std::size_t i = 0; i < edges_.size (); ++i)
+ edges_[i]->dotPrint (os, da) << std::endl;
da ["style"]="dotted";
da ["dir"] = "both";
da ["arrowtail"]="dot";
- // TODO: This is very ugly. There ought to be a better way of
- // getting the real from() Node.
- // We should be using Edge::dotPrint (...) instead of the following
- // paragraph.
- da.insert ("shape", "onormal");
- da.insertWithQuote ("label", name());
- dot::Tooltip tp; tp.addLine ("Edge constains:");
- populateTooltip (tp);
- da.insertWithQuote ("tooltip", tp.toStr());
- da.insertWithQuote ("labeltooltip", tp.toStr());
- os << waypoints_.back().second->id () << " -> " << to()->id () << " " << da << ";";
- return os;
+ return Edge::dotPrint (os, da);
}
std::ostream& LevelSetEdge::print (std::ostream& os) const
@@ -686,7 +628,13 @@ namespace hpp {
g->insertHistogram (hist_);
}
- ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint() const
+ void LevelSetEdge::initialize ()
+ {
+ Edge::initialize();
+ buildHistogram ();
+ }
+
+ ConstraintSetPtr_t LevelSetEdge::buildConfigConstraint()
{
std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
@@ -728,29 +676,34 @@ namespace hpp {
void LevelSetEdge::insertParamConstraint (const NumericalConstraintPtr_t& nm,
const segments_t& passiveDofs)
{
+ isInit_ = false;
paramNumericalConstraints_.push_back (nm);
paramPassiveDofs_.push_back (passiveDofs);
}
void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
{
+ isInit_ = false;
insertParamConstraint (NumericalConstraint::create (function, ineq));
}
void LevelSetEdge::insertParamConstraint (const LockedJointPtr_t lockedJoint)
{
+ isInit_ = false;
paramLockedJoints_.push_back (lockedJoint);
}
void LevelSetEdge::insertConditionConstraint (const NumericalConstraintPtr_t& nm,
const segments_t& passiveDofs)
{
+ isInit_ = false;
condNumericalConstraints_.push_back (nm);
condPassiveDofs_.push_back (passiveDofs);
}
void LevelSetEdge::insertConditionConstraint (const LockedJointPtr_t lockedJoint)
{
+ isInit_ = false;
condLockedJoints_.push_back (lockedJoint);
}
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index bce00dc3b13e885203977e0a85086be093a104b3..e0ffc977dd9799399094079fd96da50b2f1faa78 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -48,6 +48,7 @@ namespace hpp {
void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm,
const segments_t& passiveDofs)
{
+ isInit_ = false;
numericalConstraints_.push_back(nm);
passiveDofs_.push_back (passiveDofs);
}
@@ -59,6 +60,7 @@ namespace hpp {
void GraphComponent::resetNumericalConstraints ()
{
+ isInit_ = false;
numericalConstraints_.clear();
passiveDofs_.clear();
}
@@ -66,11 +68,13 @@ namespace hpp {
void GraphComponent::addLockedJointConstraint
(const LockedJointPtr_t& constraint)
{
+ isInit_ = false;
lockedJoints_.push_back (constraint);
}
void GraphComponent::resetLockedJoints ()
{
+ isInit_ = false;
lockedJoints_.clear();
}
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 68e4dc7bbf25fb43c6ca0dfad4b2b936fd51ccea..c3812b75acc4ee07b9a5e4d41a8f20eb82ef7d9b 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -47,15 +47,23 @@ namespace hpp {
);
}
+ void Graph::initialize ()
+ {
+ hists_.clear ();
+ assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
+ for (std::size_t i = 1; i < components_.size(); ++i)
+ components_[i].lock()->initialize();
+ isInit_ = true;
+ }
+
StateSelectorPtr_t Graph::createNodeSelector (const std::string& name)
{
- stateSelector_ = StateSelector::create (name);
- stateSelector_->parentGraph (wkPtr_);
- return stateSelector_;
+ return createStateSelector (name);
}
StateSelectorPtr_t Graph::createStateSelector (const std::string& name)
{
+ isInit_ = false;
stateSelector_ = StateSelector::create (name);
stateSelector_->parentGraph (wkPtr_);
return stateSelector_;
@@ -63,18 +71,19 @@ namespace hpp {
void Graph::nodeSelector (StateSelectorPtr_t ns)
{
- stateSelector_ = ns;
- stateSelector_->parentGraph (wkPtr_);
+ stateSelector (ns);
}
void Graph::stateSelector (StateSelectorPtr_t ns)
{
+ isInit_ = false;
stateSelector_ = ns;
stateSelector_->parentGraph (wkPtr_);
}
void Graph::maxIterations (size_type iterations)
{
+ isInit_ = false;
maxIterations_ = iterations;
}
@@ -85,6 +94,7 @@ namespace hpp {
void Graph::errorThreshold (const value_type& threshold)
{
+ isInit_ = false;
errorThreshold_ = threshold;
}
@@ -140,27 +150,27 @@ namespace hpp {
return stateSelector_->chooseEdge (from);
}
- ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state)
+ ConstraintSetPtr_t Graph::configConstraint (const StatePtr_t& state) const
{
return state->configConstraint ();
}
bool Graph::getConfigErrorForNode (ConfigurationIn_t config,
const StatePtr_t& state,
- vector_t& error)
+ vector_t& error) const
{
- return configConstraint (state)->isSatisfied (config, error);
+ return getConfigErrorForState (config, state, error);
}
bool Graph::getConfigErrorForState (ConfigurationIn_t config,
const StatePtr_t& state,
- vector_t& error)
+ vector_t& error) const
{
return configConstraint (state)->isSatisfied (config, error);
}
bool Graph::getConfigErrorForEdge (ConfigurationIn_t config,
- const EdgePtr_t& edge, vector_t& error)
+ const EdgePtr_t& edge, vector_t& error) const
{
ConstraintSetPtr_t cs (pathConstraint (edge));
ConfigProjectorPtr_t cp (cs->configProjector ());
@@ -170,7 +180,7 @@ namespace hpp {
bool Graph::getConfigErrorForEdgeLeaf
(ConfigurationIn_t leafConfig, ConfigurationIn_t config,
- const EdgePtr_t& edge, vector_t& error)
+ const EdgePtr_t& edge, vector_t& error) const
{
ConstraintSetPtr_t cs (pathConstraint (edge));
ConfigProjectorPtr_t cp (cs->configProjector ());
@@ -178,12 +188,12 @@ namespace hpp {
return cs->isSatisfied (config, error);
}
- ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge)
+ ConstraintSetPtr_t Graph::configConstraint (const EdgePtr_t& edge) const
{
return edge->configConstraint ();
}
- ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge)
+ ConstraintSetPtr_t Graph::pathConstraint (const EdgePtr_t& edge) const
{
return edge->pathConstraint ();
}
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index fd48f6d1fc868499b2143c54e95d2d9dcd091f04..6c78d18378382d209b24257740f5c048788412e0 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -40,6 +40,8 @@
#include <hpp/manipulation/graph/guided-state-selector.hh>
#include <hpp/manipulation/problem-solver.hh>
+#define CASE_TO_STRING(var, value) ( (var & value) ? std::string(#value) : std::string() )
+
namespace hpp {
namespace manipulation {
namespace graph {
@@ -120,13 +122,7 @@ namespace hpp {
struct CaseTraits {
static const bool pregrasp = (gCase & WithPreGrasp);
static const bool preplace = (gCase & WithPrePlace);
- /// FIXME
- // It should be
- // static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));
- // but when NoPlace | WithPreGrasp, we need a LevelSetEdge after
- // the pregrasp waypoint state. Sadly the current implementation of
- // WaypointEdge does not allow the last edge of type other than Edge.
- static const bool intersec = pregrasp || preplace || ((gCase & GraspOnly) && (gCase & PlaceOnly));
+ static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));
static const bool valid =
( (gCase & WithPreGrasp) || (gCase & GraspOnly) || (gCase & NoGrasp) )
@@ -145,6 +141,17 @@ namespace hpp {
static inline const StatePtr_t& Nintersec (const StateArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; }
static inline const StatePtr_t& Npreplace (const StateArray& n) { assert (preplace); return n[1 + (pregrasp?1:0) + (intersec?1:0)]; }
+ static inline std::string caseToString ()
+ {
+ return CASE_TO_STRING (gCase, NoGrasp)
+ + CASE_TO_STRING (gCase, GraspOnly)
+ + CASE_TO_STRING (gCase, WithPreGrasp)
+ + " - "
+ + CASE_TO_STRING (gCase, NoPlace)
+ + CASE_TO_STRING (gCase, PlaceOnly)
+ + CASE_TO_STRING (gCase, WithPrePlace);
+ }
+
static inline EdgePtr_t makeWE (
const std::string& name,
const StatePtr_t& from, const StatePtr_t& to,
@@ -190,8 +197,8 @@ namespace hpp {
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
gls = linkWaypoint <LevelSetEdge> (n, T-1, T, name, "ls");
- for (std::size_t i = 0; i < Nedges - 1; ++i)
- we->setWaypoint (i, e[i], n[i]);
+ for (std::size_t i = 0; i < Nedges; ++i)
+ we->setWaypoint (i, e[i], n[i+1]);
we->setWaypoint (T-1, gls, n[T]);
gls->state (n.front());
gls->setShort (pregrasp);
@@ -217,8 +224,11 @@ namespace hpp {
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
pls = linkWaypoint <LevelSetEdge> (n, T+1, T, name, "ls");
- for (std::size_t i = Nedges - 1; i != 0; --i)
+ // for (std::size_t i = Nedges - 1; i != 0; --i)
+ for (std::size_t k = 0; k < Nedges; ++k) {
+ std::size_t i = Nedges - 1 - k;
we->setWaypoint (Nedges - 1 - i, e[i], n[i]);
+ }
we->setWaypoint (Nedges - 1 - T, pls, n[T]);
pls->state (n.back ());
pls->setShort (preplace);
@@ -255,16 +265,17 @@ namespace hpp {
EdgeArray e;
WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(WaypointEdge, edge);
if (forward)
- for (std::size_t i = 0; i < Nedges - 1; ++i) {
+ for (std::size_t i = 0; i < Nedges; ++i) {
e[i] = linkWaypoint <Edge> (states, i, i + 1, name);
we->setWaypoint (i, e[i], states[i+1]);
}
else
- for (std::size_t i = Nedges - 1; i != 0; --i) {
+ // for (std::size_t i = Nedges - 1; i != 0; --i) {
+ for (std::size_t k = 0; k < Nedges; ++k) {
+ std::size_t i = Nedges - 1 - k;
e[i] = linkWaypoint <Edge> (states, i + 1, i, name);
we->setWaypoint (Nedges - 1 - i, e[i], states[i]);
}
- e[(forward?Nedges - 1:0)] = we;
return e;
}
@@ -333,6 +344,8 @@ namespace hpp {
const FoliatedManifold& submanifoldDef)
{
typedef CaseTraits<gCase> T;
+ hppDout (info, "Creating edges " << forwName << " and " << backName
+ << "\ncase is " << T::caseToString ());
assert (T::valid && "Not a valid case.");
typedef typename T::StateArray StateArray;
typedef typename T::EdgeArray EdgeArray;
@@ -444,20 +457,21 @@ namespace hpp {
const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
- NumericalConstraintPtr_t gc = handle->createGrasp (gripper);
+ NumericalConstraintPtr_t gc = handle->createGrasp (gripper, "");
grasp.nc.nc.push_back (gc);
grasp.nc.pdof.push_back (segments_t ());
grasp.nc_path.nc.push_back (gc);
// TODO: see function declaration
grasp.nc_path.pdof.push_back (segments_t ());
- NumericalConstraintPtr_t gcc = handle->createGraspComplement (gripper);
+ NumericalConstraintPtr_t gcc = handle->createGraspComplement
+ (gripper, "");
if (gcc->function ().outputSize () > 0) {
grasp.nc_fol.nc.push_back (gcc);
grasp.nc_fol.pdof.push_back (segments_t());
}
const value_type c = handle->clearance () + gripper->clearance ();
- NumericalConstraintPtr_t pgc = handle->createPreGrasp (gripper, c);
+ NumericalConstraintPtr_t pgc = handle->createPreGrasp (gripper, c, "");
pregrasp.nc.nc.push_back (pgc);
pregrasp.nc.pdof.push_back (segments_t());
pregrasp.nc_path.nc.push_back (pgc);
@@ -551,6 +565,7 @@ namespace hpp {
typedef std::vector<CompiledRule> CompiledRules_t;
struct Result {
+ ProblemSolverPtr_t ps;
GraphPtr_t graph;
typedef unsigned long stateid_type;
std::tr1::unordered_map<stateid_type, StateAndManifold_t> states;
@@ -572,8 +587,8 @@ namespace hpp {
CompiledRules_t rules;
CompiledRule::Status defaultAcceptationPolicy;
- Result (const Grippers_t& grippers, const Objects_t& objects, GraphPtr_t g) :
- graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects),
+ Result (const ProblemSolverPtr_t problem, const Grippers_t& grippers, const Objects_t& objects, GraphPtr_t g) :
+ ps (problem), graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects),
defaultAcceptationPolicy (CompiledRule::Refuse)
{
BOOST_FOREACH (const Object_t& o, objects) {
@@ -648,6 +663,8 @@ namespace hpp {
inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint (
const index_t& iG, const index_t& iOH)
{
+ typedef core::ProblemSolver CPS_t;
+
boost::array<NumericalConstraintPtr_t,3>& gcs =
graspCs [iG * nOH + iOH];
if (!gcs[0]) {
@@ -655,10 +672,17 @@ namespace hpp {
<< iG << ", " << iOH << ")");
const GripperPtr_t& g (gs[iG]);
const HandlePtr_t& h (handle (iOH));
- gcs[0] = h->createGrasp (g);
- gcs[1] = h->createGraspComplement (g);
- const value_type c = h->clearance () + g->clearance ();
- gcs[2] = h->createPreGrasp (g, c);
+ const std::string& grasp = g->name() + " grasps " + h->name();
+ if (!ps->CPS_t::has<NumericalConstraintPtr_t>(grasp)) {
+ ps->createGraspConstraint (grasp, g->name(), h->name());
+ }
+ gcs[0] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp);
+ gcs[1] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp + "/complement");
+ const std::string& pregrasp = g->name() + " pregrasps " + h->name();
+ if (!ps->CPS_t::has<NumericalConstraintPtr_t>(pregrasp)) {
+ ps->createPreGraspConstraint (pregrasp, g->name(), h->name());
+ }
+ gcs[2] = ps->CPS_t::get<NumericalConstraintPtr_t>(pregrasp);
}
return gcs;
}
@@ -682,9 +706,10 @@ namespace hpp {
}
/// Check if an object can be placed
- bool objectCanBePlaced (const Object_t&) const
+ bool objectCanBePlaced (const Object_t& o) const
{
- return true; // o.get<0>().get<0>();
+ // If the object has no joint, then it cannot be placed.
+ return (o.get<0>().get<2>().size() > 0);
}
/// Check is an object is grasped by the GraspV_t
@@ -986,6 +1011,7 @@ namespace hpp {
}
void graphBuilder (
+ const ProblemSolverPtr_t& ps,
const Objects_t& objects,
const Grippers_t& grippers,
GraphPtr_t graph,
@@ -995,7 +1021,7 @@ namespace hpp {
StateSelectorPtr_t ns = graph->stateSelector ();
if (!ns) throw std::logic_error ("The graph does not have a StateSelector");
- Result r (grippers, objects, graph);
+ Result r (ps, grippers, objects, graph);
r.setRules (rules);
IndexV_t availG (r.nG), availOH (r.nOH);
@@ -1095,7 +1121,7 @@ namespace hpp {
graph->maxIterations (ps->maxIterProjection ());
graph->errorThreshold (ps->errorThreshold ());
- graphBuilder (objects, grippers, graph, rules);
+ graphBuilder (ps, objects, grippers, graph, rules);
ps->constraintGraph (graph);
return graph;
}
diff --git a/src/graph/state.cc b/src/graph/state.cc
index 1819c2f681ba14ddb05e46b3250cc11d35bfbf96..b8e441c182cadd5318f439f1f2490afeb35247fc 100644
--- a/src/graph/state.cc
+++ b/src/graph/state.cc
@@ -27,14 +27,12 @@ namespace hpp {
namespace manipulation {
namespace graph {
State::State (const std::string& name) :
- GraphComponent (name), configConstraints_ (new Constraint_t()),
+ GraphComponent (name), configConstraints_ (),
isWaypoint_ (false)
{}
State::~State ()
- {
- if (configConstraints_) delete configConstraints_;
- }
+ {}
StatePtr_t State::create (const std::string& name)
{
@@ -107,23 +105,21 @@ namespace hpp {
return os;
}
- ConstraintSetPtr_t State::configConstraint() const
+ void State::initialize()
{
- if (!*configConstraints_) {
- std::string n = "(" + name () + ")";
- GraphPtr_t g = graph_.lock ();
- ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
-
- ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
-
- g->insertLockedJoints (proj);
- insertLockedJoints (proj);
- configConstraints_->set (constraint);
- }
- return configConstraints_->get ();
+ isInit_ = true;
+
+ std::string n = "(" + name () + ")";
+ GraphPtr_t g = graph_.lock ();
+ configConstraints_ = ConstraintSet::create (g->robot (), "Set " + n);
+
+ ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
+ g->insertNumericalConstraints (proj);
+ insertNumericalConstraints (proj);
+ configConstraints_->addConstraint (proj);
+
+ g->insertLockedJoints (proj);
+ insertLockedJoints (proj);
}
void State::updateWeight (const EdgePtr_t& e, const Weight_t& w)
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index cd73d9641232ef60bac033a075ade2cfaf3367e2..0012c15fe9bd7177c8cdd1246a3aadbebbbc32c0 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -109,12 +109,12 @@ namespace hpp {
bool NodeBin::operator<(const NodeBin& rhs) const
{
- return state_->id () < rhs.state ()->id ();
+ return state()->id () < rhs.state ()->id ();
}
bool NodeBin::operator==(const NodeBin& rhs) const
{
- return state_ == rhs.state ();
+ return state() == rhs.state ();
}
const StatePtr_t& NodeBin::state () const
@@ -152,7 +152,7 @@ namespace hpp {
std::ostream& NodeBin::printValue (std::ostream& os) const
{
- return os << "NodeBin (" << state_->name () << ")";
+ return os << "NodeBin (" << state()->name () << ")";
}
LeafHistogramPtr_t LeafHistogram::create (const Foliation f)
@@ -198,7 +198,7 @@ namespace hpp {
void StateHistogram::add (const RoadmapNodePtr_t& n)
{
- iterator it = insert (NodeBin (graph_->getState (n)));
+ iterator it = insert (NodeBin (constraintGraph()->getState (n)));
it->push_back (n);
if (numberOfObservations()%10 == 0) {
hppDout (info, *this);
@@ -218,7 +218,7 @@ namespace hpp {
HistogramPtr_t StateHistogram::clone () const
{
- return HistogramPtr_t (new StateHistogram (graph_));
+ return HistogramPtr_t (new StateHistogram (constraintGraph()));
}
unsigned int LeafBin::numberOfObsOutOfConnectedComponent (const core::ConnectedComponentPtr_t& cc) const
diff --git a/src/handle.cc b/src/handle.cc
index 6d1edda8d2c2d5c65246a8af66566d4c4e8557f0..3524ace4c774cd8da22d6d5d29a2056112ac8dac 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -104,20 +104,22 @@ namespace hpp {
}
NumericalConstraintPtr_t Handle::createGrasp
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
using core::ExplicitRelativeTransformation;
// If handle is on a freeflying object, create an explicit constraint
if (is6Dmask(mask_) && isHandleOnR3SO3 (*this)) {
+ if (n.empty())
+ n = "Explicit_relative_transform_" + name() + "_" + gripper->name();
return ExplicitRelativeTransformation::create
- ("Explicit_relative_transform_" + name () + "_" + gripper->name (),
- gripper->joint ()->robot (), gripper->joint (), joint (),
+ (n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition())->createNumericalConstraint();
}
+ if (n.empty())
+ n = "Grasp_" + maskToStr(mask_) + "_" + name() + "_" + gripper->name();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Grasp_" + maskToStr(mask_) + "_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -125,22 +127,24 @@ namespace hpp {
}
NumericalConstraintPtr_t Handle::createGraspComplement
- (const GripperPtr_t& gripper) const
+ (const GripperPtr_t& gripper, std::string n) const
{
core::DevicePtr_t robot = gripper->joint()->robot();
if (is6Dmask(mask_)) {
+ if (n.empty())
+ n = "GraspComp_(0,0,0,0,0,0)_" + name () + "_" + gripper->name ();
return NumericalConstraint::create (
boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
- robot->configSize(), robot->numberDof (),
- "GraspComp_(0,0,0,0,0,0)_" + name () + "_" + gripper->name ()
- ))
+ robot->configSize(), robot->numberDof (), n))
);
} else {
// TODO handle cases where rotations or translation are allowed.
std::vector<bool> Cmask = complementMask(mask_);
+ if (n.empty())
+ n = "Transformation_" + maskToStr(Cmask) + "_" + name ()
+ + "_" + gripper->name ();
RelativeTransformationPtr_t function = RelativeTransformation::create
- ("Transformation_" + maskToStr(Cmask) + "_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -154,15 +158,17 @@ namespace hpp {
}
NumericalConstraintPtr_t Handle::createPreGrasp
- (const GripperPtr_t& gripper, const value_type& shift) const
+ (const GripperPtr_t& gripper, const value_type& shift, std::string n) const
{
using boost::assign::list_of;
Transform3f transform = gripper->objectPositionInJoint ()
* Transform3f (I3, vector3_t (shift,0,0));
+ if (n.empty())
+ n = "Pregrasp_ " + maskToStr(mask_) + "_" + name ()
+ + "_" + gripper->name ();
return NumericalConstraintPtr_t
(NumericalConstraint::create (RelativeTransformation::create
- ("Pregrasp_ " + maskToStr(mask_) + "_" + name ()
- + "_" + gripper->name (),
+ (n,
gripper->joint()->robot(),
gripper->joint (), joint (),
transform, localPosition(), mask_)));
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
index 5fa3a0ff1da3152d30badae64ee2d59de760256d..afb685a4eb475476e90416c546f4cc01bf104c5b 100644
--- a/src/path-optimization/spline-gradient-based.cc
+++ b/src/path-optimization/spline-gradient-based.cc
@@ -90,9 +90,8 @@ namespace hpp {
// The path should always go through the start and end states of the
// transition.
- // FIXME problem of waypoint edges...
- graph::WaypointEdgePtr_t we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, transition);
- graph::StatePtr_t from = (we ? we->waypoint<graph::Edge>(we->nbWaypoints() - 1)->to() : transition->from());
+ assert(!HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, transition));
+ graph::StatePtr_t from = transition->from();
graph::StatePtr_t to = transition->to();
graph::StatePtr_t from2 = from, to2 = to;
@@ -241,11 +240,11 @@ namespace hpp {
// ----------- Instanciate -------------------------------------------- //
- template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
- template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>;
- template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>;
+ // template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
+ // template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>;
+ // template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>;
template class SplineGradientBased<core::path::BernsteinBasis, 1>; // equivalent to StraightPath
- template class SplineGradientBased<core::path::BernsteinBasis, 2>;
+ // template class SplineGradientBased<core::path::BernsteinBasis, 2>;
template class SplineGradientBased<core::path::BernsteinBasis, 3>;
} // namespace pathOptimization
} // namespace manipulation
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 3d558ffed1811543eba65ce1ce291bd9f83b2813..694ea363be0f880e561cfdc47d3e50288c8e579e 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -33,6 +33,7 @@
#include <hpp/core/path-projector/global.hh>
#include <hpp/core/path-projector/recursive-hermite.hh>
#include <hpp/core/roadmap.hh>
+#include <hpp/core/steering-method/hermite.hh>
#include <hpp/core/steering-method/straight.hh>
#include <hpp/core/comparison-type.hh>
@@ -127,16 +128,20 @@ namespace hpp {
parent_t::add <core::PathProjectorBuilder_t> ("RecursiveHermite",
createPathProjector <core::pathProjector::RecursiveHermite>);
- parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
- parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
- parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
+ // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
+ // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
+ // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
- parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
+ // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
using core::SteeringMethodBuilder_t;
parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
GraphSteeringMethod::create <core::SteeringMethodStraight>);
+ parent_t::add <SteeringMethodBuilder_t> ("Graph-Straight",
+ GraphSteeringMethod::create <core::steeringMethod::Straight>);
+ parent_t::add <SteeringMethodBuilder_t> ("Graph-Hermite",
+ GraphSteeringMethod::create <core::steeringMethod::Hermite>);
parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut",
pathOptimization::Keypoints::create);
@@ -184,6 +189,14 @@ namespace hpp {
return constraintGraph_;
}
+ void ProblemSolver::initConstraintGraph ()
+ {
+ if (!constraintGraph_)
+ throw std::runtime_error ("The graph is not defined.");
+ initSteeringMethod();
+ constraintGraph_->initialize();
+ }
+
void ProblemSolver::createPlacementConstraint
(const std::string& name, const StringList_t& surface1,
const StringList_t& surface2, const value_type& margin)
@@ -231,7 +244,11 @@ namespace hpp {
addNumericalConstraint (name, NumericalConstraint::create
(constraints.first));
addNumericalConstraint (complementName, NumericalConstraint::create
- (constraints.second, core::Equality::create ()));
+ (constraints.second,
+ core::ComparisonTypes::create
+ (constraints.second->outputSize(),
+ core::ComparisonType::Equality))
+ );
}
void ProblemSolver::createPrePlacementConstraint
@@ -280,6 +297,35 @@ namespace hpp {
addNumericalConstraint (name, NumericalConstraint::create (cvxShape));
}
+ void ProblemSolver::createGraspConstraint
+ (const std::string& name, const std::string& gripper,
+ const std::string& handle)
+ {
+ GripperPtr_t g = robot_->get <GripperPtr_t> (gripper);
+ if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
+ HandlePtr_t h = robot_->get <HandlePtr_t> (handle);
+ if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
+ const std::string cname = name + "/complement";
+ NumericalConstraintPtr_t constraint (h->createGrasp (g, name));
+ NumericalConstraintPtr_t complement (h->createGraspComplement (g, cname));
+ addNumericalConstraint ( name, constraint);
+ addNumericalConstraint (cname, complement);
+ }
+
+ void ProblemSolver::createPreGraspConstraint
+ (const std::string& name, const std::string& gripper,
+ const std::string& handle)
+ {
+ GripperPtr_t g = robot_->get <GripperPtr_t> (gripper);
+ if (!g) throw std::runtime_error ("No gripper with name " + gripper + ".");
+ HandlePtr_t h = robot_->get <HandlePtr_t> (handle);
+ if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
+
+ value_type c = h->clearance () + g->clearance ();
+ NumericalConstraintPtr_t constraint = h->createPreGrasp (g, c, name);
+ addNumericalConstraint (name, constraint);
+ }
+
void ProblemSolver::pathValidationType (const std::string& type,
const value_type& tolerance)
{
diff --git a/src/symbolic-component.cc b/src/symbolic-component.cc
index 6b4da162cfd7b401762b1c36f96631f535f7c421..3eb5ed2028c227ceb12be9eca823dbd64af02936 100644
--- a/src/symbolic-component.cc
+++ b/src/symbolic-component.cc
@@ -32,7 +32,7 @@ namespace hpp {
void SymbolicComponent::addNode (const RoadmapNodePtr_t& node)
{
assert(state_);
- graph::StatePtr_t state = roadmap_->getState(node);
+ graph::StatePtr_t state = roadmap_.lock()->getState(node);
// Sanity check
if (state_ == state) // Oops
@@ -44,7 +44,7 @@ namespace hpp {
void SymbolicComponent::setFirstNode (const RoadmapNodePtr_t& node)
{
assert(!state_);
- state_ = roadmap_->getState(node);
+ state_ = roadmap_.lock()->getState(node);
nodes_.push_back(node);
}
@@ -52,18 +52,18 @@ namespace hpp {
{
if (otherCC->state_ != state_) return false;
SymbolicComponents_t::const_iterator it = std::find
- (to_.begin(), to_.end(), otherCC);
+ (to_.begin(), to_.end(), otherCC.get());
if (it == to_.end()) return false;
it = std::find
- (from_.begin(), from_.end(), otherCC);
+ (from_.begin(), from_.end(), otherCC.get());
if (it == from_.end()) return false;
return true;
}
void SymbolicComponent::canReach (const SymbolicComponentPtr_t& otherCC)
{
- to_.insert(otherCC);
- otherCC->from_.insert(weak_.lock());
+ to_.insert(otherCC.get());
+ otherCC->from_.insert(this);
}
void SymbolicComponent::merge (SymbolicComponentPtr_t other)
@@ -79,11 +79,11 @@ namespace hpp {
// Add other's nodes to this list.
nodes_.insert (nodes_.end (), other->nodes_.begin(), other->nodes_.end());
- from_.erase (other);
- other->from_.erase (weak_.lock());
+ from_.erase (other.get());
+ other->from_.erase (this);
from_.insert (other->from_.begin(), other->from_.end());
- to_.erase (other);
- other->to_.erase (weak_.lock());
+ to_.erase (other.get());
+ other->to_.erase (this);
to_.insert (other->to_.begin(), other->to_.end());
}