diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 2f2468bdd2c9903756ba7f300ce804df4e1012fb..cde31f153348029a841c0dd8ace02d2bf96847fd 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -102,12 +102,8 @@ namespace hpp {
typedef std::vector <ObjectPtr_t> Objects_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
- typedef core::ExplicitNumericalConstraintPtr_t
- ExplicitNumericalConstraintPtr_t;
- typedef core::LockedJoint LockedJoint;
- typedef core::LockedJointPtr_t LockedJointPtr_t;
- typedef core::NumericalConstraint NumericalConstraint;
- typedef core::NumericalConstraintPtr_t NumericalConstraintPtr_t;
+ typedef constraints::LockedJoint LockedJoint;
+ typedef constraints::LockedJointPtr_t LockedJointPtr_t;
typedef hpp::core::ComparisonTypes_t ComparisonTypes_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 1dd384bf192b433330081bd28ed0020156860f51..4b0e22077fda7f371044f38653abb932499f5a75 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -28,6 +28,7 @@
namespace hpp {
namespace manipulation {
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
/// \addtogroup constraint_graph
/// \{
@@ -307,8 +308,8 @@ namespace hpp {
/// \name Foliation definition
/// \{
- /// Insert a NumericalConstraint that parametrizes the foliation
- void insertParamConstraint (const NumericalConstraintPtr_t& nm,
+ /// Insert a numerical constraint that parametrizes the foliation
+ void insertParamConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
void insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypes_t ineq)
@@ -317,17 +318,11 @@ namespace hpp {
/// Insert a LockedJoint that parametrizes the foliation
void insertParamConstraint (const LockedJointPtr_t lockedJoint);
- /// Insert a NumericalConstraint that defines the foliation
- ///
- /// The manifold represented the foliation is defined by this
- /// constraints.
- void insertConditionConstraint (const NumericalConstraintPtr_t& nm,
+ /// Insert a numerical constraint that defines the foliation
+ void insertConditionConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
/// Insert a LockedJoint that defines the foliation
- ///
- /// The manifold represented the foliation is defined by this
- /// constraints.
void insertConditionConstraint (const LockedJointPtr_t lockedJoint);
/// \}
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 9007b9e4ae53800fc28564411a6a8e1c52722d78..493164a33465cde122f4184f9a4c19cc216f27b3 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -29,6 +29,7 @@
namespace hpp {
namespace manipulation {
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
HPP_MAKE_EXCEPTION ( HPP_MANIPULATION_DLLAPI, Bad_function_call );
@@ -52,11 +53,11 @@ namespace hpp {
return id_;
}
- /// Add core::NumericalConstraint to the component.
+ /// Add Implicit to the component.
/// \param passiveDofs see ConfigProjector::addNumericalConstraint
// for more information.
virtual void addNumericalConstraint (
- const NumericalConstraintPtr_t& numConstraint,
+ const ImplicitPtr_t& numConstraint,
const segments_t& passiveDofs = segments_t ());
/// Add core::DifferentiableFunction to the component.
@@ -75,7 +76,7 @@ namespace hpp {
virtual void resetLockedJoints ();
/// Insert the numerical constraints in a ConfigProjector
- /// \return true is at least one NumericalConstraintPtr_t was inserted.
+ /// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
/// Insert the LockedJoint constraints in a ConstraintSet
diff --git a/include/hpp/manipulation/graph/graph.hh b/include/hpp/manipulation/graph/graph.hh
index c432ccfee926b2db8547915850c0373576e95802..4848798ccad30c229a4d7f7469fd05f4069077b4 100644
--- a/include/hpp/manipulation/graph/graph.hh
+++ b/include/hpp/manipulation/graph/graph.hh
@@ -25,6 +25,7 @@
namespace hpp {
namespace manipulation {
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
/// \addtogroup constraint_graph
/// \{
@@ -118,9 +119,9 @@ namespace hpp {
/// priorly to graph construction makes possible to replace
/// the constraint and its complement by the combination of
/// both that is an explicit constraint.
- void registerConstraints (const NumericalConstraintPtr_t& constraint,
- const NumericalConstraintPtr_t& complement,
- const NumericalConstraintPtr_t& both);
+ void registerConstraints (const ImplicitPtr_t& constraint,
+ const ImplicitPtr_t& complement,
+ const ImplicitPtr_t& both);
/// Test whether two constraints are complement of one another
///
@@ -133,9 +134,9 @@ namespace hpp {
/// combined they constitute a complement relative transformation
/// constraint. \sa Graph::registerConstraints
/// \warning argument order matters.
- bool isComplement (const NumericalConstraintPtr_t& constraint,
- const NumericalConstraintPtr_t& complement,
- NumericalConstraintPtr_t& combinationOfBoth) const;
+ bool isComplement (const ImplicitPtr_t& constraint,
+ const ImplicitPtr_t& complement,
+ ImplicitPtr_t& combinationOfBoth) const;
/// Constraint to project onto the Node.
/// \param state the state on which to project.
@@ -316,12 +317,12 @@ namespace hpp {
size_type maxIterations_;
struct ConstraintAndComplement_t {
- NumericalConstraintPtr_t constraint;
- NumericalConstraintPtr_t complement;
- NumericalConstraintPtr_t both;
- ConstraintAndComplement_t (const NumericalConstraintPtr_t& constr,
- const NumericalConstraintPtr_t& comp,
- const NumericalConstraintPtr_t& b) :
+ ImplicitPtr_t constraint;
+ ImplicitPtr_t complement;
+ ImplicitPtr_t both;
+ ConstraintAndComplement_t (const ImplicitPtr_t& constr,
+ const ImplicitPtr_t& comp,
+ const ImplicitPtr_t& b) :
constraint (constr), complement (comp), both (b)
{
}
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index ce733e56f1e543b24518cea6a8c20f031f15fe8a..381c57fae288aab69ca6fc4e67dff22c80fa31a4 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -28,6 +28,7 @@
namespace hpp {
namespace manipulation {
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
namespace helper {
/// \defgroup helper Helpers to build the graph of constraints
@@ -150,22 +151,22 @@ namespace hpp {
/// The placement foliation constraint is built using
/// hpp::constraints::ConvexShapeMatcherComplement
void strictPlacementManifold (
- const NumericalConstraintPtr_t placement,
- const NumericalConstraintPtr_t preplacement,
- const NumericalConstraintPtr_t placementComplement,
+ const ImplicitPtr_t placement,
+ const ImplicitPtr_t preplacement,
+ const ImplicitPtr_t placementComplement,
FoliatedManifold& place, FoliatedManifold& preplace);
/// The placement foliation constraint is built locked joints
/// It is faster than strictPlacementManifold but the foliation
/// parametrisation is redundant.
void relaxedPlacementManifold (
- const NumericalConstraintPtr_t placement,
- const NumericalConstraintPtr_t preplacement,
+ const ImplicitPtr_t placement,
+ const ImplicitPtr_t preplacement,
const LockedJoints_t objectLocks,
FoliatedManifold& place, FoliatedManifold& preplace);
- typedef boost::tuple <NumericalConstraintPtr_t,
- NumericalConstraintPtr_t,
+ typedef boost::tuple <ImplicitPtr_t,
+ ImplicitPtr_t,
LockedJoints_t>
PlacementConstraint_t;
typedef std::vector <HandlePtr_t> Handles_t;
diff --git a/include/hpp/manipulation/graph/state-selector.hh b/include/hpp/manipulation/graph/state-selector.hh
index d5a88d53f550c4d2101ba2d76584885c37fa3fea..1a6e101a71512e5f8a69faf1261ace326dc5a0be 100644
--- a/include/hpp/manipulation/graph/state-selector.hh
+++ b/include/hpp/manipulation/graph/state-selector.hh
@@ -52,8 +52,7 @@ namespace hpp {
virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
/// Should never be called.
- void addNumericalConstraint (
- const core::NumericalConstraintPtr_t& /* function */,
+ void addNumericalConstraint (const constraints::ImplicitPtr_t& /* function */,
const segments_t& /* passiveDofs */ = segments_t ())
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
@@ -61,7 +60,7 @@ namespace hpp {
/// Should never be called.
void addLockedJointConstraint
- (const core::LockedJoint& /* constraint */)
+ (const constraints::LockedJoint& /* constraint */)
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
}
diff --git a/include/hpp/manipulation/graph/state.hh b/include/hpp/manipulation/graph/state.hh
index 2e00d5e2052eaabc1742d11752f14fc58961d8d9..78ef08c746ff088c394450bfb23e22980a1aed0f 100644
--- a/include/hpp/manipulation/graph/state.hh
+++ b/include/hpp/manipulation/graph/state.hh
@@ -19,10 +19,9 @@
# include <boost/function.hpp>
-#include <hpp/core/locked-joint.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
-#include <hpp/core/numerical-constraint.hh>
+#include <hpp/constraints/implicit.hh>
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/deprecated.hh"
@@ -33,6 +32,8 @@
namespace hpp {
namespace manipulation {
+ using constraints::Implicit;
+ using constraints::ImplicitPtr_t;
namespace graph {
/// \addtogroup constraint_graph
/// \{
@@ -110,8 +111,8 @@ namespace hpp {
return configConstraints_;
}
- /// Add core::NumericalConstraint to the component.
- virtual void addNumericalConstraintForPath (const NumericalConstraintPtr_t& nm,
+ /// Add constraints::Implicit to the component.
+ virtual void addNumericalConstraintForPath (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ())
{
isInit_ = false;
@@ -124,11 +125,12 @@ namespace hpp {
HPP_MANIPULATION_DEPRECATED
{
isInit_ = false;
- numericalConstraintsForPath_.push_back (NumericalConstraint::create (function,ineq));
+ numericalConstraintsForPath_.push_back
+ (Implicit::create (function,ineq));
}
/// Insert the numerical constraints in a ConfigProjector
- /// \return true is at least one NumericalConstraintPtr_t was inserted.
+ /// \return true is at least one ImplicitPtr_t was inserted.
bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
{
assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
diff --git a/include/hpp/manipulation/handle.hh b/include/hpp/manipulation/handle.hh
index 3a2f621fbd9c90151ad5e9897fd5c1d8b7435c22..d53869b1b9fa4481c0a7d70b5c1a357ed9977a98 100644
--- a/include/hpp/manipulation/handle.hh
+++ b/include/hpp/manipulation/handle.hh
@@ -27,6 +27,7 @@
namespace hpp {
namespace manipulation {
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
/// Part of an object that is aimed at being grasped
class HPP_MANIPULATION_DLLAPI Handle
{
@@ -100,9 +101,10 @@ namespace hpp {
/// \return the constraint of relative transformation between the handle
/// and the gripper.
/// The degrees of freedom of the relative transformation that are
- /// constrained are determined by the mask. \sa NumericalConstraint::mask.
+ /// constrained are determined by the mask.
+ /// \sa constraints::Implicit::mask.
/// The constraint is not parameterizable (has constant right hand side).
- virtual NumericalConstraintPtr_t createGrasp
+ virtual ImplicitPtr_t createGrasp
(const GripperPtr_t& gripper, std::string name) const;
/// Create complement constraint of gripper grasping this handle
@@ -111,14 +113,14 @@ namespace hpp {
/// constitute a full relative transformation constraint.
/// The complement constraint is parameterizable (has non constant right
/// hand side).
- virtual NumericalConstraintPtr_t createGraspComplement
+ virtual ImplicitPtr_t createGraspComplement
(const GripperPtr_t& gripper, std::string name) const;
/// Create constraint composed of grasp constraint and its complement
/// \param gripper object containing the gripper information
/// \return the composition of grasp constraint and its complement, that
/// that is a full relative transformation constraint.
- virtual NumericalConstraintPtr_t createGraspAndComplement
+ virtual ImplicitPtr_t createGraspAndComplement
(const GripperPtr_t& gripper, std::string name) const;
/// Create constraint corresponding to a pregrasping task.
@@ -128,7 +130,7 @@ namespace hpp {
/// \note 6 DOFs of the relative transformation between the handle and the gripper
/// are constrained. The transformation is shifted along x-axis of
/// value shift.
- virtual NumericalConstraintPtr_t createPreGrasp
+ virtual ImplicitPtr_t createPreGrasp
(const GripperPtr_t& gripper, const value_type& shift, std::string name) const;
/// Get the clearance
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 5b5575354bfedb73fb5592967cb5098d8b61d112..53d3b3d52b7a9d40918102e531e87f03e944497a 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -26,6 +26,7 @@
#include <hpp/core/path-validation.hh>
#include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/locked-joint.hh>
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/problem.hh"
@@ -212,7 +213,7 @@ namespace hpp {
for (itnc1 = nc.begin(), itpdof1 = pdof.begin(); itnc1 != nc.end(); ++itnc1, ++itpdof1) {
itnc2 = itnc1; ++itnc2;
itpdof2 = itpdof1; ++itpdof2;
- NumericalConstraintPtr_t combination;
+ constraints::ImplicitPtr_t combination;
while (itnc2 != nc.end()) {
assert (*itnc1 != *itnc2);
if ( graph->isComplement (*itnc1, *itnc2, combination)
@@ -761,7 +762,8 @@ namespace hpp {
return constraint;
}
- void LevelSetEdge::insertParamConstraint (const NumericalConstraintPtr_t& nm,
+ void LevelSetEdge::insertParamConstraint
+ (const constraints::ImplicitPtr_t& nm,
const segments_t& passiveDofs)
{
isInit_ = false;
@@ -772,7 +774,7 @@ namespace hpp {
void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypes_t ineq)
{
isInit_ = false;
- insertParamConstraint (NumericalConstraint::create (function, ineq));
+ insertParamConstraint (constraints::Implicit::create (function, ineq));
}
void LevelSetEdge::insertParamConstraint (const LockedJointPtr_t lockedJoint)
@@ -781,7 +783,8 @@ namespace hpp {
paramLockedJoints_.push_back (lockedJoint);
}
- void LevelSetEdge::insertConditionConstraint (const NumericalConstraintPtr_t& nm,
+ void LevelSetEdge::insertConditionConstraint
+ (const constraints::ImplicitPtr_t& nm,
const segments_t& passiveDofs)
{
isInit_ = false;
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index b609ea8eb1383f220a87be13f11413f555356518..3458280237316b39f5d6942623cfac64fc13537f 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -28,6 +28,7 @@
namespace hpp {
namespace manipulation {
namespace graph {
+ typedef constraints::Implicit Implicit;
const std::string& GraphComponent::name() const
{
return name_;
@@ -50,7 +51,7 @@ namespace hpp {
isInit_ = false;
}
- void GraphComponent::addNumericalConstraint (const NumericalConstraintPtr_t& nm,
+ void GraphComponent::addNumericalConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs)
{
isInit_ = false;
@@ -60,7 +61,7 @@ namespace hpp {
void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypes_t& ineq)
{
- addNumericalConstraint (NumericalConstraint::create (function,ineq));
+ addNumericalConstraint (Implicit::create (function,ineq));
}
void GraphComponent::resetNumericalConstraints ()
diff --git a/src/graph/graph.cc b/src/graph/graph.cc
index 97c8458c6b472c0bd0d84d9bda372ec8083ad4e2..021aef36eaf29e670441653bdff0bdfbe4d65fc3 100644
--- a/src/graph/graph.cc
+++ b/src/graph/graph.cc
@@ -27,6 +27,8 @@
namespace hpp {
namespace manipulation {
namespace graph {
+ typedef constraints::Implicit Implicit;
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
const ProblemPtr_t& problem)
{
@@ -159,17 +161,17 @@ namespace hpp {
}
void Graph::registerConstraints
- (const NumericalConstraintPtr_t& constraint,
- const NumericalConstraintPtr_t& complement,
- const NumericalConstraintPtr_t& both)
+ (const ImplicitPtr_t& constraint,
+ const ImplicitPtr_t& complement,
+ const ImplicitPtr_t& both)
{
constraintsAndComplements_.push_back (ConstraintAndComplement_t
(constraint, complement, both));
}
- bool Graph::isComplement (const NumericalConstraintPtr_t& constraint,
- const NumericalConstraintPtr_t& complement,
- NumericalConstraintPtr_t& combinationOfBoth)
+ bool Graph::isComplement (const ImplicitPtr_t& constraint,
+ const ImplicitPtr_t& complement,
+ ImplicitPtr_t& combinationOfBoth)
const
{
for (ConstraintsAndComplements_t::const_iterator it =
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index f898acb89fadea96921531326acdef33ff6f9660..66598e5b3936a563ef9f98ad4eb4189f5e461698 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -32,6 +32,7 @@
#include <pinocchio/multibody/model.hpp>
#include <hpp/constraints/differentiable-function.hh>
+#include <hpp/constraints/locked-joint.hh>
#include <hpp/manipulation/handle.hh>
#include <hpp/manipulation/graph/state.hh>
@@ -46,6 +47,8 @@ namespace hpp {
namespace manipulation {
namespace graph {
namespace helper {
+ typedef constraints::Implicit Implicit;
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
template <bool forPath>
void NumericalConstraintsAndPassiveDofs::addToComp
(GraphComponentPtr_t comp) const
@@ -457,13 +460,13 @@ namespace hpp {
const GripperPtr_t& gripper, const HandlePtr_t& handle,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
- NumericalConstraintPtr_t gc = handle->createGrasp (gripper, "");
+ 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 ());
- NumericalConstraintPtr_t gcc = handle->createGraspComplement
+ ImplicitPtr_t gcc = handle->createGraspComplement
(gripper, "");
if (gcc->function ().outputSize () > 0) {
grasp.nc_fol.nc.push_back (gcc);
@@ -471,7 +474,7 @@ namespace hpp {
}
const value_type c = handle->clearance () + gripper->clearance ();
- NumericalConstraintPtr_t pgc = handle->createPreGrasp (gripper, c, "");
+ 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);
@@ -479,9 +482,9 @@ namespace hpp {
}
void strictPlacementManifold (
- const NumericalConstraintPtr_t placement,
- const NumericalConstraintPtr_t preplacement,
- const NumericalConstraintPtr_t placementComplement,
+ const ImplicitPtr_t placement,
+ const ImplicitPtr_t preplacement,
+ const ImplicitPtr_t placementComplement,
FoliatedManifold& place, FoliatedManifold& preplace)
{
place.nc.nc.push_back (placement);
@@ -500,8 +503,8 @@ namespace hpp {
}
void relaxedPlacementManifold (
- const NumericalConstraintPtr_t placement,
- const NumericalConstraintPtr_t preplacement,
+ const ImplicitPtr_t placement,
+ const ImplicitPtr_t preplacement,
const LockedJoints_t objectLocks,
FoliatedManifold& place, FoliatedManifold& preplace)
{
@@ -578,7 +581,7 @@ namespace hpp {
}
};
std::tr1::unordered_set<edgeid_type, edgeid_hash> edges;
- std::vector< boost::array<NumericalConstraintPtr_t,3> > graspCs;
+ std::vector< boost::array<ImplicitPtr_t,3> > graspCs;
index_t nG, nOH;
GraspV_t dims;
const Grippers_t& gs;
@@ -660,10 +663,10 @@ namespace hpp {
edges.insert(edgeid_type(stateid(g1), stateid(g2)));
}
- inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint (
+ inline boost::array<ImplicitPtr_t,3>& graspConstraint (
const index_t& iG, const index_t& iOH)
{
- boost::array<NumericalConstraintPtr_t,3>& gcs =
+ boost::array<ImplicitPtr_t,3>& gcs =
graspCs [iG * nOH + iOH];
if (!gcs[0]) {
hppDout (info, "Create grasps constraints for ("
@@ -766,7 +769,7 @@ namespace hpp {
void graspManifold (const index_t& iG, const index_t& iOH,
FoliatedManifold& grasp, FoliatedManifold& pregrasp)
{
- boost::array<NumericalConstraintPtr_t,3>& gcs
+ boost::array<ImplicitPtr_t,3>& gcs
= graspConstraint (iG, iOH);
grasp.nc.nc.push_back (gcs[0]);
grasp.nc.pdof.push_back (segments_t ());
diff --git a/src/handle.cc b/src/handle.cc
index 8f7c24e334b6987cf5a95c330636f38dabb13c22..c5a8c13e81b6b611c75130f302e56ce1cc4fdff4 100644
--- a/src/handle.cc
+++ b/src/handle.cc
@@ -31,12 +31,16 @@
#include <hpp/constraints/generic-transformation.hh>
-#include <hpp/core/numerical-constraint.hh>
-#include <hpp/core/explicit-numerical-constraint.hh>
+#include <hpp/constraints/implicit.hh>
+#include <hpp/constraints/explicit.hh>
#include <hpp/core/explicit-relative-transformation.hh>
namespace hpp {
namespace manipulation {
+ using constraints::Implicit;
+ using constraints::ImplicitPtr_t;
+ using constraints::Explicit;
+ using constraints::ExplicitPtr_t;
std::string Handle::className ("Handle");
namespace {
static const matrix3_t I3 = matrix3_t::Identity();
@@ -54,7 +58,7 @@ namespace hpp {
};
}
- using core::ExplicitNumericalConstraint;
+ using constraints::Explicit;
using constraints::DifferentiableFunction;
bool isHandleOnFreeflyer (const Handle& handle)
@@ -104,7 +108,7 @@ namespace hpp {
mask_ = mask;
}
- NumericalConstraintPtr_t Handle::createGrasp
+ ImplicitPtr_t Handle::createGrasp
(const GripperPtr_t& gripper, std::string n) const
{
using core::ExplicitRelativeTransformation;
@@ -117,8 +121,8 @@ namespace hpp {
(n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (), localPosition())->createNumericalConstraint();
}
- return NumericalConstraintPtr_t
- (NumericalConstraint::create (RelativeTransformation::create
+ return ImplicitPtr_t
+ (Implicit::create (RelativeTransformation::create
(n,
gripper->joint()->robot(),
gripper->joint (), joint (),
@@ -126,7 +130,7 @@ namespace hpp {
localPosition(), mask_)));
}
- NumericalConstraintPtr_t Handle::createGraspComplement
+ ImplicitPtr_t Handle::createGraspComplement
(const GripperPtr_t& gripper, std::string n) const
{
if (n.empty()) {
@@ -136,7 +140,7 @@ namespace hpp {
}
core::DevicePtr_t robot = gripper->joint()->robot();
if (is6Dmask(mask_)) {
- return NumericalConstraint::create (
+ return Implicit::create (
boost::shared_ptr <ZeroDiffFunc> (new ZeroDiffFunc (
robot->configSize(), robot->numberDof (), n))
);
@@ -148,12 +152,12 @@ namespace hpp {
gripper->joint (), joint (),
gripper->objectPositionInJoint (),
localPosition(), Cmask);
- return NumericalConstraint::create (function,
+ return Implicit::create (function,
ComparisonTypes_t(function->outputSize(), constraints::Equality));
}
}
- NumericalConstraintPtr_t Handle::createGraspAndComplement
+ ImplicitPtr_t Handle::createGraspAndComplement
(const GripperPtr_t& gripper, std::string n) const
{
using boost::assign::list_of;
@@ -173,7 +177,7 @@ namespace hpp {
}
// If handle is on a freeflying object, create an explicit constraint
if (isHandleOnFreeflyer (*this)) {
- ExplicitNumericalConstraintPtr_t enc
+ ExplicitPtr_t enc
(ExplicitRelativeTransformation::create
(n, gripper->joint ()->robot (), gripper->joint (), joint (),
gripper->objectPositionInJoint (),
@@ -181,8 +185,8 @@ namespace hpp {
enc->comparisonType (comp);
return enc;
}
- return NumericalConstraintPtr_t
- (NumericalConstraint::create (RelativeTransformation::create
+ return ImplicitPtr_t
+ (Implicit::create (RelativeTransformation::create
(n,
gripper->joint()->robot(),
gripper->joint (), joint (),
@@ -192,7 +196,7 @@ namespace hpp {
(true)), comp));
}
- NumericalConstraintPtr_t Handle::createPreGrasp
+ ImplicitPtr_t Handle::createPreGrasp
(const GripperPtr_t& gripper, const value_type& shift, std::string n) const
{
Transform3f transform = gripper->objectPositionInJoint ()
@@ -200,8 +204,8 @@ namespace hpp {
if (n.empty())
n = "Pregrasp_ " + maskToStr(mask_) + "_" + name ()
+ "_" + gripper->name ();
- return NumericalConstraintPtr_t
- (NumericalConstraint::create (RelativeTransformation::create
+ return ImplicitPtr_t
+ (Implicit::create (RelativeTransformation::create
(n,
gripper->joint()->robot(),
gripper->joint (), joint (),
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index c759b3a8d2952be826747b189ccc899786de0e87..ffde8570585ea39fb09f54e6a40773d747496a49 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -66,6 +66,8 @@
namespace hpp {
namespace manipulation {
+ typedef constraints::Implicit Implicit;
+ typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace {
struct PartialShortcutTraits :
core::pathOptimization::PartialShortcutTraits {
@@ -271,9 +273,9 @@ namespace hpp {
constraints.first->setNormalMargin (margin);
- addNumericalConstraint (name, NumericalConstraint::create
+ addNumericalConstraint (name, Implicit::create
(constraints.first));
- addNumericalConstraint (complementName, NumericalConstraint::create
+ addNumericalConstraint (complementName, Implicit::create
(constraints.second,
ComparisonTypes_t
(constraints.second->outputSize(),
@@ -324,7 +326,7 @@ namespace hpp {
cvxShape->setNormalMargin (margin + width);
- addNumericalConstraint (name, NumericalConstraint::create (cvxShape));
+ addNumericalConstraint (name, Implicit::create (cvxShape));
}
void ProblemSolver::createGraspConstraint
@@ -340,9 +342,9 @@ namespace hpp {
if (!h) throw std::runtime_error ("No handle with name " + handle + ".");
const std::string cname = name + "/complement";
const std::string bname = name + "/hold";
- NumericalConstraintPtr_t constraint (h->createGrasp (g, name));
- NumericalConstraintPtr_t complement (h->createGraspComplement (g, cname));
- NumericalConstraintPtr_t both (h->createGraspAndComplement (g, bname));
+ ImplicitPtr_t constraint (h->createGrasp (g, name));
+ ImplicitPtr_t complement (h->createGraspComplement (g, cname));
+ ImplicitPtr_t both (h->createGraspAndComplement (g, bname));
addNumericalConstraint ( name, constraint);
addNumericalConstraint (cname, complement);
addNumericalConstraint (bname, both);
@@ -359,7 +361,7 @@ namespace hpp {
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);
+ ImplicitPtr_t constraint = h->createPreGrasp (g, c, name);
addNumericalConstraint (name, constraint);
}
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 6dcc7e93651304f8b799687c9bae8c97c5972da2..8280383c1bc5df4d9a3ac213b3474f881f1010f8 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/src/steering-method/cross-state-optimization.cc
@@ -28,6 +28,7 @@
#include <hpp/pinocchio/configuration.hh>
+#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/solver/by-substitution.hh>
#include <hpp/core/explicit-numerical-constraint.hh>
@@ -342,12 +343,12 @@ namespace hpp {
// TODO handle numerical constraints
using namespace ::hpp::core;
- ExplicitNumericalConstraintPtr_t enc;
+ constraints::ExplicitPtr_t enc;
const NumericalConstraints_t& ncs = trans->numericalConstraints();
for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
_nc != ncs.end(); ++_nc) {
NumericalConstraintPtr_t nc (*_nc);
- enc = HPP_DYNAMIC_PTR_CAST (ExplicitNumericalConstraint, nc);
+ enc = HPP_DYNAMIC_PTR_CAST (constraints::Explicit, nc);
DifferentiableFunctionPtr_t f, ef;
// i = Input, o = Output,
@@ -420,11 +421,11 @@ namespace hpp {
void _add (const NumericalConstraints_t& ncs, const std::size_t& i)
{
using namespace ::hpp::core;
- ExplicitNumericalConstraintPtr_t enc;
+ constraints::ExplicitPtr_t enc;
for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
_nc != ncs.end(); ++_nc) {
NumericalConstraintPtr_t nc (*_nc);
- enc = HPP_DYNAMIC_PTR_CAST (ExplicitNumericalConstraint, nc);
+ enc = HPP_DYNAMIC_PTR_CAST (constraints::Explicit, nc);
bool added = false;
if (enc) {
added = solver.explicitConstraintSet().add (
diff --git a/tests/path-projection.cc b/tests/path-projection.cc
index ee77980cf197807590c57b3dfd682f49a0dcee8f..fdb17625d810ff5bfe38454f1b56fd183c64b977 100644
--- a/tests/path-projection.cc
+++ b/tests/path-projection.cc
@@ -70,7 +70,7 @@ using hpp::core::ConstraintSet;
using hpp::core::ConstraintSetPtr_t;
using hpp::core::ConfigProjector;
using hpp::core::ConfigProjectorPtr_t;
-using hpp::core::NumericalConstraint;
+using hpp::constraints::Implicit;
using boost::assign::list_of;
@@ -181,7 +181,7 @@ int main (int , char**) {
list_of (false)(true)(false).convert_to_container<std::vector<bool> >());
ConstraintSetPtr_t cs = ConstraintSet::create (r, "test-cs");
ConfigProjectorPtr_t proj = ConfigProjector::create (r, "test", 1e-4, 20);
- proj->add (NumericalConstraint::create (c));
+ proj->add (Implicit::create (c));
cs->addConstraint (proj);
ProblemPtr_t problem (new Problem (r));
WeighedDistancePtr_t dist = WeighedDistance::createWithWeight