Merge remote-tracking branch 'jmirabel/devel' into devel

include/hpp/core/configuration-shooter.hh

@@ -44,7 +44,11 @@ namespace hpp {
 
       /// Shoot a random configuration
       /// \param q the configuration (resized if necessary).
-      virtual void shoot (Configuration_t& q) const = 0;
+      ///
+      /// \deprecated This method is virtual for backward compatibility. It will
+      /// become non-virtual in the future. Child classes should rather implement
+      /// \ref impl_shoot so that both prototype of method shoot remain available.
+      virtual void shoot (Configuration_t& q) const { impl_shoot(q); }
 
       virtual ~ConfigurationShooter () {};
     protected:
@@ -56,6 +60,8 @@ namespace hpp {
     {
   weakPtr_ = weak;
     }
+
+      virtual void impl_shoot (Configuration_t& q) const = 0;
     private:
       ConfigurationShooterWkPtr_t weakPtr_;
     }; // class

include/hpp/core/configuration-shooter/gaussian.hh

@@ -44,7 +44,6 @@ namespace hpp {
             ptr->init (shPtr);
             return shPtr;
           }
-          virtual void shoot (Configuration_t& q) const;
 
           void center (ConfigurationIn_t c)
           {
@@ -92,6 +91,7 @@ namespace hpp {
             weak_ = self;
           }
 
+          virtual void impl_shoot (Configuration_t& q) const;
         private:
           const DevicePtr_t& robot_;
           /// The mean value

include/hpp/core/configuration-shooter/uniform.hh

@@ -42,7 +42,6 @@ namespace hpp {
           ptr->init (shPtr);
           return shPtr;
         }
-        virtual void shoot (Configuration_t& q) const;
 
         void sampleExtraDOF(bool sampleExtraDOF){
             sampleExtraDOF_=sampleExtraDOF;
@@ -61,6 +60,7 @@ namespace hpp {
           weak_ = self;
         }
 
+        virtual void impl_shoot (Configuration_t& q) const;
       private:
         DevicePtr_t robot_;
         bool sampleExtraDOF_;

include/hpp/core/problem-solver.hh

@@ -500,6 +500,11 @@ namespace hpp {
       {
 	return distanceBetweenObjects_;
       }
+
+      pinocchio::GeomModelPtr_t obstacleGeomModel () const
+      { return obstacleModel_; }
+      pinocchio::GeomDataPtr_t  obstacleGeomData () const
+      { return obstacleData_; }
       /// \}
 
       /// Local vector of objects considered for collision detection

include/hpp/core/time-parameterization/polynomial.hh

@@ -108,7 +108,7 @@ namespace hpp {
               tn *= t;
               res += a[i] * tn;
             }
-            assert (!std::isnan(res));
+            assert (res == res);
             return res;
           }
 

src/configuration-shooter/gaussian.cc

@@ -106,7 +106,7 @@ namespace hpp {
         ::pinocchio::details::Dispatch<ComputeSigmasStep>::run(jmodel.derived(), ComputeSigmasStep::ArgsType(model, sigmas));
       }
 
-      void Gaussian::shoot (Configuration_t& config) const
+      void Gaussian::impl_shoot (Configuration_t& config) const
       {
         static boost::random::mt19937 eng;
         vector_t velocity (robot_->numberDof());

src/configuration-shooter/uniform.cc

@@ -26,7 +26,7 @@ namespace hpp {
   namespace core {
     namespace configurationShooter {
 
-      void Uniform::shoot (Configuration_t& config) const
+      void Uniform::impl_shoot (Configuration_t& config) const
       {
         size_type extraDim = robot_->extraConfigSpace ().dimension ();
         size_type offset = robot_->configSize () - extraDim;

src/path-optimization/simple-time-parameterization.cc

@@ -45,6 +45,10 @@ namespace hpp {
           a[1] = B;
           hppDout (info, "Time parametrization returned " << a.transpose()
               << ", " << T);
+          if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
+            HPP_THROW(std::logic_error, "Invalid time parameterization "
+                "coefficients: " << a.transpose());
+          }
           return TimeParameterizationPtr_t (new Polynomial (a));
         }
 
@@ -60,6 +64,10 @@ namespace hpp {
           a[3] = - 2 * a[2] / (3 * T);
           hppDout (info, "Time parametrization returned " << a.transpose()
               << ", " << T);
+          if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
+            HPP_THROW(std::logic_error, "Invalid time parameterization "
+                "coefficients: " << a.transpose());
+          }
           return TimeParameterizationPtr_t (new Polynomial (a));
         }
 
@@ -88,6 +96,10 @@ namespace hpp {
           a[5] =   6 * (s1 - s0) / Tpow;
           hppDout (info, "Time parametrization returned " << a.transpose()
               << ", " << T);
+          if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
+            HPP_THROW(std::logic_error, "Invalid time parameterization "
+                "coefficients: " << a.transpose());
+          }
           return TimeParameterizationPtr_t (new Polynomial (a));
         }
 
@@ -98,7 +110,9 @@ namespace hpp {
           const value_type thr = std::sqrt(Eigen::NumTraits<value_type>::dummy_precision());
           if (   fabs(tp->value(0) - sr.first) >= thr
               || fabs(tp->value(T) - sr.second) >= thr) {
-            throw std::logic_error("Boundaries of TimeParameterization are not correct.");
+            HPP_THROW(std::logic_error, "Interval of TimeParameterization result"
+                " is not correct. Expected " << sr.first << ", " << sr.second <<
+                ". Got " << tp->value(0) << ", " << tp->value(T));
           }
           if (order >= 1
               && (fabs(tp->derivative(0, 1)) > thr
@@ -153,12 +167,11 @@ namespace hpp {
                  cb ((ub + lb) / 2);
         for (size_type i=0; i < cb.size(); ++i) {
           if (std::isnan (cb [i])) {
-            std::ostringstream oss;
-            oss << "in SimpleTimeParameterization::optimize:\n";
-            oss << "  the velocities of the input device should be bounded\n";
-            oss << "  velocity bounds at rank " << i << " are [" << lb[i]
-                << ", " << ub[i] << "].";
-            throw std::runtime_error(oss.str());
+            HPP_THROW(std::runtime_error,
+              "in SimpleTimeParameterization::optimize:\n"
+              << "  the velocities of the input device should be bounded\n"
+              << "  velocity bounds at rank " << i << " are [" << lb[i]
+              << ", " << ub[i] << "].");
           }
         }
         assert (cb.size() + robot->extraConfigSpace().dimension()
@@ -198,7 +211,10 @@ namespace hpp {
           const value_type B = std::min(
               ( ub.cwiseProduct(v_inv)).minCoeff(),
               (-lb.cwiseProduct(v_inv)).minCoeff());
-          assert (B > 0);
+          if (B <= 0 || B != B) {
+            HPP_THROW(std::runtime_error,"Invalid parametrization derivative "
+                "velocity bound: " << B);
+          }
 
           // Compute the polynom and total time
           value_type T;