Merge pull request #188 from jmirabel/devel

Minor updates.

include/hpp/core/config-projector.hh

@@ -297,6 +297,18 @@ namespace hpp {
         return *solver_;
       }
 
+      /// Set the line search type.
+      void lineSearchType (LineSearchType ls)
+      {
+        lineSearchType_ = ls;
+      }
+
+      /// Get the line search type.
+      LineSearchType lineSearchType () const
+      {
+        return lineSearchType_;
+      }
+
       static void defaultLineSearch (LineSearchType ls);
 
     protected:

include/hpp/core/steering-method/straight.hh

@@ -21,12 +21,8 @@
 
 # include <hpp/core/config.hh>
 # include <hpp/core/fwd.hh>
-# include <hpp/core/problem.hh>
 # include <hpp/core/steering-method/fwd.hh>
 # include <hpp/core/steering-method.hh>
-# include <hpp/core/straight-path.hh>
-# include <hpp/core/distance.hh>
-# include <hpp/core/config-projector.hh>
 
 namespace hpp {
   namespace core {
@@ -64,20 +60,8 @@ namespace hpp {
 
           /// create a path between two configurations
           virtual PathPtr_t impl_compute (ConfigurationIn_t q1,
-              ConfigurationIn_t q2) const
-          {
-            value_type length = (*problem_.distance()) (q1, q2);
-            ConstraintSetPtr_t c;
-            if (constraints() && constraints()->configProjector ()) {
-              c = HPP_STATIC_PTR_CAST (ConstraintSet, constraints()->copy ());
-              c->configProjector()->rightHandSideFromConfig (q1); 
-            } else {
-              c = constraints ();
-            }
-            PathPtr_t path = StraightPath::create
-              (problem_.robot(), q1, q2, length, c);
-            return path;
-          }
+              ConfigurationIn_t q2) const;
+
         protected:
           /// Constructor with robot
           /// Weighed distance is created from robot

src/CMakeLists.txt

@@ -94,6 +94,7 @@ path-validation/no-validation.hh
   steering-method/dubins.cc
   steering-method/snibud.cc
   steering-method/spline.cc
+  steering-method/straight.cc
   straight-path.cc
   interpolated-path.cc
   visibility-prm-planner.cc

src/plugin.cc

@@ -56,7 +56,8 @@ namespace hpp {
         void* library = dlopen(lib.c_str(), RTLD_NOW);
         error = dlerror ();
         if (error != NULL) {
-          hppDout (error, "Error loading library " << lib << ": " << error);
+          throw std::runtime_error ("Error loading library " + lib + ": " +
+              error);
           return false;
         }
         if (library == NULL) {
@@ -67,11 +68,13 @@ namespace hpp {
         PluginFunction_t function = reinterpret_cast<PluginFunction_t>(dlsym(library, "createProblemSolverPlugin"));
         error = dlerror ();
         if (error != NULL) {
-          hppDout (error, "Error loading library " << lib << ":\n" << error);
+          throw std::runtime_error ("Error loading library " + lib + ": " +
+              error);
           return false;
         }
         if (function == NULL) {
-          hppDout (error, "Symbol createProblemSolverPlugin of (correctly loaded) library " << lib << " is NULL.");
+          throw std::runtime_error ("Symbol createProblemSolverPlugin of "
+              "(correctly loaded) library " + lib + " is NULL.");
           return false;
         }
 

src/problem-solver.cc

@@ -529,7 +529,7 @@ namespace hpp {
       if (robot_)
 	constraints_->addConstraint (constraint);
       else
-	hppDout (error, "Cannot add constraint while robot is not set");
+        throw std::logic_error ("Cannot add constraint while robot is not set");
     }
 
     void ProblemSolver::resetConstraints ()

src/steering-method/straight.cc

+// Copyright (c) 2019 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-core
+// hpp-core is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-core is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-core  If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <hpp/core/steering-method/straight.hh>
+
+#include <hpp/core/problem.hh>
+#include <hpp/core/straight-path.hh>
+#include <hpp/core/distance.hh>
+#include <hpp/core/config-projector.hh>
+
+namespace hpp {
+  namespace core {
+    namespace steeringMethod {
+      PathPtr_t Straight::impl_compute (ConfigurationIn_t q1, ConfigurationIn_t q2) const
+      {
+        value_type length = (*problem_.distance()) (q1, q2);
+        ConstraintSetPtr_t c;
+        if (constraints() && constraints()->configProjector ()) {
+          c = HPP_STATIC_PTR_CAST (ConstraintSet, constraints()->copy ());
+          c->configProjector()->rightHandSideFromConfig (q1); 
+          c->configProjector()->lineSearchType (ConfigProjector::Backtracking);
+        } else {
+          c = constraints ();
+        }
+        PathPtr_t path = StraightPath::create
+          (problem_.robot(), q1, q2, length, c);
+        return path;
+      }
+    } // namespace steeringMethod
+  } // namespace core
+} // namespace hpp