From 55aa595fd598e73a1921fe7eaaaf8fcffc54887a Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 12 Dec 2014 14:08:55 +0100
Subject: [PATCH] Update to commit e7866c4 in hpp-core.
---
doc/main.hh | 2 +-
include/hpp/manipulation/fwd.hh | 6 +-
include/hpp/manipulation/graph/edge.hh | 8 +-
include/hpp/manipulation/graph/fwd.hh | 10 +-
.../hpp/manipulation/graph/graph-component.hh | 19 ++--
.../hpp/manipulation/graph/node-selector.hh | 3 +-
include/hpp/manipulation/graph/node.hh | 6 +-
include/hpp/manipulation/problem-solver.hh | 25 +++--
src/graph/edge.cc | 95 ++++++++++---------
src/graph/graph-component.cc | 13 +--
src/graph/statistics.cc | 4 +-
src/problem-solver.cc | 21 ++--
12 files changed, 113 insertions(+), 99 deletions(-)
diff --git a/doc/main.hh b/doc/main.hh
index 750032a1..b502ca42 100644
--- a/doc/main.hh
+++ b/doc/main.hh
@@ -29,7 +29,7 @@ namespace hpp {
and Edge::node(), to retrive this Node.
Edge also contains two sets of \LHPP{core,Constraint}:
\li Edge::configConstraint() returns a \LHPP{core,ConstraintSet} used to generate a configuration lying in Edge::to()
- and respecting the offset (previsously set using hpp::core::Constraint::offsetFromConfig),
+ and respecting the rightHandSide (previsously set using hpp::core::ConfigProjector::leftHandSideFromConfig),
\li Edge::pathConstraint() returns a \LHPP{core,ConstraintSet} to be inserted in \LHPP{core,Path} represented
by this Edge.
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 9cae85c5..cd714a76 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -82,8 +82,8 @@ namespace hpp {
typedef std::map <JointConstPtr_t, JointPtr_t> JointMap_t;
typedef core::Constraint Constraint;
typedef core::ConstraintPtr_t ConstraintPtr_t;
- typedef core::LockedDof LockedDof;
- typedef core::LockedDofPtr_t LockedDofPtr_t;
+ typedef core::LockedJoint LockedJoint;
+ typedef core::LockedJointPtr_t LockedJointPtr_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
typedef core::ConstraintSet ConstraintSet;
@@ -96,7 +96,7 @@ namespace hpp {
typedef std::pair< GripperPtr_t, HandlePtr_t> Grasp_t;
typedef boost::shared_ptr <Grasp_t> GraspPtr_t;
typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t;
- typedef std::map <std::string, LockedDofPtr_t> LockedDofConstraintMap_t;
+ typedef std::map <std::string, LockedJointPtr_t> LockedDofConstraintMap_t;
typedef fcl::TriangleP Triangle;
typedef std::list <Triangle> TriangleList;
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 9edfaf82..1d636aff 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -222,9 +222,9 @@ namespace hpp {
LeafHistogramPtr_t histogram () const;
- void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const EquationTypePtr_t ineq);
+ void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq);
- void insertConfigConstraint (const LockedDofPtr_t lockedDof);
+ void insertConfigConstraint (const LockedJointPtr_t lockedDof);
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -249,8 +249,8 @@ namespace hpp {
/// Extra DifferentiableFunctions_t
DifferentiableFunctions_t extraNumericalFunctions_;
- /// Extra LockedDofs_t
- LockedDofs_t extraLockedDofs_;
+ /// Extra LockedJoints_t
+ LockedJoints_t extraLockedDofs_;
/// 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 9b4ee224..925e1c26 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/include/hpp/manipulation/graph/fwd.hh
@@ -46,18 +46,18 @@ namespace hpp {
typedef hpp::core::Constraint Constraint;
typedef hpp::core::ConstraintPtr_t ConstraintPtr_t;
- typedef hpp::core::LockedDof LockedDof;
- typedef hpp::core::LockedDofPtr_t LockedDofPtr_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::ConstraintSet ConstraintSet;
typedef hpp::core::ConstraintSetPtr_t ConstraintSetPtr_t;
typedef hpp::core::Equality Equality;
- typedef hpp::core::EquationTypePtr_t EquationTypePtr_t;
+ typedef hpp::core::ComparisonTypePtr_t ComparisonTypePtr_t;
typedef hpp::core::DifferentiableFunctionPtr_t DifferentiableFunctionPtr_t;
- typedef std::pair < DifferentiableFunctionPtr_t, EquationTypePtr_t > DiffFuncAndIneqPair_t;
+ typedef std::pair < DifferentiableFunctionPtr_t, ComparisonTypePtr_t > DiffFuncAndIneqPair_t;
typedef std::list < DiffFuncAndIneqPair_t > DifferentiableFunctions_t;
- typedef std::list < LockedDofPtr_t > LockedDofs_t;
+ typedef std::list < LockedJointPtr_t > LockedJoints_t;
class Histogram;
class NodeHistogram;
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index 74f66eeb..2d7806a1 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -52,24 +52,25 @@ namespace hpp {
virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function) __attribute__ ((deprecated));
/// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const EquationTypePtr_t& ineq);
+ virtual void addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq);
- /// Add core::LockedDof constraint to the component.
- virtual void addLockedDofConstraint (const LockedDofPtr_t& constraint);
+ /// Add core::LockedJoint constraint to the component.
+ virtual void addLockedJointConstraint
+ (const LockedJointPtr_t& constraint);
/// Insert the numerical constraints in a ConfigProjector
/// \return true is at least one DifferentiableFunctionPtr_t was inserted.
bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
- /// Insert the LockedDof constraints in a ConstraintSet
- /// \return true is at least one LockedDofPtr_t was inserted.
+ /// Insert the LockedJoint constraints in a ConstraintSet
+ /// \return true is at least one LockedJointPtr_t was inserted.
bool insertLockedDofs (ConstraintSetPtr_t cs) const;
/// Get a reference to the DifferentiableFunctions_t
const DifferentiableFunctions_t& numericalConstraints() const;
- /// Get a reference to the LockedDofs_t
- const LockedDofs_t& lockedDofConstraints () const;
+ /// Get a reference to the LockedJoints_t
+ const LockedJoints_t& lockedDofConstraints () const;
/// Set the parent graph.
void parentGraph(const GraphWkPtr_t& parent);
@@ -86,8 +87,8 @@ namespace hpp {
/// Stores the numerical constraints.
DifferentiableFunctions_t numericalConstraints_;
- /// List of LockedDof constraints
- LockedDofs_t lockedDofConstraints_;
+ /// List of LockedJoint constraints
+ LockedJoints_t lockedDofConstraints_;
/// A weak pointer to the parent graph.
GraphWkPtr_t graph_;
diff --git a/include/hpp/manipulation/graph/node-selector.hh b/include/hpp/manipulation/graph/node-selector.hh
index 2a644e59..8d854855 100644
--- a/include/hpp/manipulation/graph/node-selector.hh
+++ b/include/hpp/manipulation/graph/node-selector.hh
@@ -49,7 +49,8 @@ namespace hpp {
}
/// Should never be called.
- void addLockedDofConstraint (const core::LockedDof& /* constraint */)
+ void addLockedJointConstraint
+ (const core::LockedJoint& /* constraint */)
{
HPP_THROW_EXCEPTION (Bad_function_call, "This component does not have constraints.");
}
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index c8e98485..55d41874 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -19,7 +19,7 @@
# include <boost/function.hpp>
-#include <hpp/core/locked-dof.hh>
+#include <hpp/core/locked-joint.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
@@ -90,7 +90,7 @@ namespace hpp {
}
/// Add core::DifferentiableFunction to the component.
- virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const EquationTypePtr_t& ineq)
+ virtual void addNumericalConstraintForPath (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
{
numericalConstraintsForPath_.push_back (DiffFuncAndIneqPair_t(function,ineq));
}
@@ -101,7 +101,7 @@ namespace hpp {
{
for (DifferentiableFunctions_t::const_iterator it = numericalConstraintsForPath_.begin();
it != numericalConstraintsForPath_.end(); it++)
- proj->addConstraint (it->first, it->second);
+ proj->addFunction (it->first, it->second);
return !numericalConstraintsForPath_.empty ();
}
diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh
index 01e0b3da..84936c27 100644
--- a/include/hpp/manipulation/problem-solver.hh
+++ b/include/hpp/manipulation/problem-solver.hh
@@ -21,6 +21,7 @@
# include <map>
# include <hpp/core/problem-solver.hh>
# include <hpp/model/device.hh>
+# include "hpp/manipulation/deprecated.hh"
# include "hpp/manipulation/object.hh"
# include "hpp/manipulation/robot.hh"
# include "hpp/manipulation/fwd.hh"
@@ -117,28 +118,32 @@ namespace hpp {
/// return NULL if no grasp named graspName
GraspPtr_t grasp(const DifferentiableFunctionPtr_t& constraint) const;
- /// Add a LockedDof constraint to the map
+ /// Add a LockedJoint constraint to the map
/// \param name key of the constraint as stored in an internal map.
/// \param lockedDof the constraint to add.
- void addLockedDofConstraint (const std::string& name,
- const LockedDofPtr_t& lockedDof)
+ void addLockedJointConstraint (const std::string& name,
+ const LockedJointPtr_t& lockedDof)
{
lockedDofConstraintMap_ [name] = lockedDof;
}
- /// Get a LockedDof constraint by name
+ /// Get a LockedJoint constraint by name
/// \param name key of the constraint as stored in an internal map.
- LockedDofPtr_t lockedDofConstraint (const std::string& name) const;
+ LockedJointPtr_t lockedDofConstraint (const std::string& name) const;
/// Reset constraint set and put back the disable collisions
/// between gripper and handle
virtual void resetConstraints ();
- /// Add differentialFunction to the config projector
- /// Build the config projector if not constructed
- virtual void addConstraintToConfigProjector(
- const std::string& constraintName,
- const DifferentiableFunctionPtr_t& constraint);
+ /// Add differential function to the config projector
+ /// \param constraintName Name given to config projector if created by
+ /// this method.
+ /// \param functionName name of the function as stored in internal map.
+ /// Build the config projector if not yet constructed.
+ /// If constraint is a graps, deactivate collision between gripper and
+ /// object.
+ virtual void addFunctionToConfigProjector
+ (const std::string& constraintName, const std::string& functionName);
/// Build a composite robot from several robots and objects
/// \param robotName Name of the composite robot,
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 31f3a7d9..7b27926f 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -151,7 +151,7 @@ namespace hpp {
bool Edge::build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2, const core::WeighedDistance& d) const
{
ConstraintSetPtr_t constraints = pathConstraint ();
- constraints->offsetFromConfig(q1);
+ constraints->configProjector ()->rightHandSideFromConfig(q1);
if (!constraints->isSatisfied (q1) || !constraints->isSatisfied (q2)) {
return false;
}
@@ -165,22 +165,24 @@ namespace hpp {
return applyConstraints (*(nnear->configuration ()), q);
}
- bool Edge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
+ bool Edge::applyConstraints (ConfigurationIn_t qoffset,
+ ConfigurationOut_t q) const
{
ConstraintSetPtr_t c = configConstraint ();
- c->offsetFromConfig (qoffset);
ConfigProjectorPtr_t proj = c->configProjector ();
+ proj->rightHandSideFromConfig (qoffset);
if (c->apply (q)) {
return true;
}
- if (proj) {
- ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
- if (ss.nbFailure () > ss.nbSuccess ()) {
- hppDout (warning, c->name () << " fails often." << std::endl << ss);
- } else {
- hppDout (warning, c->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
- }
- }
+ assert (proj);
+ ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
+ if (ss.nbFailure () > ss.nbSuccess ()) {
+ hppDout (warning, c->name () << " fails often." << std::endl << ss);
+ } else {
+ hppDout (warning, c->name () << " succeeds at rate "
+ << (double)(ss.nbSuccess ()) / ss.numberOfObservations ()
+ << ".");
+ }
return false;
}
@@ -322,47 +324,48 @@ namespace hpp {
bool LevelSetEdge::applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const
{
+#if 0
// First, get an offset from the histogram that is not in the same connected component.
statistics::DiscreteDistribution < core::NodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
const Configuration_t& levelsetTarget = *(distrib ()->configuration ()),
q_offset = *(n_offset->configuration ());
// Then, set the offset.
ConstraintSetPtr_t cs = extraConfigConstraint ();
- cs->offsetFromConfig (q_offset);
-
const ConfigProjectorPtr_t cp = cs->configProjector ();
- if (cp) {
- vector_t offset = cp->offsetFromConfig (q_offset);
- size_t row = 0, nbRows = 0;
- for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
- it != extraNumericalFunctions_.end (); ++it) {
- const core::DifferentiableFunction& f = *(it->first);
- nbRows = f.outputSize ();
- vector_t value = vector_t::Zero (nbRows);
- if (f.isParametric ()) {
- f (value, levelsetTarget);
- }
- offset.segment (row, nbRows) = value;
- row += nbRows;
- }
- cp->offset (offset);
- }
- for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
- it != extraLockedDofs_.end (); ++it) {
- (*it)->offsetFromConfig (levelsetTarget);
+ assert (cp);
+ vector_t offset = cp->rightHandSideFromConfig (q_offset);
+ size_t row = 0, nbRows = 0;
+ for (DifferentiableFunctions_t::const_iterator it =
+ extraNumericalFunctions_.begin ();
+ it != extraNumericalFunctions_.end (); ++it) {
+ const core::DifferentiableFunction& f = *(it->first);
+ nbRows = f.outputSize ();
+ vector_t value = vector_t::Zero (nbRows);
+ // TODO: fix this function
+ if (f.isParametric ()) {
+ f (value, levelsetTarget);
+ }
+ offset.segment (row, nbRows) = value;
+ row += nbRows;
+ }
+ cp->rightHandSide (offset);
+ for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
+ it != extraLockedDofs_.end (); ++it) {
+ (*it)->valueFromFromConfig (levelsetTarget);
}
// Eventually, do the projection.
if (cs->apply (q))
return true;
- if (cp) {
- ::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
- if (ss.nbFailure () > ss.nbSuccess ()) {
- hppDout (warning, cs->name () << " fails often." << std::endl << ss);
- } else {
- hppDout (warning, cs->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
- }
- }
+ ::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
+ if (ss.nbFailure () > ss.nbSuccess ()) {
+ hppDout (warning, cs->name () << " fails often." << std::endl << ss);
+ } else {
+ hppDout (warning, cs->name () << " succeeds at rate "
+ << (double)(ss.nbSuccess ()) / ss.numberOfObservations ()
+ << ".");
+ }
+#endif
return false;
}
@@ -392,12 +395,12 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
it != extraNumericalFunctions_.end (); ++it) {
- proj->addConstraint (it->first);
+ proj->addFunction (it->first);
}
constraint->addConstraint (proj);
}
- for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
+ for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
it != extraLockedDofs_.end (); ++it)
constraint->addConstraint (*it);
@@ -422,7 +425,7 @@ namespace hpp {
bool hasDiffFunc = g->insertNumericalConstraints (proj);
for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
it != extraNumericalFunctions_.end (); ++it) {
- proj->addConstraint (it->first, it->second);
+ proj->addFunction (it->first, it->second);
}
hasDiffFunc = !extraNumericalFunctions_.empty () || hasDiffFunc;
hasDiffFunc = insertNumericalConstraints (proj) || hasDiffFunc;
@@ -431,7 +434,7 @@ namespace hpp {
constraint->addConstraint (proj);
g->insertLockedDofs (constraint);
- for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
+ for (LockedJoints_t::const_iterator it = extraLockedDofs_.begin ();
it != extraLockedDofs_.end (); ++it) {
constraint->addConstraint (*it);
}
@@ -442,12 +445,12 @@ namespace hpp {
return extraConstraints_->get ();
}
- void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const EquationTypePtr_t ineq)
+ void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
{
extraNumericalFunctions_.push_back (DiffFuncAndIneqPair_t (function, ineq));
}
- void LevelSetEdge::insertConfigConstraint (const LockedDofPtr_t lockedDof)
+ void LevelSetEdge::insertConfigConstraint (const LockedJointPtr_t lockedDof)
{
extraLockedDofs_.push_back (lockedDof);
}
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index acc48779..f9085f71 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -18,7 +18,7 @@
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
-#include <hpp/core/locked-dof.hh>
+#include <hpp/core/locked-joint.hh>
namespace hpp {
namespace manipulation {
@@ -66,12 +66,13 @@ namespace hpp {
assert (false);
}
- void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const EquationTypePtr_t& ineq)
+ void GraphComponent::addNumericalConstraint (const DifferentiableFunctionPtr_t& function, const ComparisonTypePtr_t& ineq)
{
numericalConstraints_.push_back(DiffFuncAndIneqPair_t(function,ineq));
}
- void GraphComponent::addLockedDofConstraint (const LockedDofPtr_t& constraint)
+ void GraphComponent::addLockedJointConstraint
+ (const LockedJointPtr_t& constraint)
{
lockedDofConstraints_.push_back (constraint);
}
@@ -80,13 +81,13 @@ namespace hpp {
{
for (DifferentiableFunctions_t::const_iterator it = numericalConstraints_.begin();
it != numericalConstraints_.end(); ++it)
- proj->addConstraint (it->first, it->second);
+ proj->addFunction (it->first, it->second);
return !numericalConstraints_.empty ();
}
bool GraphComponent::insertLockedDofs (ConstraintSetPtr_t cs) const
{
- for (LockedDofs_t::const_iterator it = lockedDofConstraints_.begin();
+ for (LockedJoints_t::const_iterator it = lockedDofConstraints_.begin();
it != lockedDofConstraints_.end(); ++it)
cs->addConstraint (*it);
return !lockedDofConstraints_.empty ();
@@ -97,7 +98,7 @@ namespace hpp {
return numericalConstraints_;
}
- const LockedDofs_t& GraphComponent::lockedDofConstraints () const
+ const LockedJoints_t& GraphComponent::lockedDofConstraints () const
{
return lockedDofConstraints_;
}
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 88568ded..56d58b96 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -153,7 +153,9 @@ namespace hpp {
void LeafHistogram::add (const core::NodePtr_t& n)
{
- iterator it = insert (LeafBin (constraint_->offsetFromConfig (*n->configuration ())));
+ iterator it = insert
+ (LeafBin (constraint_->configProjector ()->rightHandSideFromConfig
+ (*n->configuration ())));
it->push_back (n);
if (numberOfObservations()%10 == 0) {
hppDout (info, *this);
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 3df76121..b0576ad2 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -107,12 +107,12 @@ namespace hpp {
return constraintGraph_;
}
- LockedDofPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
+ LockedJointPtr_t ProblemSolver::lockedDofConstraint (const std::string& name) const
{
LockedDofConstraintMap_t::const_iterator it =
lockedDofConstraintMap_.find (name);
if (it == lockedDofConstraintMap_.end ()) {
- throw std::runtime_error ("No LockedDof constraint with this name");
+ throw std::runtime_error ("No LockedJoint constraint with this name");
}
return it->second;
}
@@ -150,15 +150,16 @@ namespace hpp {
}
}
- void ProblemSolver::addConstraintToConfigProjector (
- const std::string& constraintName,
- const DifferentiableFunctionPtr_t& constraint)
+ void ProblemSolver::addFunctionToConfigProjector
+ (const std::string& constraintName, const std::string& functionName)
{
- core::ProblemSolver::addConstraintToConfigProjector(constraintName,
- constraint);
- if ( grasp(constraint) ) {
- GripperPtr_t gripper = grasp(constraint)->first;
- HandlePtr_t handle = grasp(constraint)->second;
+ core::ProblemSolver::addFunctionToConfigProjector (constraintName,
+ functionName);
+ DifferentiableFunctionPtr_t constraint =
+ numericalConstraint (functionName);
+ if (GraspPtr_t g = grasp (constraint)) {
+ GripperPtr_t gripper = g->first;
+ HandlePtr_t handle = g->second;
JointPtr_t joint1 = handle->joint();
model::JointVector_t joints = gripper->getDisabledCollisions();
for (model::JointVector_t::iterator itJoint = joints.begin() ;
--
GitLab