diff --git a/include/hpp/manipulation/axial-handle.hh b/include/hpp/manipulation/axial-handle.hh
index 9ea966fd85ff41db76ae4a47a0a0ec56fcd7507c..77dbb4d51fe9e9480815b6505da7040b5370987b 100644
--- a/include/hpp/manipulation/axial-handle.hh
+++ b/include/hpp/manipulation/axial-handle.hh
@@ -74,18 +74,6 @@ namespace hpp {
virtual NumericalConstraintPtr_t createPreGrasp
(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.
- /// \param gripper object containing the gripper information
- /// \param shift the target value along the x-axis
- /// \param width width of the interval of freedom of gripper along x-axis.
- /// \return the constraint of relative position between the handle and
- /// the gripper.
- /// \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, std::string name) const;
-
virtual std::ostream& print (std::ostream& os) const;
protected:
/// Constructor
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 2a80e4cd801dfdeb32d462d149861b084b6fb42a..a7432aa41d7c7bf69bc528785dddc0a5bcb60610 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -106,10 +106,7 @@ namespace hpp {
typedef core::LockedJointPtr_t LockedJointPtr_t;
typedef core::NumericalConstraint NumericalConstraint;
typedef core::NumericalConstraintPtr_t NumericalConstraintPtr_t;
- typedef core::ComparisonTypesPtr_t ComparisonTypesPtr_t;
- typedef core::EqualToZero EqualToZero;
- typedef core::Equality Equality;
- typedef core::ComparisonTypes ComparisonTypes;
+ typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
HPP_PREDEF_CLASS (ConstraintSet);
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 1c6be4ed7bfd3531cca864ca71c93ff117af9f2d..641a69bb401c8fa530d241b04894d3790a974d7d 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -308,7 +308,7 @@ namespace hpp {
void insertParamConstraint (const NumericalConstraintPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
- void insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
+ void insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypes_t ineq)
HPP_MANIPULATION_DEPRECATED;
/// Insert a LockedJoint that parametrizes the foliation
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index 6ec846d6ffa21e68f2d9701f8042b1284cf2efb5..425bd85d2149e2ce7c38dcb95fd78866737a8a26 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -48,15 +48,6 @@ namespace hpp {
typedef ::hpp::statistics::DiscreteDistribution< EdgePtr_t > Neighbors_t;
typedef std::vector < StateSelectorPtr_t > StateSelectors_t;
- typedef hpp::core::Constraint Constraint;
- typedef hpp::core::ConstraintPtr_t ConstraintPtr_t;
- typedef hpp::core::LockedJoint LockedJoint;
- typedef hpp::core::LockedJointPtr_t LockedJointPtr_t;
- typedef hpp::core::ConfigProjector ConfigProjector;
- typedef hpp::core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
- typedef hpp::core::Equality Equality;
- typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
- typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
typedef hpp::core::segments_t segments_t;
typedef std::vector <segments_t> IntervalsContainer_t;
typedef hpp::core::NumericalConstraints_t NumericalConstraints_t;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index b5479a568fe32cc6948765e3f1330ddaef4772a8..0c4e769686cb3b3a941e5a1ec3037897689656e5 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -61,7 +61,7 @@ namespace hpp {
/// Add core::DifferentiableFunction to the component.
virtual void addNumericalConstraint
- (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
+ (const DifferentiableFunctionPtr_t& function, const ComparisonTypes_t& ineq)
HPP_MANIPULATION_DEPRECATED;
/// Reset the numerical constraints stored in the component.
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 66b54fcfd1d381922bdb252b699b7d471b41943a..2e00d5e2052eaabc1742d11752f14fc58961d8d9 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -120,7 +120,7 @@ namespace hpp {
}
/// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
+ virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypes_t& ineq)
HPP_MANIPULATION_DEPRECATED
{
isInit_ = false;
diff --git a/src/axial-handle.cc b/src/axial-handle.cc
index 5446451cdb51f11e3fbb3e31f5e2544144778e7e..8dcaae9d773241e393c03bf01c12a6f702a7cb7f 100644
--- a/src/axial-handle.cc
+++ b/src/axial-handle.cc
@@ -67,14 +67,12 @@ namespace hpp {
(true);
if (n.empty())
n = "Transformation_(0,0,0,0,0,1)_" + name() + "_" + gripper->name();
- return NumericalConstraintPtr_t
- (NumericalConstraint::create (RelativeTransformation::create
- (n,
- gripper->joint()->robot(),
- gripper->joint (), joint (),
- gripper->objectPositionInJoint (),
- localPosition(), mask),
- core::Equality::create ()));
+ return NumericalConstraint::create (RelativeTransformation::create
+ (n, gripper->joint()->robot(),
+ gripper->joint (), joint (),
+ gripper->objectPositionInJoint (),
+ localPosition(), mask),
+ ComparisonTypes_t (1, constraints::Equality));
}
NumericalConstraintPtr_t AxialHandle::createPreGrasp
@@ -95,33 +93,6 @@ namespace hpp {
localPosition(), mask)));
}
- NumericalConstraintPtr_t AxialHandle::createPreGraspComplement
- (const GripperPtr_t& gripper, const value_type& shift,
- const value_type& width, std::string n) const
- {
- using boost::assign::list_of;
- using core::DoubleInequality;
- std::vector <bool> mask = list_of (true)(false)(false)(false)(false)
- (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
- (n,
- gripper->joint()->robot(),
- gripper->joint (), joint (),
- transform, localPosition(), mask),
- DoubleInequality::create (width)));
- }
-
- NumericalConstraintPtr_t AxialHandle::createGraspAndComplement
- (const GripperPtr_t& gripper, std::string n) const
- {
- return Handle::createGraspAndComplement (gripper, n);
- }
-
HandlePtr_t AxialHandle::clone () const
{
AxialHandlePtr_t self = weakPtr_.lock ();
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 31ee35142167db40899715d5aaef75c8f42e3cb2..df1d0b2102889c8287fc3c02f1c36877e91c0bc8 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -636,6 +636,12 @@ namespace hpp {
ConstraintSetPtr_t param = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
+
+ for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
+ it != paramLockedJoints_.end (); ++it) {
+ proj->add (*it);
+ }
+
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
@@ -647,11 +653,6 @@ namespace hpp {
param->addConstraint (proj);
param->edge (wkPtr_.lock ());
- for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
- it != paramLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
-
f.parametrizer (param);
// The codition
@@ -662,6 +663,12 @@ namespace hpp {
// edge.
ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
+
+ for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
+ it != condLockedJoints_.end (); ++it) {
+ proj->add (*it);
+ }
+
itpdof = condPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
it != condNumericalConstraints_.end (); ++it) {
@@ -669,10 +676,7 @@ namespace hpp {
++itpdof;
}
assert (itpdof == condPassiveDofs_.end ());
- for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
- it != condLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
+
f.condition (cond);
cond->addConstraint (proj);
@@ -702,6 +706,18 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+
+ g->insertLockedJoints (proj);
+ for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
+ it != paramLockedJoints_.end (); ++it) {
+ proj->add (*it);
+ }
+ insertLockedJoints (proj);
+ to ()->insertLockedJoints (proj);
+ if (state () != to ()) {
+ state ()->insertLockedJoints (proj);
+ }
+
g->insertNumericalConstraints (proj);
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
@@ -718,17 +734,6 @@ namespace hpp {
}
constraint->addConstraint (proj);
- g->insertLockedJoints (proj);
- for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
- it != paramLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
- insertLockedJoints (proj);
- to ()->insertLockedJoints (proj);
- if (state () != to ()) {
- state ()->insertLockedJoints (proj);
- }
-
constraint->edge (wkPtr_.lock ());
return constraint;
}
@@ -741,7 +746,7 @@ namespace hpp {
paramPassiveDofs_.push_back (passiveDofs);
}
- void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
+ void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypes_t ineq)
{
isInit_ = false;
insertParamConstraint (NumericalConstraint::create (function, ineq));
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index e0ffc977dd9799399094079fd96da50b2f1faa78..b53bfd0a08b04fac7e4c91f5ff75c3c31526f139 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -53,7 +53,7 @@ namespace hpp {
passiveDofs_.push_back (passiveDofs);
}
- void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
+ void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypes_t& ineq)
{
addNumericalConstraint (NumericalConstraint::create (function,ineq));
}
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 6c78d18378382d209b24257740f5c048788412e0..cbe0f3ce7339539efda66a8e196e185ae6e03470 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -975,12 +975,12 @@ namespace hpp {
const IndexV_t& idx_g, const IndexV_t& idx_oh,
const GraspV_t& grasps, const int depth)
{
- if (idx_g.empty () || idx_oh.empty ()) return;
- IndexV_t nIdx_g (idx_g.size() - 1);
- IndexV_t nIdx_oh (idx_oh.size() - 1);
bool curGraspIsAllowed = r.graspIsAllowed(grasps);
if (curGraspIsAllowed) makeState (r, grasps, depth);
+ if (idx_g.empty () || idx_oh.empty ()) return;
+ IndexV_t nIdx_g (idx_g.size() - 1);
+ IndexV_t nIdx_oh (idx_oh.size() - 1);
for (IndexV_t::const_iterator itx_g = idx_g.begin ();
itx_g != idx_g.end (); ++itx_g) {
// Copy all element except itx_g
diff --git a/src/handle.cc b/src/handle.cc
index 797db73157630daeb80791c0819ec04bce607aaf..8f7c24e334b6987cf5a95c330636f38dabb13c22 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -148,11 +148,8 @@ namespace hpp {
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
localPosition(), Cmask);
- return NumericalConstraintPtr_t (NumericalConstraint::create
- (function,
- core::ComparisonTypes::create(function->outputSize(),
- core::ComparisonType::Equality)
- ));
+ return NumericalConstraint::create (function,
+ ComparisonTypes_t(function->outputSize(), constraints::Equality));
}
}
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index fd688e57a01d0f21a1502326c6cf723f349d4fe7..2a5da2a7515cfdb4432dde376e64cad0264b044e 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -35,7 +35,6 @@
#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>
#include "hpp/manipulation/package-config.hh" // HPP_MANIPULATION_HAS_WHOLEBODY_STEP
@@ -245,9 +244,9 @@ namespace hpp {
(constraints.first));
addNumericalConstraint (complementName, NumericalConstraint::create
(constraints.second,
- core::ComparisonTypes::create
+ ComparisonTypes_t
(constraints.second->outputSize(),
- core::ComparisonType::Equality))
+ constraints::Equality))
);
}