[ProblemSolver] Handle LockedJoint and Implicit instances in the same container

NEWS

                                                                 -*- outline -*-
+* Handle LockedJoint and Implicit instances in the same container in ProblemSolver class. make method ProblemSolver::addLockedJointToConfigProjector deprecated.
 * Fix bug in Reeds and Shepp paths when curvature is not equal to 1.
 New in 4.6.0
 * Keep frames of obstacles.

include/hpp/core/problem-solver.hh

@@ -31,6 +31,12 @@
 
 namespace hpp {
   namespace core {
+    /// member ProblemSolver::lockedJoints has been removed. LockedJointPtr_t
+    /// instances are now stored with constraints::ImplicitPtr_t in
+    /// member numericalConstraints.
+    class Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead
+    {
+    };
     typedef boost::function < DevicePtr_t (const std::string&) > RobotBuilder_t;
     typedef boost::function < PathOptimizerPtr_t (const Problem&) >
       PathOptimizerBuilder_t;
@@ -247,8 +253,12 @@ namespace hpp {
       /// \param lockedJointName name of the locked joint as stored in internal
       ///        map.
       /// Build the config projector if not yet constructed.
+      /// \deprecated LockedJoint instances are now handled as other numerical
+      ///             constraints. Call addNumericalConstraintToConfigProjector
+      ///             instead.
       virtual void addLockedJointToConfigProjector
-	(const std::string& configProjName, const std::string& lockedJointName);
+	(const std::string& configProjName, const std::string& lockedJointName)
+        HPP_CORE_DEPRECATED;
 
       /// Add a a numerical constraint in local map.
       /// \param name name of the numerical constraint as stored in local map,
@@ -560,8 +570,10 @@ namespace hpp {
 
       /// Container of constraints::Implicit
       Container <constraints::ImplicitPtr_t>      numericalConstraints;
-      /// Container of LockedJoint
-      Container <LockedJointPtr_t>              lockedJoints;
+      /// member lockedJoints has been removed. LockedJointPtr_t
+      /// instances are now stored with constraints::ImplicitPtr_t in
+      /// member numericalConstraints.
+      Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead                    lockedJoints;
       /// Container of CenterOfMassComputation
       Container <CenterOfMassComputationPtr_t>  centerOfMassComputations;
       /// Container of passive DoFs (as segments_t)

src/problem-solver.cc

@@ -567,21 +567,7 @@ namespace hpp {
     void ProblemSolver::addLockedJointToConfigProjector
     (const std::string& configProjName, const std::string& lockedJointName)
     {
-      if (!robot_) {
-	hppDout (error, "Cannot add constraint while robot is not set");
-      }
-      ConfigProjectorPtr_t  configProjector = constraints_->configProjector ();
-      if (!configProjector) {
-	configProjector = ConfigProjector::create
-	  (robot_, configProjName, errorThreshold_, maxIterProjection_);
-	constraints_->addConstraint (configProjector);
-      }
-      if (!lockedJoints.has (lockedJointName)) {
-        std::stringstream ss; ss << "Function " << lockedJointName <<
-                                " does not exists";
-        throw std::invalid_argument (ss.str());
-      }
-      configProjector->add (lockedJoints.get(lockedJointName));
+      addNumericalConstraintToConfigProjector (configProjName, lockedJointName);
     }
 
     void ProblemSolver::comparisonType (const std::string& name,
@@ -590,11 +576,9 @@ namespace hpp {
       constraints::ImplicitPtr_t nc;
       if (numericalConstraints.has (name))
         nc = numericalConstraints.get(name);
-      else if (lockedJoints.has (name))
-        nc = lockedJoints.get(name);
       else
-        throw std::runtime_error (name + std::string (" is neither a numerical "
-              "constraint nor a locked joint"));
+        throw std::runtime_error (name + std::string (" is not a numerical "
+              "constraint."));
       nc->comparisonType (types);
     }
 
@@ -604,11 +588,9 @@ namespace hpp {
       constraints::ImplicitPtr_t nc;
       if (numericalConstraints.has (name))
         nc = numericalConstraints.get(name);
-      else if (lockedJoints.has (name))
-        nc = lockedJoints.get(name);
       else
-        throw std::runtime_error (name + std::string (" is neither a numerical "
-              "constraint nor a locked joint"));
+        throw std::runtime_error (name + std::string (" is not a numerical "
+              "constraint."));
       ComparisonTypes_t eqtypes (nc->function().outputDerivativeSize(), type);
       nc->comparisonType (eqtypes);
     }
@@ -618,11 +600,9 @@ namespace hpp {
       constraints::ImplicitPtr_t nc;
       if (numericalConstraints.has (name))
         nc = numericalConstraints.get(name);
-      else if (lockedJoints.has (name))
-        nc = lockedJoints.get(name);
       else
-        throw std::runtime_error (name + std::string (" is neither a numerical "
-              "constraint nor a locked joint"));
+        throw std::runtime_error (name + std::string (" is not a numerical "
+              "constraint."));
       return nc->comparisonType ();
     }