diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index fd821d8e0992d358ad4539cd9b531d56c84f2509..960121443a9fe60fc7e010868b9d1e3003a85463 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -26,8 +26,8 @@
namespace hpp {
namespace manipulation {
HPP_PREDEF_CLASS (Device);
- typedef boost::shared_ptr <Device> DevicePtr_t;
- typedef boost::shared_ptr <const Device> DeviceConstPtr_t;
+ typedef shared_ptr <Device> DevicePtr_t;
+ typedef shared_ptr <const Device> DeviceConstPtr_t;
typedef pinocchio::Joint Joint;
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointIndex JointIndex;
@@ -43,33 +43,33 @@ namespace hpp {
typedef pinocchio::LiegroupSpace LiegroupSpace;
typedef pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t;
HPP_PREDEF_CLASS (AxialHandle);
- typedef boost::shared_ptr <AxialHandle> AxialHandlePtr_t;
+ typedef shared_ptr <AxialHandle> AxialHandlePtr_t;
HPP_PREDEF_CLASS (Handle);
- typedef boost::shared_ptr <Handle> HandlePtr_t;
+ typedef shared_ptr <Handle> HandlePtr_t;
HPP_PREDEF_CLASS (Object);
- typedef boost::shared_ptr <Object> ObjectPtr_t;
- typedef boost::shared_ptr <const Object> ObjectConstPtr_t;
+ typedef shared_ptr <Object> ObjectPtr_t;
+ typedef shared_ptr <const Object> ObjectConstPtr_t;
HPP_PREDEF_CLASS (ProblemSolver);
typedef ProblemSolver* ProblemSolverPtr_t;
HPP_PREDEF_CLASS (Problem);
- typedef boost::shared_ptr <Problem> ProblemPtr_t;
- typedef boost::shared_ptr <const Problem> ProblemConstPtr_t;
+ typedef shared_ptr <Problem> ProblemPtr_t;
+ typedef shared_ptr <const Problem> ProblemConstPtr_t;
HPP_PREDEF_CLASS (Roadmap);
- typedef boost::shared_ptr <Roadmap> RoadmapPtr_t;
+ typedef shared_ptr <Roadmap> RoadmapPtr_t;
HPP_PREDEF_CLASS (RoadmapNode);
typedef RoadmapNode* RoadmapNodePtr_t;
typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
HPP_PREDEF_CLASS (ConnectedComponent);
- typedef boost::shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
+ typedef shared_ptr<ConnectedComponent> ConnectedComponentPtr_t;
HPP_PREDEF_CLASS (LeafConnectedComp);
- typedef boost::shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
- typedef boost::shared_ptr<const LeafConnectedComp>
+ typedef shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t;
+ typedef shared_ptr<const LeafConnectedComp>
LeafConnectedCompConstPtr_t;
typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t;
HPP_PREDEF_CLASS (WeighedLeafConnectedComp);
- typedef boost::shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
+ typedef shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t;
HPP_PREDEF_CLASS (WeighedDistance);
- typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t;
+ typedef shared_ptr<WeighedDistance> WeighedDistancePtr_t;
typedef constraints::RelativeOrientation RelativeOrientation;
typedef constraints::RelativePosition RelativePosition;
typedef constraints::RelativeOrientationPtr_t RelativeOrientationPtr_t;
@@ -86,17 +86,17 @@ namespace hpp {
typedef core::vectorIn_t vectorIn_t;
typedef core::vectorOut_t vectorOut_t;
HPP_PREDEF_CLASS (ManipulationPlanner);
- typedef boost::shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
+ typedef shared_ptr < ManipulationPlanner > ManipulationPlannerPtr_t;
HPP_PREDEF_CLASS (GraphPathValidation);
- typedef boost::shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
+ typedef shared_ptr < GraphPathValidation > GraphPathValidationPtr_t;
HPP_PREDEF_CLASS (SteeringMethod);
- typedef boost::shared_ptr < SteeringMethod > SteeringMethodPtr_t;
+ typedef shared_ptr < SteeringMethod > SteeringMethodPtr_t;
typedef core::PathOptimizer PathOptimizer;
typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphOptimizer);
- typedef boost::shared_ptr < GraphOptimizer > GraphOptimizerPtr_t;
+ typedef shared_ptr < GraphOptimizer > GraphOptimizerPtr_t;
HPP_PREDEF_CLASS (GraphNodeOptimizer);
- typedef boost::shared_ptr < GraphNodeOptimizer > GraphNodeOptimizerPtr_t;
+ typedef shared_ptr < GraphNodeOptimizer > GraphNodeOptimizerPtr_t;
typedef core::PathProjectorPtr_t PathProjectorPtr_t;
typedef std::vector <pinocchio::DevicePtr_t> Devices_t;
@@ -112,7 +112,7 @@ namespace hpp {
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS (ConstraintSet);
- typedef boost::shared_ptr <ConstraintSet> ConstraintSetPtr_t;
+ typedef shared_ptr <ConstraintSet> ConstraintSetPtr_t;
typedef core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef core::ConfigurationShooter ConfigurationShooter;
typedef core::ConfigurationShooterPtr_t ConfigurationShooterPtr_t;
@@ -136,14 +136,14 @@ namespace hpp {
namespace pathOptimization {
HPP_PREDEF_CLASS (SmallSteps);
- typedef boost::shared_ptr < SmallSteps > SmallStepsPtr_t;
+ typedef shared_ptr < SmallSteps > SmallStepsPtr_t;
HPP_PREDEF_CLASS (Keypoints);
- typedef boost::shared_ptr < Keypoints > KeypointsPtr_t;
+ typedef shared_ptr < Keypoints > KeypointsPtr_t;
} // namespace pathOptimization
namespace problemTarget {
HPP_PREDEF_CLASS (State);
- typedef boost::shared_ptr < State > StatePtr_t;
+ typedef shared_ptr < State > StatePtr_t;
} // namespace problemTarget
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph-path-validation.hh b/include/hpp/manipulation/graph-path-validation.hh
index 9bfe494af70c100724c08ed9dcec515a1bb51de3..ef36ad7d0f3b5a04d45f112bc45a9010e77a4e7e 100644
--- a/include/hpp/manipulation/graph-path-validation.hh
+++ b/include/hpp/manipulation/graph-path-validation.hh
@@ -105,7 +105,7 @@ namespace hpp {
/// hpp::core::ObstacleUserInterface::addObstacle in case of success.
void addObstacle (const hpp::core::CollisionObjectConstPtr_t& object)
{
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->addObstacle (object);
}
@@ -119,7 +119,7 @@ namespace hpp {
void removeObstacleFromJoint (const JointPtr_t& joint,
const pinocchio::CollisionObjectConstPtr_t& obstacle)
{
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->removeObstacleFromJoint (joint, obstacle);
}
@@ -132,7 +132,7 @@ namespace hpp {
/// success.
void filterCollisionPairs (const core::RelativeMotion::matrix_type& relMotion)
{
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->filterCollisionPairs (relMotion);
}
@@ -145,7 +145,7 @@ namespace hpp {
/// success.
void setSecurityMargins(const matrix_t& securityMargins)
{
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->setSecurityMargins(securityMargins);
}
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 5ea948be020cb87421d7e12b684c2337606651db..989be3ab753e0a0a68e5ac9bae82a4e2417cb099 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -555,18 +555,14 @@ namespace hpp {
/// Insert a constraints parameterizing the target state foliation
/// \param nm the numerical constraint,
- /// \param passiveDofs the passive degrees of freedom of the
- /// constraint.
- void insertParamConstraint (const ImplicitPtr_t& nm,
- const segments_t& passiveDofs = segments_t ());
+ void insertParamConstraint (const ImplicitPtr_t& nm);
/// Get constraints parameterizing the target state foliation
const NumericalConstraints_t& paramConstraints() const;
/// Insert a condition constraint
/// \sa LevelSetEdge::histogram
- void insertConditionConstraint (const ImplicitPtr_t& nm,
- const segments_t& passiveDofs = segments_t ());
+ void insertConditionConstraint (const ImplicitPtr_t& nm);
/// Get constraints parameterizing the target state foliation
/// \sa LevelSetEdge::histogram
@@ -595,12 +591,10 @@ namespace hpp {
// Parametrizer
// NumericalConstraints_t
NumericalConstraints_t paramNumericalConstraints_;
- IntervalsContainer_t paramPassiveDofs_;
// Condition
// NumericalConstraints_t
NumericalConstraints_t condNumericalConstraints_;
- IntervalsContainer_t condPassiveDofs_;
/// 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 5806bde298bcec447467651f2f0186b609b1e512..ff6cdd5d137f89555210f09f2603b1449d6b0391 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -32,15 +32,15 @@ namespace hpp {
HPP_PREDEF_CLASS (StateSelector);
HPP_PREDEF_CLASS (GraphComponent);
HPP_PREDEF_CLASS (GuidedStateSelector);
- typedef boost::shared_ptr < Graph > GraphPtr_t;
- typedef boost::shared_ptr < State > StatePtr_t;
- typedef boost::shared_ptr < Edge > EdgePtr_t;
- typedef boost::shared_ptr < WaypointEdge > WaypointEdgePtr_t;
- typedef boost::shared_ptr < LevelSetEdge > LevelSetEdgePtr_t;
- typedef boost::shared_ptr < StateSelector > StateSelectorPtr_t;
- typedef boost::shared_ptr < GuidedStateSelector >
+ typedef shared_ptr < Graph > GraphPtr_t;
+ typedef shared_ptr < State > StatePtr_t;
+ typedef shared_ptr < Edge > EdgePtr_t;
+ typedef shared_ptr < WaypointEdge > WaypointEdgePtr_t;
+ typedef shared_ptr < LevelSetEdge > LevelSetEdgePtr_t;
+ typedef shared_ptr < StateSelector > StateSelectorPtr_t;
+ typedef shared_ptr < GuidedStateSelector >
GuidedStateSelectorPtr_t;
- typedef boost::shared_ptr < GraphComponent > GraphComponentPtr_t;
+ typedef shared_ptr < GraphComponent > GraphComponentPtr_t;
typedef std::vector < GraphComponentWkPtr_t > GraphComponents_t;
typedef std::vector < StatePtr_t > States_t;
typedef std::vector < EdgePtr_t > Edges_t;
@@ -56,13 +56,13 @@ namespace hpp {
class Histogram;
class StateHistogram;
class LeafHistogram;
- typedef boost::shared_ptr <Histogram> HistogramPtr_t;
- typedef boost::shared_ptr <StateHistogram> StateHistogramPtr_t;
- typedef boost::shared_ptr <LeafHistogram> LeafHistogramPtr_t;
+ typedef shared_ptr <Histogram> HistogramPtr_t;
+ typedef shared_ptr <StateHistogram> StateHistogramPtr_t;
+ typedef shared_ptr <LeafHistogram> LeafHistogramPtr_t;
typedef std::list < HistogramPtr_t > Histograms_t;
class Validation;
- typedef boost::shared_ptr < Validation > ValidationPtr_t;
+ typedef shared_ptr < Validation > ValidationPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index f0c9c45586c6e1ca87a99bb28d02aefb4738375a..890ea10b1b00ee856ad87f291aad893a58a10e00 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -51,11 +51,8 @@ namespace hpp {
}
/// Add Implicit to the component.
- /// \param passiveDofs see ConfigProjector::addNumericalConstraint
- // for more information.
virtual void addNumericalConstraint (
- const ImplicitPtr_t& numConstraint,
- const segments_t& passiveDofs = segments_t ());
+ const ImplicitPtr_t& numConstraint);
/// Add a cost function Implicit to the component.
virtual void addNumericalCost (const ImplicitPtr_t& numCost);
@@ -63,43 +60,16 @@ namespace hpp {
/// Reset the numerical constraints stored in the component.
virtual void resetNumericalConstraints ();
- /// Add core::LockedJoint constraint to the component.
- /// \deprecated LockedJoint are handled as classical explicit
- /// constraint
- virtual void addLockedJointConstraint
- (const LockedJointPtr_t& constraint) HPP_MANIPULATION_DEPRECATED;
-
- /// \deprecated LockedJoint are handled as classical explicit
- /// constraint
- /// Reset the locked joint in the component.
- virtual void resetLockedJoints () HPP_MANIPULATION_DEPRECATED;
-
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
- /// Insert the LockedJoint constraints in a ConstraintSet
- /// \return true is at least one LockedJointPtr_t was inserted.
- /// \deprecated LockedJoint are handled as classical explicit
- /// constraint
- bool insertLockedJoints (ConfigProjectorPtr_t& cs) const
- HPP_MANIPULATION_DEPRECATED;
-
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalConstraints() const;
/// Get a reference to the NumericalConstraints_t
const NumericalConstraints_t& numericalCosts() const;
- /// Get a reference to the NumericalConstraints_t
- 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;
-
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
@@ -131,12 +101,8 @@ namespace hpp {
/// Stores the numerical constraints.
NumericalConstraints_t numericalConstraints_;
- /// Stores the passive dofs for each numerical constraints.
- IntervalsContainer_t passiveDofs_;
/// Stores the numerical costs.
NumericalConstraints_t numericalCosts_;
- /// 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/graph.hh b/include/hpp/manipulation/graph/graph.hh
index ade3266940a0a376f0651882f62b41f75b8b7640..8bd7099df6d33b6044596f3d70fa1569937c153d 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -17,7 +17,7 @@
#ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
# define HPP_MANIPULATION_GRAPH_GRAPH_HH
-# include <boost/tuple/tuple.hpp>
+# include <tuple>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/constraint-set.hh"
# include "hpp/manipulation/fwd.hh"
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index 381c57fae288aab69ca6fc4e67dff22c80fa31a4..06bad86eda099addb82c357417eed3f1d4024c3e 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -20,7 +20,7 @@
# include <string>
# include <algorithm>
-# include <boost/tuple/tuple.hpp>
+# include <tuple>
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
@@ -35,41 +35,28 @@ namespace hpp {
/// \addtogroup helper
/// \{
- struct NumericalConstraintsAndPassiveDofs {
- NumericalConstraints_t nc;
- IntervalsContainer_t pdof;
- NumericalConstraintsAndPassiveDofs merge
- (const NumericalConstraintsAndPassiveDofs& other) {
- NumericalConstraintsAndPassiveDofs ret;
- // ret.nc.reserve (nc.size() + other.nc.size());
- ret.pdof.reserve (pdof.size() + other.pdof.size());
-
- std::copy (nc.begin(), nc.end(), ret.nc.begin());
- std::copy (other.nc.begin(), other.nc.end(), ret.nc.begin());
-
- std::copy (pdof.begin(), pdof.end(), ret.pdof.begin());
- std::copy (other.pdof.begin(), other.pdof.end(), ret.pdof.begin());
- return ret;
- }
-
- template <bool forPath> void addToComp (GraphComponentPtr_t comp) const;
-
- template <bool param> void specifyFoliation (LevelSetEdgePtr_t lse) const;
- };
+ NumericalConstraints_t merge_nc
+ (const NumericalConstraints_t& a, const NumericalConstraints_t& b) {
+ NumericalConstraints_t nc;
+ nc.reserve (a.size() + b.size());
+ std::copy (a.begin(), a.end(), nc.begin());
+ std::copy (b.begin(), b.end(), nc.begin());
+ return nc;
+ }
struct FoliatedManifold {
// Manifold definition
- NumericalConstraintsAndPassiveDofs nc;
+ NumericalConstraints_t nc;
LockedJoints_t lj;
- NumericalConstraintsAndPassiveDofs nc_path;
+ NumericalConstraints_t nc_path;
// Foliation definition
- NumericalConstraintsAndPassiveDofs nc_fol;
+ NumericalConstraints_t nc_fol;
LockedJoints_t lj_fol;
FoliatedManifold merge (const FoliatedManifold& other) {
FoliatedManifold both;
- both.nc = nc.merge (other.nc);
- both.nc_path = nc_path.merge (other.nc_path);
+ both.nc = merge_nc(nc, other.nc);
+ both.nc_path = merge_nc(nc_path, other.nc_path);
std::copy (lj.begin (), lj.end (), both.lj.end ());
std::copy (other.lj.begin (), other.lj.end (), both.lj.end ());
@@ -81,10 +68,10 @@ namespace hpp {
void specifyFoliation (LevelSetEdgePtr_t lse) const;
bool foliated () const {
- return !lj_fol.empty () || !nc_fol.nc.empty ();
+ return !lj_fol.empty () || !nc_fol.empty ();
}
bool empty () const {
- return lj.empty () && nc.nc.empty ();
+ return lj.empty () && nc.empty ();
}
};
@@ -142,8 +129,6 @@ namespace hpp {
/// \li pregrasp
/// \li placement
- /// \todo when the handle is a free flying object, add the robot DOFs
- /// as passive dofs to the numerical constraints for paths
void graspManifold (
const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp);
@@ -165,10 +150,10 @@ namespace hpp {
const LockedJoints_t objectLocks,
FoliatedManifold& place, FoliatedManifold& preplace);
- typedef boost::tuple <ImplicitPtr_t,
- ImplicitPtr_t,
- LockedJoints_t>
- PlacementConstraint_t;
+ typedef std::tuple <ImplicitPtr_t,
+ ImplicitPtr_t,
+ LockedJoints_t>
+ PlacementConstraint_t;
typedef std::vector <HandlePtr_t> Handles_t;
typedef std::vector <GripperPtr_t> Grippers_t;
/// Tuple representing an object as follows:
@@ -176,7 +161,7 @@ namespace hpp {
/// \li Handles_t list of handles of the object
/// \li std::size_t the index of this tuple in Objects_t.
/// \note the index must be unique, as object equallity is checked using this index.
- typedef boost::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t;
+ typedef std::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t;
typedef std::vector <Object_t> Objects_t;
/// Fill a Graph
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 916326cc79b10aa6fcba100321d2a378b6c35b23..52f230d2be3567bc78757903f1e359523f5580e0 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -17,7 +17,7 @@
#ifndef HPP_MANIPULATION_GRAPH_STATE_HH
# define HPP_MANIPULATION_GRAPH_STATE_HH
-# include <boost/function.hpp>
+#include <functional>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
@@ -44,10 +44,10 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI State : public GraphComponent
{
public:
- typedef boost::function < EdgePtr_t
- (const std::string&,
- const GraphWkPtr_t&,
- const StateWkPtr_t&, const StateWkPtr_t&) >
+ typedef std::function < EdgePtr_t
+ (const std::string&,
+ const GraphWkPtr_t&,
+ const StateWkPtr_t&, const StateWkPtr_t&) >
EdgeFactory;
/// Destructor
~State ();
@@ -127,25 +127,18 @@ namespace hpp {
}
/// Add constraints::Implicit to the component.
- virtual void addNumericalConstraintForPath (const ImplicitPtr_t& nm,
- const segments_t& passiveDofs = segments_t ())
+ virtual void addNumericalConstraintForPath (const ImplicitPtr_t& nm)
{
invalidate();
numericalConstraintsForPath_.push_back (nm);
- passiveDofsForPath_.push_back (passiveDofs);
}
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
{
- assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
- IntervalsContainer_t::const_iterator itpdofs = passiveDofsForPath_.begin ();
- for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
- it != numericalConstraintsForPath_.end(); it++) {
- proj->add (*it, *itpdofs);
- itpdofs++;
- }
+ for (const auto& nc : numericalConstraintsForPath_)
+ proj->add (nc);
return !numericalConstraintsForPath_.empty ();
}
@@ -183,7 +176,6 @@ namespace hpp {
/// Stores the numerical constraints for path.
NumericalConstraints_t numericalConstraintsForPath_;
- IntervalsContainer_t passiveDofsForPath_;
/// A selector that will implement the selection of the next state.
StateSelectorWkPtr_t selector_;
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index 6c3a8ba781f4abadb7604805459caf27e7623c4b..13aaffb03f44cdf8952023bcd249064c29f67a53 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -198,7 +198,7 @@ namespace hpp {
graph::GraphPtr_t graph_;
};
typedef StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED;
- typedef boost::shared_ptr <StateHistogram> NodeHistogramPtr_t;
+ typedef shared_ptr <StateHistogram> NodeHistogramPtr_t;
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
index f50da1b3e6d9553ffea48eea6d1a1c557386d99c..570e0d6bc39944fd7d4309886da396b570101411 100644
--- a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
+++ b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
@@ -34,7 +34,7 @@ namespace hpp {
{
public:
typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
- typedef boost::shared_ptr<EnforceTransitionSemantic> Ptr_t;
+ typedef shared_ptr<EnforceTransitionSemantic> Ptr_t;
static Ptr_t create (const core::ProblemConstPtr_t& problem) {
ProblemConstPtr_t p (HPP_DYNAMIC_PTR_CAST(const Problem, problem));
diff --git a/include/hpp/manipulation/path-optimization/random-shortcut.hh b/include/hpp/manipulation/path-optimization/random-shortcut.hh
index b95767f73abe3130e72aab5011e634218755552f..45e1088db75abb3f8e71105cb1685d093664a153 100644
--- a/include/hpp/manipulation/path-optimization/random-shortcut.hh
+++ b/include/hpp/manipulation/path-optimization/random-shortcut.hh
@@ -28,7 +28,7 @@ namespace hpp {
/// \{
namespace pathOptimization {
HPP_PREDEF_CLASS (RandomShortcut);
- typedef boost::shared_ptr<RandomShortcut> RandomShortcutPtr_t;
+ typedef shared_ptr<RandomShortcut> RandomShortcutPtr_t;
class HPP_MANIPULATION_DLLAPI RandomShortcut :
public core::pathOptimization::RandomShortcut
diff --git a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
index 6af8561f4cea3f95ebb2e98646a7cfcaad9a2af8..6be91d527bc43d2e56be45d253f515a16c5772d1 100644
--- a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+++ b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
@@ -38,7 +38,7 @@ namespace hpp {
};
typedef core::pathOptimization::SplineGradientBased<PolynomeBasis, SplineOrder>
Parent_t;
- typedef boost::shared_ptr<SplineGradientBased> Ptr_t;
+ typedef shared_ptr<SplineGradientBased> Ptr_t;
using typename Parent_t::Spline;
using typename Parent_t::SplinePtr_t;
diff --git a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
index 133601a9dcc6a0af095c780a94234af4c7f22588..c4f2a2497cd0abe678e575a8c56bddeeadfdf03d 100644
--- a/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/path-planner/end-effector-trajectory.hh
@@ -43,10 +43,10 @@ namespace hpp {
protected:
virtual Configurations_t impl_solve (vectorIn_t target) = 0;
};
- typedef boost::shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
+ typedef shared_ptr<IkSolverInitialization> IkSolverInitializationPtr_t;
HPP_PREDEF_CLASS (EndEffectorTrajectory);
- typedef boost::shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
+ typedef shared_ptr<EndEffectorTrajectory> EndEffectorTrajectoryPtr_t;
class HPP_MANIPULATION_DLLAPI EndEffectorTrajectory : public core::PathPlanner
{
diff --git a/include/hpp/manipulation/serialization.hh b/include/hpp/manipulation/serialization.hh
index cfb18356e557547890ef18f1d86035836898fad7..568562d810e83372508c6566ed472bd4ba00484f 100644
--- a/include/hpp/manipulation/serialization.hh
+++ b/include/hpp/manipulation/serialization.hh
@@ -44,7 +44,7 @@ manipulation::graph::GraphPtr_t getGraphFromArchive(Archive& ar, const std::stri
}
template<class Archive, class GraphCompT>
-inline void serializeGraphComponent(Archive & ar, boost::shared_ptr<GraphCompT>& c, const unsigned int version)
+inline void serializeGraphComponent(Archive & ar, shared_ptr<GraphCompT>& c, const unsigned int version)
{
(void) version;
diff --git a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
index 542b8f99acf297a0e628c8b1b66885df6575c514..73b3052921e8c2d1d0268c301db38abf61cffcb4 100644
--- a/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
+++ b/include/hpp/manipulation/steering-method/end-effector-trajectory.hh
@@ -28,7 +28,7 @@ namespace hpp {
/// \{
namespace steeringMethod {
HPP_PREDEF_CLASS (EndEffectorTrajectory);
- typedef boost::shared_ptr < EndEffectorTrajectory > EndEffectorTrajectoryPtr_t;
+ typedef shared_ptr < EndEffectorTrajectory > EndEffectorTrajectoryPtr_t;
using core::PathPtr_t;
diff --git a/include/hpp/manipulation/steering-method/fwd.hh b/include/hpp/manipulation/steering-method/fwd.hh
index 467557d9d37ee8684783ecdeab090c12ee5b19bc..e3b66268d2e14f7e5a1fb8bb61bbd774e6f21bbd 100644
--- a/include/hpp/manipulation/steering-method/fwd.hh
+++ b/include/hpp/manipulation/steering-method/fwd.hh
@@ -27,9 +27,9 @@ namespace hpp {
namespace manipulation {
namespace steeringMethod {
HPP_PREDEF_CLASS (Graph);
- typedef boost::shared_ptr < Graph > GraphPtr_t;
+ typedef shared_ptr < Graph > GraphPtr_t;
HPP_PREDEF_CLASS (CrossStateOptimization);
- typedef boost::shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t;
+ typedef shared_ptr < CrossStateOptimization > CrossStateOptimizationPtr_t;
} // namespace steeringMethod
} // namespace manipulation
} // namespace hpp
diff --git a/src/connected-component.cc b/src/connected-component.cc
index ce7e0fd863658890a2d62fc48c81d09a8843ec5c..bf1249093884372b3b1eb0255491abab20d8ef0a 100644
--- a/src/connected-component.cc
+++ b/src/connected-component.cc
@@ -59,7 +59,7 @@ namespace hpp {
void ConnectedComponent::merge (const core::ConnectedComponentPtr_t& otherCC)
{
core::ConnectedComponent::merge(otherCC);
- const ConnectedComponentPtr_t other = boost::static_pointer_cast <ConnectedComponent> (otherCC);
+ const ConnectedComponentPtr_t other = static_pointer_cast <ConnectedComponent> (otherCC);
/// take all graph states in other->graphStateMap_ and put them in this->graphStateMap_
/// if they already exist in this->graphStateMap_, append roadmap nodes from other graph state
/// to graph state in this.
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 524bd8b5a978f66b126e687374460c423d6786ec..74c6821689c58cc61834b97f3548b5d74b5aae93 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -63,7 +63,7 @@ namespace hpp {
void Edge::relativeMotion(const RelativeMotion::matrix_type & m)
{
if(!isInit_) throw std::logic_error("The graph must be initialized before changing the relative motion matrix.");
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) oui->filterCollisionPairs(m);
relMotion_ = m;
@@ -214,61 +214,43 @@ namespace hpp {
const GraphPtr_t& graph)
{
NumericalConstraints_t nc;
- std::vector <segments_t> pdof;
- for (std::vector <GraphComponentPtr_t>::const_iterator it
- (components.begin ()); it != components.end (); ++it) {
- nc.insert (nc.end (), (*it)->numericalConstraints ().begin (),
- (*it)->numericalConstraints ().end ());
- pdof.insert (pdof.end (), (*it)->passiveDofs ().begin (),
- (*it)->passiveDofs ().end ());
- }
- assert (nc.size () == pdof.size ());
- NumericalConstraints_t::iterator itnc1, itnc2;
- std::vector <segments_t>::iterator itpdof1, itpdof2;
+ for (const auto& gc : components)
+ nc.insert(nc.end(), gc->numericalConstraints().begin(),
+ gc->numericalConstraints ().end ());
// Remove duplicate constraints
- for (itnc1 = nc.begin(), itpdof1 = pdof.begin(); itnc1 != nc.end(); ++itnc1, ++itpdof1) {
- itnc2 = itnc1; ++itnc2;
- itpdof2 = itpdof1; ++itpdof2;
- while (itnc2 != nc.end()) {
- if (*itnc1 == *itnc2) {
- itnc2 = nc.erase (itnc2);
- itpdof2 = pdof.erase (itpdof2);
- } else {
- ++itnc2;
- ++itpdof2;
- }
- }
- }
+ auto end = nc.end();
+ for (auto it = nc.begin(); it != end; ++it)
+ end = std::remove(std::next(it), end, *it);
+ nc.erase(end, nc.end());
+
+ NumericalConstraints_t::iterator itnc1, itnc2;
+
+ itnc2 = nc.end();
+ for (itnc1 = nc.begin(); itnc1 != itnc2; ++itnc1)
+ itnc2 = std::remove(std::next(itnc1), itnc2, *itnc1);
+ nc.erase(itnc2, nc.end());
// Look for complement
- for (itnc1 = nc.begin(), itpdof1 = pdof.begin(); itnc1 != nc.end(); ++itnc1, ++itpdof1) {
- itnc2 = itnc1; ++itnc2;
- itpdof2 = itpdof1; ++itpdof2;
+ for (itnc1 = nc.begin(); itnc1 != nc.end(); ++itnc1) {
+ const auto& c1 = *itnc1;
+ itnc2 = std::next(itnc1);
constraints::ImplicitPtr_t combination;
- while (itnc2 != nc.end()) {
+ itnc2 = std::find_if(std::next(itnc1), nc.end(),
+ [&c1, &combination, &graph](const auto& c2) -> bool {
+ assert (c1 != c2);
+ return graph->isComplement (c1, c2, combination)
+ || graph->isComplement (c2, c1, combination);
+ });
+ if (itnc2 != nc.end()) {
assert (*itnc1 != *itnc2);
- if ( graph->isComplement (*itnc1, *itnc2, combination)
- || graph->isComplement (*itnc2, *itnc1, combination)) {
- // Replace constraint by combination of both and remove
- // complement.
- *itnc1 = combination;
- nc.erase (itnc2);
- pdof.erase (itpdof2);
- break;
- } else {
- ++itnc2;
- ++itpdof2;
- }
+ *itnc1 = combination;
+ nc.erase (itnc2);
}
}
- NumericalConstraints_t::iterator itnc (nc.begin ());
- std::vector <segments_t>::iterator itpdof (pdof.begin ());
- while (itnc != nc.end ()) {
- proj->add (*itnc, *itpdof);
- ++itnc; ++itpdof;
- }
+ for (const auto& _nc : nc)
+ proj->add (_nc);
}
ConstraintSetPtr_t Edge::buildConfigConstraint()
@@ -335,7 +317,7 @@ namespace hpp {
// TODO this path validation will not contain obstacles added after
// its creation.
pathValidation_ = problem->pathValidationFactory ();
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
if (oui) {
relMotion_ = RelativeMotion::matrix (g->robot());
@@ -742,13 +724,8 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
- IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
- it != paramNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == paramPassiveDofs_.end ());
+ for (const auto& nc : paramNumericalConstraints_)
+ proj->add (nc);
param->addConstraint (proj);
param->edge (wkPtr_.lock ());
@@ -764,13 +741,8 @@ namespace hpp {
ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
- itpdof = condPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
- it != condNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == condPassiveDofs_.end ());
+ for (const auto& nc : condNumericalConstraints_)
+ proj->add (nc);
f.condition (cond);
cond->addConstraint (proj);
@@ -817,13 +789,8 @@ namespace hpp {
// - the state in which the transition lies if different
g->insertNumericalConstraints (proj);
- IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
- it != paramNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == paramPassiveDofs_.end ());
+ for (const auto& nc : paramNumericalConstraints_)
+ proj->add (nc);
insertNumericalConstraints (proj);
stateTo ()->insertNumericalConstraints (proj);
@@ -837,12 +804,10 @@ namespace hpp {
}
void LevelSetEdge::insertParamConstraint
- (const constraints::ImplicitPtr_t& nm,
- const segments_t& passiveDofs)
+ (const constraints::ImplicitPtr_t& nm)
{
invalidate ();
paramNumericalConstraints_.push_back (nm);
- paramPassiveDofs_.push_back (passiveDofs);
}
const NumericalConstraints_t& LevelSetEdge::paramConstraints() const
@@ -851,12 +816,10 @@ namespace hpp {
}
void LevelSetEdge::insertConditionConstraint
- (const constraints::ImplicitPtr_t& nm,
- const segments_t& passiveDofs)
+ (const constraints::ImplicitPtr_t& nm)
{
invalidate ();
condNumericalConstraints_.push_back (nm);
- condPassiveDofs_.push_back (passiveDofs);
}
const NumericalConstraints_t& LevelSetEdge::conditionConstraints() const
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 0900acc1a30a762fc4a8e7ebfa91583e50e75e01..ec63fa05dd6755e7d460202c8aed0ad516812dff 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -18,7 +18,6 @@
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
-#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/constraints/differentiable-function.hh>
@@ -51,12 +50,10 @@ namespace hpp {
invalidate();
}
- void GraphComponent::addNumericalConstraint (const ImplicitPtr_t& nm,
- const segments_t& passiveDofs)
+ void GraphComponent::addNumericalConstraint (const ImplicitPtr_t& nm)
{
invalidate();
numericalConstraints_.push_back(nm);
- passiveDofs_.push_back (passiveDofs);
}
void GraphComponent::addNumericalCost (const ImplicitPtr_t& cost)
@@ -69,33 +66,15 @@ namespace hpp {
{
invalidate();
numericalConstraints_.clear();
- passiveDofs_.clear();
numericalCosts_.clear();
}
- void GraphComponent::addLockedJointConstraint
- (const LockedJointPtr_t& constraint)
- {
- addNumericalConstraint (constraint);
- }
-
- void GraphComponent::resetLockedJoints ()
- {
- }
-
bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
{
- IntervalsContainer_t::const_iterator itpdof = passiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = numericalConstraints_.begin();
- it != numericalConstraints_.end(); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == passiveDofs_.end ());
- for (NumericalConstraints_t::const_iterator it = numericalCosts_.begin();
- it != numericalCosts_.end(); ++it) {
- proj->add (*it, 1);
- }
+ for (const auto& nc : numericalConstraints_)
+ proj->add (nc);
+ for (const auto& nc : numericalCosts_)
+ proj->add (nc, 1);
return !numericalConstraints_.empty ();
}
@@ -109,16 +88,6 @@ namespace hpp {
return numericalCosts_;
}
- const std::vector <segments_t>& GraphComponent::passiveDofs() const
- {
- return passiveDofs_;
- }
-
- const LockedJoints_t& GraphComponent::lockedJoints () const
- {
- return lockedJoints_;
- }
-
GraphPtr_t GraphComponent::parentGraph() const
{
return graph_.lock ();
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index ecd245b27e7f261df677d12be708bb4d9b696236..94e56a06d12ae424e4deeb511932cb01e08d99ba 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -19,10 +19,10 @@
#include <tr1/unordered_map>
#include <tr1/unordered_set>
-#include <boost/array.hpp>
+#include <iterator>
+#include <array>
+
#include <boost/regex.hpp>
-#include <boost/foreach.hpp>
-#include <boost/assign/list_of.hpp>
#include <pinocchio/multibody/model.hpp>
@@ -49,9 +49,10 @@ namespace hpp {
namespace helper {
typedef constraints::Implicit Implicit;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
+
template <bool forPath>
- void NumericalConstraintsAndPassiveDofs::addToComp
- (GraphComponentPtr_t comp) const
+ void addToComp
+ (const NumericalConstraints_t& nc, GraphComponentPtr_t comp)
{
if (nc.empty ()) return;
StatePtr_t n;
@@ -59,49 +60,41 @@ namespace hpp {
n = HPP_DYNAMIC_PTR_CAST (State, comp);
if (!n) throw std::logic_error ("Wrong type: expect a State");
}
- NumericalConstraints_t::const_iterator it;
- IntervalsContainer_t::const_iterator itpdof = pdof.begin ();
- for (it = nc.begin (); it != nc.end (); ++it) {
- if (*it) {
- if (forPath) n->addNumericalConstraintForPath (*it, *itpdof);
- else comp->addNumericalConstraint (*it, *itpdof);
+ for (const auto& c : nc)
+ if (c) {
+ if (forPath) n->addNumericalConstraintForPath (c);
+ else comp->addNumericalConstraint (c);
}
- ++itpdof;
- }
- assert (itpdof == pdof.end ());
}
template <bool param>
- void NumericalConstraintsAndPassiveDofs::specifyFoliation
- (LevelSetEdgePtr_t lse) const
+ void specifyFoliation
+ (const NumericalConstraints_t& nc, LevelSetEdgePtr_t lse)
{
- NumericalConstraints_t::const_iterator it;
- IntervalsContainer_t::const_iterator itpdof = pdof.begin ();
- for (it = nc.begin (); it != nc.end (); ++it) {
- if (*it) {
- if (param) lse->insertParamConstraint (*it, *itpdof);
- else lse->insertConditionConstraint (*it, *itpdof);
+ for (const auto& c : nc)
+ if (c) {
+ if (param) lse->insertParamConstraint (c);
+ else lse->insertConditionConstraint (c);
}
- ++itpdof;
- }
- assert (itpdof == pdof.end ());
}
void FoliatedManifold::addToState (StatePtr_t comp) const
{
- nc.addToComp <false> (comp);
- nc_path.addToComp <true> (comp);
+ addToComp<false>(nc, comp);
+ addToComp<false>(nc_path, comp);
}
void FoliatedManifold::addToEdge (EdgePtr_t comp) const
{
- nc_fol.addToComp <false> (comp);
+ addToComp <false> (nc_fol, comp);
}
void FoliatedManifold::specifyFoliation (LevelSetEdgePtr_t lse) const
{
- nc.specifyFoliation <false> (lse);
- nc_fol.specifyFoliation <true> (lse);
+ for (const auto& c : nc)
+ lse->insertConditionConstraint (c);
+ for (const auto& c : nc_fol)
+ lse->insertConditionConstraint (c);
}
namespace {
@@ -121,8 +114,8 @@ namespace hpp {
static const std::size_t Nedges = 1 + nbWaypoints;
// static const std::size_t iNpregrasp = pregrasp?1 + 1:nbWaypoints;
// static const std::size_t iNpreplace = pregrasp?1 + 1:nbWaypoints;
- typedef boost::array <StatePtr_t, Nstates> StateArray;
- typedef boost::array <EdgePtr_t, Nedges> EdgeArray;
+ typedef std::array <StatePtr_t, Nstates> StateArray;
+ typedef std::array <EdgePtr_t, Nedges> EdgeArray;
static inline const StatePtr_t& Npregrasp (const StateArray& n) { assert (pregrasp); return n[1]; }
static inline const StatePtr_t& Nintersec (const StateArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; }
@@ -145,7 +138,7 @@ namespace hpp {
const size_type& w)
{
if (Nedges > 1) {
- WaypointEdgePtr_t we = boost::static_pointer_cast <WaypointEdge>
+ WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
(from->linkTo (name, to, w, WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
return we;
@@ -179,7 +172,7 @@ namespace hpp {
{
if (Nedges > 1) {
const std::size_t T = (pregrasp?1:0) + (intersec?1:0);
- WaypointEdgePtr_t we = boost::static_pointer_cast <WaypointEdge>
+ WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
(n.front()->linkTo (name + "_ls", n.back(), w,
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
@@ -193,7 +186,7 @@ namespace hpp {
} else {
assert (gCase == (GraspOnly | NoPlace)
&& "Cannot implement a LevelSetEdge for grasping");
- gls = boost::static_pointer_cast <LevelSetEdge>
+ gls = static_pointer_cast <LevelSetEdge>
(n.front()->linkTo (name + "_ls", n.back(), w,
LevelSetEdge::create));
return gls;
@@ -206,7 +199,7 @@ namespace hpp {
{
if (Nedges > 1) {
const std::size_t T = (pregrasp?1:0) + (intersec?1:0);
- WaypointEdgePtr_t we = boost::static_pointer_cast <WaypointEdge>
+ WaypointEdgePtr_t we = static_pointer_cast <WaypointEdge>
(n.back()->linkTo (name + "_ls", n.front(), w,
WaypointEdge::create));
we->nbWaypoints (nbWaypoints);
@@ -223,7 +216,7 @@ namespace hpp {
} else {
assert (gCase == (NoGrasp | PlaceOnly)
&& "Cannot implement a LevelSetEdge for placement");
- pls = boost::static_pointer_cast <LevelSetEdge>
+ pls = static_pointer_cast <LevelSetEdge>
(n.back()->linkTo (name + "_ls", n.front(), w,
LevelSetEdge::create));
return pls;
@@ -231,7 +224,7 @@ namespace hpp {
}
template <typename EdgeType>
- static inline boost::shared_ptr<EdgeType> linkWaypoint (
+ static inline shared_ptr<EdgeType> linkWaypoint (
const StateArray& states,
const std::size_t& iF, const std::size_t& iT,
const std::string& prefix,
@@ -240,7 +233,7 @@ namespace hpp {
std::stringstream ss;
ss << prefix << "_" << iF << iT;
if (suffix.length () > 0) ss << "_" << suffix;
- return boost::static_pointer_cast <EdgeType>
+ return static_pointer_cast <EdgeType>
(states[iF]->linkTo (ss.str(), states[iT], -1, EdgeType::create));
}
@@ -376,7 +369,7 @@ namespace hpp {
if (levelSetPlace)
weBackLs = T::makeLSEplace (name, n, eB, 10*wBack, pls);
- Edges_t ret = boost::assign::list_of (weForw)(weBack);
+ Edges_t ret { weForw, weBack };
if (levelSetPlace) {
if (!place.foliated ()) {
@@ -445,24 +438,17 @@ namespace hpp {
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
ImplicitPtr_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 ());
+ grasp.nc.push_back (gc);
+ grasp.nc_path.push_back (gc);
ImplicitPtr_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());
- }
+ if (gcc->function ().outputSize () > 0)
+ grasp.nc_fol.push_back (gcc);
const value_type c = handle->clearance () + gripper->clearance ();
ImplicitPtr_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);
- pregrasp.nc_path.pdof.push_back (segments_t());
+ pregrasp.nc.push_back (pgc);
+ pregrasp.nc_path.push_back (pgc);
}
void strictPlacementManifold (
@@ -471,19 +457,13 @@ namespace hpp {
const ImplicitPtr_t placementComplement,
FoliatedManifold& place, FoliatedManifold& preplace)
{
- place.nc.nc.push_back (placement);
- place.nc.pdof.push_back (segments_t());
- place.nc_path.nc.push_back (placement);
- place.nc_path.pdof.push_back (segments_t());
- if (placementComplement && placementComplement->function().outputSize () > 0) {
- place.nc_fol.nc.push_back (placementComplement);
- place.nc_fol.pdof.push_back (segments_t());
- }
+ place.nc.push_back (placement);
+ place.nc_path.push_back (placement);
+ if (placementComplement && placementComplement->function().outputSize () > 0)
+ place.nc_fol.push_back (placementComplement);
- preplace.nc.nc.push_back (preplacement);
- preplace.nc.pdof.push_back (segments_t());
- preplace.nc_path.nc.push_back (preplacement);
- preplace.nc_path.pdof.push_back (segments_t());
+ preplace.nc.push_back (preplacement);
+ preplace.nc_path.push_back (preplacement);
}
void relaxedPlacementManifold (
@@ -493,22 +473,18 @@ namespace hpp {
FoliatedManifold& place, FoliatedManifold& preplace)
{
if (placement) {
- place.nc.nc.push_back (placement);
- place.nc.pdof.push_back (segments_t());
+ place.nc.push_back (placement);
// The placement constraints are not required in the path, as long as
// they are satisfied at both ends and the object does not move. The
// former condition is ensured by the placement constraints on both
// ends and the latter is ensure by the LockedJoint constraints.
- place.nc_path.nc.push_back (placement);
- place.nc_path.pdof.push_back (segments_t());
+ place.nc_path.push_back (placement);
}
std::copy (objectLocks.begin(), objectLocks.end(), std::back_inserter(place.lj_fol));
if (placement && preplacement) {
- preplace.nc.nc.push_back (preplacement);
- preplace.nc.pdof.push_back (segments_t());
- // preplace.nc_path.nc.push_back (preplacement);
- // preplace.nc_path.pdof.push_back (segments_t());
+ preplace.nc.push_back (preplacement);
+ // preplace.nc_path.push_back (preplacement);
}
}
@@ -517,9 +493,7 @@ namespace hpp {
typedef std::vector <index_t> IndexV_t;
typedef std::list <index_t> IndexL_t;
typedef std::pair <index_t, index_t> Grasp_t;
- typedef boost::tuple <StatePtr_t,
- FoliatedManifold>
- StateAndManifold_t;
+ typedef std::tuple <StatePtr_t, FoliatedManifold> StateAndManifold_t;
//typedef std::vector <index_t, index_t> GraspV_t;
/// GraspV_t corresponds to a unique ID of a permutation.
/// - its size is the number of grippers,
@@ -565,7 +539,7 @@ namespace hpp {
}
};
std::tr1::unordered_set<edgeid_type, edgeid_hash> edges;
- std::vector< boost::array<ImplicitPtr_t,3> > graspCs;
+ std::vector< std::array<ImplicitPtr_t,3> > graspCs;
index_t nG, nOH;
GraspV_t dims;
const Grippers_t& gs;
@@ -578,11 +552,10 @@ namespace hpp {
ps (problem), graph (g), nG (grippers.size ()), nOH (0), gs (grippers), ohs (objects),
defaultAcceptationPolicy (CompiledRule::Refuse)
{
- BOOST_FOREACH (const Object_t& o, objects) {
- nOH += o.get<1>().size();
- BOOST_FOREACH (const HandlePtr_t& h, o.get<1>()) {
+ for (const Object_t& o : objects) {
+ nOH += std::get<1>(o).size();
+ for (const HandlePtr_t& h : std::get<1>(o))
handleNames.push_back(h->name());
- }
}
handleNames.push_back("");
dims.resize (nG);
@@ -647,10 +620,10 @@ namespace hpp {
edges.insert(edgeid_type(stateid(g1), stateid(g2)));
}
- inline boost::array<ImplicitPtr_t,3>& graspConstraint (
+ inline std::array<ImplicitPtr_t,3>& graspConstraint (
const index_t& iG, const index_t& iOH)
{
- boost::array<ImplicitPtr_t,3>& gcs =
+ std::array<ImplicitPtr_t,3>& gcs =
graspCs [iG * nOH + iOH];
if (!gcs[0]) {
hppDout (info, "Create grasps constraints for ("
@@ -674,18 +647,18 @@ namespace hpp {
const Object_t& object (const index_t& iOH) const {
index_t iH = iOH;
- BOOST_FOREACH (const Object_t& o, ohs) {
- if (iH < o.get<1>().size()) return o;
- iH -= o.get<1>().size();
+ for (const Object_t& o : ohs) {
+ if (iH < std::get<1>(o).size()) return o;
+ iH -= std::get<1>(o).size();
}
throw std::out_of_range ("Handle index");
}
const HandlePtr_t& handle (const index_t& iOH) const {
index_t iH = iOH;
- BOOST_FOREACH (const Object_t& o, ohs) {
- if (iH < o.get<1>().size()) return o.get<1>()[iH];
- iH -= o.get<1>().size();
+ for (const Object_t& o : ohs) {
+ if (iH < std::get<1>(o).size()) return std::get<1>(o)[iH];
+ iH -= std::get<1>(o).size();
}
throw std::out_of_range ("Handle index");
}
@@ -694,7 +667,7 @@ namespace hpp {
bool objectCanBePlaced (const Object_t& o) const
{
// If the object has no joint, then it cannot be placed.
- return (o.get<0>().get<2>().size() > 0);
+ return (std::get<2>(std::get<0>(o)).size() > 0);
}
/// Check is an object is grasped by the GraspV_t
@@ -704,7 +677,7 @@ namespace hpp {
assert (idxOH.size () == nG);
for (std::size_t i = 0; i < idxOH.size (); ++i)
if (idxOH[i] < nOH) // This grippers grasps an object
- if (o.get<2>() == object(idxOH[i]).get<2>())
+ if (std::get<2>(o) == std::get<2>(object(idxOH[i])))
return true;
return false;
}
@@ -753,22 +726,15 @@ namespace hpp {
void graspManifold (const index_t& iG, const index_t& iOH,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
- boost::array<ImplicitPtr_t,3>& gcs
+ std::array<ImplicitPtr_t,3>& gcs
= graspConstraint (iG, iOH);
- grasp.nc.nc.push_back (gcs[0]);
- grasp.nc.pdof.push_back (segments_t ());
- grasp.nc_path.nc.push_back (gcs[0]);
- // TODO: see function declaration
- grasp.nc_path.pdof.push_back (segments_t ());
- if (gcs[1]->function ().outputSize () > 0) {
- grasp.nc_fol.nc.push_back (gcs[1]);
- grasp.nc_fol.pdof.push_back (segments_t());
- }
+ grasp.nc.push_back (gcs[0]);
+ grasp.nc_path.push_back (gcs[0]);
+ if (gcs[1]->function ().outputSize () > 0)
+ grasp.nc_fol.push_back (gcs[1]);
- pregrasp.nc.nc.push_back (gcs[2]);
- pregrasp.nc.pdof.push_back (segments_t());
- pregrasp.nc_path.nc.push_back (gcs[2]);
- pregrasp.nc_path.pdof.push_back (segments_t());
+ pregrasp.nc.push_back (gcs[2]);
+ pregrasp.nc_path.push_back (gcs[2]);
}
};
@@ -791,9 +757,9 @@ namespace hpp {
const int priority)
{
StateAndManifold_t& nam = r (g);
- if (!nam.get<0>()) {
+ if (!std::get<0>(nam)) {
hppDout (info, "Creating state " << r.name (g));
- nam.get<0>() = r.graph->stateSelector ()->createState
+ std::get<0>(nam) = r.graph->stateSelector ()->createState
(r.name (g), false, priority);
// Loop over the grippers and create grasping constraints if required
FoliatedManifold unused;
@@ -801,32 +767,34 @@ namespace hpp {
for (index_t i = 0; i < r.nG; ++i) {
if (g[i] < r.nOH) {
idxsOH.insert (g[i]);
- r.graspManifold (i, g[i], nam.get<1>(), unused);
+ r.graspManifold (i, g[i], std::get<1>(nam), unused);
}
}
index_t iOH = 0;
- BOOST_FOREACH (const Object_t& o, r.ohs) {
+ for (const Object_t& o : r.ohs) {
if (!r.objectCanBePlaced(o)) continue;
bool oIsGrasped = false;
// TODO: use the fact that the set is sorted.
- // BOOST_FOREACH (const HandlePtr_t& h, o.get<1>())
- for (index_t i = 0; i < o.get<1>().size(); ++i) {
+ // for (const HandlePtr_t& h : std::get<0>(o))
+ for (index_t i = 0; i < std::get<1>(o).size(); ++i) {
if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
++iOH;
}
- if (!oIsGrasped)
- relaxedPlacementManifold (o.get<0>().get<0>(),
- o.get<0>().get<1>(),
- o.get<0>().get<2>(),
- nam.get<1>(), unused);
+ if (!oIsGrasped) {
+ const auto& pc (std::get<0>(o));
+ relaxedPlacementManifold (std::get<0>(pc),
+ std::get<1>(pc),
+ std::get<2>(pc),
+ std::get<1>(nam), unused);
+ }
}
- nam.get<1>().addToState (nam.get<0>());
+ std::get<1>(nam).addToState (std::get<0>(nam));
createLoopEdge (r.nameLoopEdge (g),
- nam.get<0>(), 0,
+ std::get<0>(nam), 0,
false,
- // TODO nam.get<1>().foliated(),
- nam.get<1>());
+ // TODO std::get<1>(nam).foliated(),
+ std::get<1>(nam));
}
return nam;
}
@@ -856,9 +824,10 @@ namespace hpp {
submanifold;
r.graspManifold (iG, gTo[iG], grasp, pregrasp);
if (!noPlace) {
- relaxedPlacementManifold (o.get<0>().get<0>(),
- o.get<0>().get<1>(),
- o.get<0>().get<2>(),
+ const auto& pc (std::get<0>(o));
+ relaxedPlacementManifold (std::get<0>(pc),
+ std::get<1>(pc),
+ std::get<2>(pc),
place, preplace);
}
std::pair<std::string, std::string> names =
@@ -873,30 +842,32 @@ namespace hpp {
}
}
index_t iOH = 0;
- BOOST_FOREACH (const Object_t& o, r.ohs) {
+ for (const Object_t& o : r.ohs) {
if (!r.objectCanBePlaced(o)) continue;
bool oIsGrasped = false;
const index_t iOHstart = iOH;
- for (; iOH < iOHstart + o.get<1>().size(); ++iOH) {
+ for (; iOH < iOHstart + std::get<1>(o).size(); ++iOH) {
if (iOH == gTo [iG]) {
oIsGrasped = true;
- iOH = iOHstart + o.get<1>().size();
+ iOH = iOHstart + std::get<1>(o).size();
break;
}
if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
}
- if (!oIsGrasped)
- relaxedPlacementManifold (o.get<0>().get<0>(),
- o.get<0>().get<1>(),
- o.get<0>().get<2>(),
+ if (!oIsGrasped) {
+ const auto& pc (std::get<0>(o));
+ relaxedPlacementManifold (std::get<0>(pc),
+ std::get<1>(pc),
+ std::get<2>(pc),
submanifold, unused);
+ }
}
}
if (pregrasp.empty ()) {
if (noPlace)
createEdges <GraspOnly | NoPlace> (
names.first , names.second,
- from.get<0> () , to.get<0>(),
+ std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
@@ -905,7 +876,7 @@ namespace hpp {
else if (preplace.empty ())
createEdges <GraspOnly | PlaceOnly> (
names.first , names.second,
- from.get<0> () , to.get<0>(),
+ std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
@@ -916,7 +887,7 @@ namespace hpp {
/*
createEdges <GraspOnly | WithPrePlace> (
names.first , names.second,
- from.get<0> () , to.get<0>(),
+ std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
@@ -927,7 +898,7 @@ namespace hpp {
if (noPlace)
createEdges <WithPreGrasp | NoPlace> (
names.first , names.second,
- from.get<0> () , to.get<0>(),
+ std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
@@ -936,7 +907,7 @@ namespace hpp {
else if (preplace.empty ())
createEdges <WithPreGrasp | PlaceOnly> (
names.first , names.second,
- from.get<0> () , to.get<0>(),
+ std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
@@ -945,7 +916,7 @@ namespace hpp {
else
createEdges <WithPreGrasp | WithPrePlace> (
names.first , names.second,
- from.get<0> () , to.get<0>(),
+ std::get<0>(from) , std::get<0>(to),
1 , 1,
grasp , pregrasp,
place , preplace,
@@ -969,7 +940,7 @@ namespace hpp {
for (IndexV_t::const_iterator itx_g = idx_g.begin ();
itx_g != idx_g.end (); ++itx_g) {
// Copy all element except itx_g
- std::copy (boost::next (itx_g), idx_g.end (),
+ std::copy (std::next (itx_g), idx_g.end (),
std::copy (idx_g.begin (), itx_g, nIdx_g.begin ())
);
for (IndexV_t::const_iterator itx_oh = idx_oh.begin ();
@@ -985,7 +956,7 @@ namespace hpp {
makeEdge (r, grasps, nGrasps, *itx_g, depth);
// Copy all element except itx_oh
- std::copy (boost::next (itx_oh), idx_oh.end (),
+ std::copy (std::next (itx_oh), idx_oh.end (),
std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
);
// Do all the possible combination below this new grasp
@@ -1037,7 +1008,7 @@ namespace hpp {
const pinocchio::Model& model = robot.model();
Grippers_t grippers (griNames.size());
index_t i = 0;
- BOOST_FOREACH (const std::string& gn, griNames) {
+ for (const std::string& gn : griNames) {
grippers[i] = robot.grippers.get (gn);
++i;
}
@@ -1045,12 +1016,12 @@ namespace hpp {
i = 0;
const value_type margin = 1e-3;
bool prePlace = (prePlaceWidth > 0);
- BOOST_FOREACH (const ObjectDef_t& od, objs) {
+ for (const ObjectDef_t& od : objs) {
// Create handles
- objects[i].get<2> () = i;
- objects[i].get<1> ().resize (od.handles.size());
- Handles_t::iterator it = objects[i].get<1> ().begin();
- BOOST_FOREACH (const std::string hn, od.handles) {
+ std::get<2>(objects[i]) = i;
+ std::get<1>(objects[i]).resize (od.handles.size());
+ Handles_t::iterator it = std::get<1>(objects[i]).begin();
+ for (const std::string hn : od.handles) {
*it = robot.handles.get (hn);
++it;
}
@@ -1064,30 +1035,27 @@ namespace hpp {
// else if contact surfaces are defined and selected
// create default placement constraint using the ProblemSolver
// methods createPlacementConstraint and createPrePlacementConstraint
+ auto& pc (std::get<0>(objects[i]));
if (ps->numericalConstraints.has(placeN)) {
- objects[i].get<0> ().get<0> () =
- ps->numericalConstraints.get (placeN);
+ std::get<0>(pc) = ps->numericalConstraints.get (placeN);
if (ps->numericalConstraints.has(preplaceN)) {
- objects[i].get<0> ().get<1> () =
- ps->numericalConstraints.get (preplaceN);
+ std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
}
} else if (!envNames.empty() && !od.shapes.empty ()) {
ps->createPlacementConstraint (placeN,
od.shapes, envNames, margin);
- objects[i].get<0> ().get<0> () =
- ps->numericalConstraints.get (placeN);
+ std::get<0>(pc) = ps->numericalConstraints.get (placeN);
if (prePlace) {
ps->createPrePlacementConstraint (preplaceN,
od.shapes, envNames, margin, prePlaceWidth);
- objects[i].get<0> ().get<1> () =
- ps->numericalConstraints.get (preplaceN);
+ std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
}
}
// Create object lock
// Loop over all frames of object, detect joint and create locked
// joint.
assert (robot.robotFrames (od.name).size () != 0);
- BOOST_FOREACH (const FrameIndex& f, robot.robotFrames (od.name)) {
+ for (const FrameIndex& f : robot.robotFrames (od.name)) {
if (model.frames[f].type != ::pinocchio::JOINT) continue;
const JointIndex j = model.frames[f].parent;
JointPtr_t oj (Joint::create (ps->robot(), j));
@@ -1097,7 +1065,7 @@ namespace hpp {
oj->configSize ()), space);
LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
ps->numericalConstraints.add ("lock_" + oj->name (), lj);
- objects[i].get<0> ().get<2> ().push_back (lj);
+ std::get<2>(pc).push_back (lj);
}
++i;
}
diff --git a/src/handle.cc b/src/handle.cc
index 36c896e475601f7f4aecfbe615a3b8e9532b19d6..7a5db0bbc6b9e7e7a7a583e722397cdb0b5f8534 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -19,8 +19,6 @@
#include <hpp/manipulation/handle.hh>
-#include <boost/assign/list_of.hpp>
-
#include <pinocchio/multibody/joint/joint-generic.hpp>
#include <hpp/util/debug.hh>
@@ -29,7 +27,6 @@
#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/pinocchio/gripper.hh>
-#include <hpp/constraints/generic-transformation.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/constraints/explicit/relative-pose.hh>
#include <hpp/constraints/generic-transformation.hh>
@@ -149,7 +146,7 @@ namespace hpp {
core::DevicePtr_t r = robot();
if (is6Dmask(mask_)) {
return Implicit::create (
- boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
+ shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
r->configSize(), r->numberDof (), n)), ComparisonTypes_t());
} else {
std::vector<bool> Cmask = complementMask(mask_);
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 473359cf43dbb0024a9cb7c71182214ad8cfebd1..fab445ad35f58aa3f4a43f34da2dfd14df85db7e 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -16,8 +16,8 @@
#include "hpp/manipulation/manipulation-planner.hh"
-#include <boost/tuple/tuple.hpp>
-#include <boost/assign/list_of.hpp>
+#include <tuple>
+#include <iterator>
#include <hpp/util/pointer.hh>
#include <hpp/util/timer.hh>
@@ -99,15 +99,16 @@ namespace hpp {
}
const std::vector<ManipulationPlanner::Reason>
- ManipulationPlanner::reasons_ = boost::assign::list_of
- (SuccessBin::createReason ("--Path could not be fully projected")) // PATH_PROJECTION_SHORTER = 0,
- (SuccessBin::createReason ("--Path could not be fully validated")) // PATH_VALIDATION_SHORTER = 1,
- (SuccessBin::createReason ("--Reached destination node")) // REACHED_DESTINATION_NODE = 2,
- (SuccessBin::createReason ("Failure")) // FAILURE = 3,
- (SuccessBin::createReason ("--Projection of configuration on edge leaf")) // PROJECTION = 4,
- (SuccessBin::createReason ("--SteeringMethod")) // STEERING_METHOD = 5,
- (SuccessBin::createReason ("--Path validation returned length 0")) // PATH_VALIDATION_ZERO = 6,
- (SuccessBin::createReason ("--Path could not be projected at all")); // PATH_PROJECTION_ZERO = 7
+ ManipulationPlanner::reasons_ = {
+ SuccessBin::createReason("--Path could not be fully projected"), // PATH_PROJECTION_SHORTER = 0,
+ SuccessBin::createReason("--Path could not be fully validated"), // PATH_VALIDATION_SHORTER = 1,
+ SuccessBin::createReason("--Reached destination node"), // REACHED_DESTINATION_NODE = 2,
+ SuccessBin::createReason("Failure"), // FAILURE = 3,
+ SuccessBin::createReason("--Projection of configuration on edge leaf"), // PROJECTION = 4,
+ SuccessBin::createReason("--SteeringMethod"), // STEERING_METHOD = 5,
+ SuccessBin::createReason("--Path validation returned length 0"), // PATH_VALIDATION_ZERO = 6,
+ SuccessBin::createReason("--Path could not be projected at all"), // PATH_PROJECTION_ZERO = 7
+ };
ManipulationPlannerPtr_t ManipulationPlanner::create
(const core::ProblemConstPtr_t& problem,
@@ -168,7 +169,7 @@ namespace hpp {
core::Nodes_t newNodes;
core::PathPtr_t path;
- typedef boost::tuple <core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
+ typedef std::tuple <core::NodePtr_t, ConfigurationPtr_t, core::PathPtr_t>
DelayedEdge_t;
typedef std::vector <DelayedEdge_t> DelayedEdges_t;
DelayedEdges_t delayedEdges;
@@ -212,11 +213,10 @@ namespace hpp {
HPP_START_TIMECOUNTER(delayedEdges);
// Insert delayed edges
- for (DelayedEdges_t::const_iterator itEdge = delayedEdges.begin ();
- itEdge != delayedEdges.end (); ++itEdge) {
- const core::NodePtr_t& near = itEdge-> get <0> ();
- const ConfigurationPtr_t& q_new = itEdge-> get <1> ();
- const core::PathPtr_t& validPath = itEdge-> get <2> ();
+ for (const auto& edge : delayedEdges) {
+ const core::NodePtr_t& near = std::get<0>(edge);
+ const ConfigurationPtr_t& q_new = std::get<1>(edge);
+ const core::PathPtr_t& validPath = std::get<2>(edge);
core::NodePtr_t newNode = roadmap ()->addNode (q_new);
roadmap ()->addEdge (near, newNode, validPath);
roadmap ()->addEdge (newNode, near, validPath->reverse());
@@ -442,7 +442,7 @@ namespace hpp {
const Configuration_t& q1 (*(*itn1)->configuration ());
graph::StatePtr_t s1 = getState (graph, *itn1);
- for (core::Nodes_t::const_iterator itn2 = boost::next (itn1);
+ for (core::Nodes_t::const_iterator itn2 = std::next (itn1);
itn2 != nodes.end (); ++itn2) {
if ((*itn1)->connectedComponent () == (*itn2)->connectedComponent ())
continue;
diff --git a/src/path-planner/end-effector-trajectory.cc b/src/path-planner/end-effector-trajectory.cc
index a898f5f76306dd31b09c41447ed4b53bf5e13be1..7a486aa2c26a1b914fdfa553f95fd435fd85c35a 100644
--- a/src/path-planner/end-effector-trajectory.cc
+++ b/src/path-planner/end-effector-trajectory.cc
@@ -208,10 +208,10 @@ namespace hpp {
continue;
}
- roadmap()->initNode (boost::make_shared<Configuration_t>(steps.col(0)));
+ roadmap()->initNode (make_shared<Configuration_t>(steps.col(0)));
core::NodePtr_t init = roadmap()-> initNode ();
core::NodePtr_t goal = roadmap()->addGoalNode (
- boost::make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
+ make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
roadmap()->addEdge (init, goal, path);
success = true;
if (feasibilityOnly_) break;
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index fadf04c003f7749cfdfdf48d832ccac0874cb16c..b7bc3f03d4ff41d49d40071eb5b740b0c9da9f3f 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -17,8 +17,6 @@
#include "hpp/manipulation/problem-solver.hh"
-#include <boost/bind.hpp>
-
#include <hpp/util/pointer.hh>
#include <hpp/util/debug.hh>
@@ -93,7 +91,7 @@ namespace hpp {
core::SteeringMethodPtr_t createSMWithGuess
(const core::ProblemConstPtr_t& problem)
{
- boost::shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem);
+ shared_ptr<ParentSM_t> sm = ParentSM_t::create (problem);
sm->innerSteeringMethod (ChildSM_t::createWithGuess (problem));
return sm;
}
@@ -158,7 +156,7 @@ namespace hpp {
createPathProjector <core::pathProjector::RecursiveHermite>);
steeringMethods.add ("Graph-SteeringMethodStraight",
- steeringMethod::Graph::create <core::SteeringMethodStraight>);
+ steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Straight",
steeringMethod::Graph::create <core::steeringMethod::Straight>);
steeringMethods.add ("Graph-Hermite",
@@ -272,27 +270,27 @@ namespace hpp {
(Constraint_t::createConstraintAndComplement
(name, robot_, floorSurfaces, objectSurfaces, margin));
- addNumericalConstraint(boost::get<0>(constraints)->function().name(),
- boost::get<0>(constraints));
- addNumericalConstraint(boost::get<1>(constraints)->function().name(),
- boost::get<1>(constraints));
- addNumericalConstraint(boost::get<2>(constraints)->function().name(),
- boost::get<2>(constraints));
+ addNumericalConstraint(std::get<0>(constraints)->function().name(),
+ std::get<0>(constraints));
+ addNumericalConstraint(std::get<1>(constraints)->function().name(),
+ std::get<1>(constraints));
+ addNumericalConstraint(std::get<2>(constraints)->function().name(),
+ std::get<2>(constraints));
// Set security margin to contact constraint
assert(HPP_DYNAMIC_PTR_CAST(constraints::ConvexShapeContact,
- boost::get<0>(constraints)->functionPtr()));
+ std::get<0>(constraints)->functionPtr()));
constraints::ConvexShapeContactPtr_t contactFunction
(HPP_STATIC_PTR_CAST(constraints::ConvexShapeContact,
- boost::get<0>(constraints)->functionPtr()));
+ std::get<0>(constraints)->functionPtr()));
contactFunction->setNormalMargin(margin);
constraintsAndComplements.push_back (
- ConstraintAndComplement_t (boost::get<0>(constraints),
- boost::get<1>(constraints),
- boost::get<2>(constraints)));
+ ConstraintAndComplement_t (std::get<0>(constraints),
+ std::get<1>(constraints),
+ std::get<2>(constraints)));
if (constraintGraph ())
- constraintGraph ()->registerConstraints(boost::get<0>(constraints),
- boost::get<1>(constraints),
- boost::get<2>(constraints));
+ constraintGraph ()->registerConstraints(std::get<0>(constraints),
+ std::get<1>(constraints),
+ std::get<2>(constraints));
}
void ProblemSolver::createPrePlacementConstraint
diff --git a/src/problem.cc b/src/problem.cc
index 1a3cf39715df6d78523bee4cbab1fd9f276e46d9..a0cdc35ad8871ed6010f521fa0dd431e6e09cdf5 100644
--- a/src/problem.cc
+++ b/src/problem.cc
@@ -75,7 +75,7 @@ namespace hpp {
{
PathValidationPtr_t pv (pvFactory_ (robot(), pvTol_));
- boost::shared_ptr<core::ObstacleUserInterface> oui =
+ shared_ptr<core::ObstacleUserInterface> oui =
HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pv);
if (oui) {
const core::ObjectStdVector_t& obstacles (collisionObstacles ());
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index e6b3d2e52c0f259afc182fbcaf61043f44703926..3d43520341ff463d115ec2dd480c9b3a7c907cf9 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -21,8 +21,6 @@
#include <queue>
#include <vector>
-#include <boost/bind.hpp>
-
#include <hpp/util/exception-factory.hh>
#include <pinocchio/multibody/model.hpp>
diff --git a/src/steering-method/graph.cc b/src/steering-method/graph.cc
index f7d778ca52a3917bb0d78da32c0b5f90b451308c..48cefdf7732044931a6db903999a3da0f67190b3 100644
--- a/src/steering-method/graph.cc
+++ b/src/steering-method/graph.cc
@@ -29,7 +29,7 @@ namespace hpp {
namespace manipulation {
SteeringMethod::SteeringMethod (const ProblemConstPtr_t& problem) :
core::SteeringMethod (problem), problem_ (problem),
- steeringMethod_ (core::SteeringMethodStraight::create (problem))
+ steeringMethod_ (core::steeringMethod::Straight::create (problem))
{
}