Add class Parameter and update parameter usage.

CMakeLists.txt

@@ -89,6 +89,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/core/numerical-constraint.hh
   include/hpp/core/locked-joint.hh
   include/hpp/core/node.hh
+  include/hpp/core/parameter.hh
   include/hpp/core/path.hh
   include/hpp/core/path-optimization/path-length.hh
   include/hpp/core/path-optimization/gradient-based.hh

include/hpp/core/parameter.hh

+//
+// Copyright 2018 CNRS
+//
+// Author: Florent Lamiraux, 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/>.
+
+#ifndef HPP_CORE_PARAMETER_HH
+# define HPP_CORE_PARAMETER_HH
+
+#include <iostream>
+#include <string>
+#include <cassert>
+#include <typeinfo>
+
+#include <hpp/core/config.hh>
+#include <hpp/core/fwd.hh>
+
+namespace hpp {
+  namespace core {
+    /*
+    class Parameter;
+    class DYNAMIC_GRAPH_DLLAPI EitherType {
+    public:
+      EitherType(const Parameter& value);
+      ~EitherType();
+      operator bool () const;
+      operator unsigned () const;
+      operator int () const;
+      operator float () const;
+      operator double () const;
+      operator std::string () const;
+      operator Vector () const;
+      operator Eigen::MatrixXd () const;
+      operator Eigen::Matrix4d () const;
+    private:
+      const Parameter* value_;
+    };
+    */
+
+    class HPP_CORE_DLLAPI Parameter
+    {
+      public:
+        enum Type {
+          NONE,
+          BOOL,
+          /// as \ref size_type
+          INT,
+          /// as \ref value_type
+          FLOAT,
+          STRING,
+          /// as \ref vector_t
+          VECTOR,
+          /// as \ref matrix_t
+          MATRIX,
+          NB_TYPES
+        };
+        ~Parameter();
+        void deleteValue ();
+        explicit Parameter(const bool& value);
+        explicit Parameter(const size_type& value);
+        explicit Parameter(const value_type& value);
+        explicit Parameter(const std::string& value);
+        explicit Parameter(const vector_t& value);
+        explicit Parameter(const matrix_t& value);
+        /// Copy constructor
+        Parameter(const Parameter& value);
+        /// Construct an empty parameter (None)
+        explicit Parameter();
+        // operator assignement
+        Parameter operator=(const Parameter& value);
+        /// Return the type of the value
+        Type type() const;
+
+        /// Return the value as a castable value into the approriate type
+        ///
+        /// For instance,
+        /// \code
+        /// Parameter v1(5.0); // v1 is of type double
+        /// Parameter v2(3);   // v2 is of type int
+        /// double x1 = v1.value();
+        /// double x2 = v2.value();
+        /// \endcode
+        /// The first assignment will succeed, while the second one will throw
+        /// an exception.
+        // const EitherType value () const;
+        /// Return the name of the type
+        static std::string typeName(Type type);
+
+        /// Output in a stream
+        HPP_CORE_DLLAPI friend std::ostream& operator<<(std::ostream& os, const Parameter& value);
+      public:
+        // friend class EitherType;
+        bool              boolValue() const;
+        size_type         intValue() const;
+        value_type        floatValue() const;
+        std::string       stringValue() const;
+        vector_t          vectorValue() const;
+        matrix_t          matrixValue() const;
+        Type type_;
+
+        const void* const value_;
+    };
+
+    class HPP_CORE_DLLAPI ParameterDescription
+    {
+      public:
+        ParameterDescription (Parameter::Type type,
+            std::string name,
+            std::string doc = "",
+            Parameter defaultValue = Parameter())
+          : name_ (name)
+          , doc_ (doc)
+          , type_ (type)
+          , defaultValue_ (defaultValue)
+        {}
+
+        ParameterDescription ()
+        {}
+
+        const std::string&     name () const { return name_; }
+        const Parameter::Type& type () const { return type_; }
+        const std::string&     doc  () const { return doc_ ; }
+        const Parameter&       defaultValue () const;
+
+      private:
+        std::string name_;
+        std::string doc_;
+        Parameter::Type type_;
+        Parameter defaultValue_;
+    };
+  } // namespace core
+} //namespace hpp
+
+#endif // HPP_CORE_PARAMETER_HH

include/hpp/core/problem.hh

@@ -29,6 +29,7 @@
 # include <hpp/core/config.hh>
 # include <hpp/core/steering-method.hh>
 # include <hpp/core/container.hh>
+# include <hpp/core/parameter.hh>
 
 namespace hpp {
   namespace core {
@@ -245,19 +246,13 @@ namespace hpp {
       /// Get a parameter named name.
       ///
       /// \param name of the parameter.
-      /// \param defaultValue value returned if there is no parameter of this
-      ///        name
-      /// \throw boost::bad_any_cast if a parameter exists but has the wrong
-      ///        type.
-      template <typename T> T getParameter
-        (const std::string& name, const T& defaultValue) const
-        throw (boost::bad_any_cast)
+      const Parameter& getParameter (const std::string& name) const
+        throw (std::invalid_argument)
       {
-        if (parameters.has(name)) {
-          const boost::any& val = parameters.get(name);
-          return boost::any_cast<T>(val);
-        }
-        return defaultValue;
+        if (parameters.has(name))
+          return parameters.get(name);
+        else
+          return parameterDescription(name).defaultValue();
       }
 
       /// Set a parameter named name.
@@ -266,17 +261,29 @@ namespace hpp {
       /// \param value value of the parameter
       /// \throw std::invalid_argument if a parameter exists but has a different
       ///        type.
-      /// \note if you do not want any type checking but would rather erase any
-      ///       previous values, use
-      ///       \code
-      ///       parameters.add(name, (ExpectedType)value);
-      ///       \endcode
-      ///       If there is an ambiguity on the type, it is recommended to
-      ///       explicitely write it.
-      void setParameter (const std::string& name, const boost::any& value)
+      void setParameter (const std::string& name, const Parameter& value)
         throw (std::invalid_argument);
 
-      Container < boost::any > parameters;
+      /// Declare a parameter
+      /// In shared library, use the following snippet in your cc file:
+      /// \code{.cpp}
+      /// HPP_START_PARAMETER_DECLARATION(name)
+      /// Problem::declareParameter (ParameterDescription (
+      ///       Parameter::FLOAT,
+      ///       "name",
+      ///       "doc",
+      ///       Parameter(value)));
+      /// HPP_END_PARAMETER_DECLARATION(name)
+      /// \endcode
+      static void declareParameter (const ParameterDescription& desc);
+
+      /// Get all the parameter descriptions
+      static const Container<ParameterDescription>& parameterDescriptions ();
+
+      /// Access one parameter description
+      static const ParameterDescription& parameterDescription (const std::string& name);
+
+      Container < Parameter > parameters;
 
     private :
       /// The robot
@@ -307,4 +314,12 @@ namespace hpp {
     /// \}
   } // namespace core
 } // namespace hpp
+
+#define HPP_START_PARAMETER_DECLARATION(name)                                  \
+  class HPP_CORE_LOCAL __initializer_class_##name {                            \
+    __initializer_class_##name () {
+
+#define HPP_END_PARAMETER_DECLARATION(name)                                    \
+  } }; __initializer_class_##name __instance_##name ();
+
 #endif // HPP_CORE_PROBLEM_HH

src/CMakeLists.txt

@@ -56,6 +56,7 @@ path-validations.cc
   # nearest-neighbor/k-d-tree.hh #
   numerical-constraint.cc #
   node.cc #
+  parameter.cc #
   path.cc #
   path-optimizer.cc #
   path-optimization/collision-constraints-result.hh #

src/parameter.cc

+// Copyright (c) 2018, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr), Florent Lamiraux
+//
+// 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/parameter.hh>
+
+#include <hpp/util/exception-factory.hh>
+
+namespace hpp {
+  namespace core {
+
+    static void* copyValue(const Parameter& value);
+
+    /*
+    EitherType::EitherType(const Parameter& value) : value_(new Parameter(value))
+    {
+    }
+
+    EitherType::~EitherType()
+    {
+      delete value_;
+      value_ = NULL;
+    }
+
+    EitherType::operator bool() const
+    {
+      return value_->boolValue();
+    }
+    EitherType::operator unsigned() const
+    {
+      return value_->unsignedValue();
+    }
+    EitherType::operator int() const
+    {
+      return value_->intValue();
+    }
+    EitherType::operator float() const
+    {
+      return value_->floatValue();
+    }
+    EitherType::operator double() const
+    {
+      return value_->doubleValue();
+    }
+    EitherType::operator std::string() const
+    {
+      return value_->stringValue();
+    }
+    EitherType::operator Vector() const
+    {
+      return value_->vectorValue();
+    }
+    EitherType::operator Eigen::MatrixXd() const
+    {
+      return value_->matrixXdValue();
+    }
+
+    EitherType::operator Eigen::Matrix4d() const
+    {
+      return value_->matrix4dValue();
+    }
+    */
+
+    void Parameter::deleteValue ()
+    {
+      switch(type_) {
+      case BOOL:
+	delete(const bool*)value_;
+	break;
+      case INT:
+	delete(const size_type*)value_;
+	break;
+      case FLOAT:
+	delete(const value_type*)value_;
+	break;
+      case STRING:
+	delete(const std::string*)value_;
+	break;
+      case VECTOR:
+	delete(const vector_t*)value_;
+	break;
+      case MATRIX:
+	delete(const matrix_t*)value_;
+	break;
+      default:;
+      }
+    }
+
+    Parameter::~Parameter()
+    {
+      deleteValue ();
+    }
+
+    Parameter::Parameter(const bool& value)
+      : type_(BOOL), value_(new bool(value))
+    {}
+
+    Parameter::Parameter(const size_type& value)
+      : type_(INT), value_(new size_type(value))
+    {}
+
+    Parameter::Parameter(const value_type& value)
+      : type_(FLOAT), value_(new value_type(value))
+    {}
+    Parameter::Parameter(const std::string& value)
+      : type_(STRING), value_(new std::string(value))
+    {}
+
+    Parameter::Parameter(const vector_t& value)
+      : type_(VECTOR), value_(new vector_t(value))
+    {}
+
+    Parameter::Parameter(const matrix_t& value)
+      : type_(MATRIX), value_(new matrix_t(value))
+    {}
+
+    Parameter::Parameter(const Parameter& value)
+      : type_(value.type_), value_(copyValue(value))
+    {}
+
+    void* copyValue(const Parameter& value)
+    {
+      void* copy;
+      switch(value.type()) {
+      case Parameter::NONE:
+	copy = NULL;
+      case Parameter::BOOL:
+	copy = new bool(value.boolValue());
+	break;
+      case Parameter::INT:
+	copy = new size_type(value.intValue());
+	break;
+      case Parameter::FLOAT:
+	copy = new value_type(value.floatValue());
+	break;
+      case Parameter::STRING:
+	copy = new std::string(value.stringValue());
+	break;
+      case Parameter::VECTOR:
+	copy = new vector_t(value.vectorValue());
+	break;
+      case Parameter::MATRIX:
+	copy = new matrix_t(value.matrixValue());
+	break;
+      default:
+	abort();
+      }
+      return copy;
+    }
+
+    Parameter::Parameter() : type_(NONE), value_(NULL)
+    {}
+
+    Parameter Parameter::operator=(const Parameter& value)
+    {
+      if (&value != this) {
+	if(value_ != 0x0)
+	  deleteValue ();
+	type_ = value.type_;
+	void** ptValue = const_cast<void**>(&value_);
+	*ptValue = copyValue(value);
+      }
+      return *this;
+    }
+
+    // const EitherType Parameter::value() const
+    // {
+      // return EitherType(*this);
+    // }
+
+    Parameter::Type Parameter::type() const
+    {
+      return type_;
+    }
+
+    inline void check (Parameter::Type type, Parameter::Type expected)
+    {
+      if (type != expected)
+        throw std::invalid_argument ("value is not a " + Parameter::typeName (expected));
+    }
+
+    bool Parameter::boolValue() const
+    {
+      check (type_, BOOL);
+      return *((const bool*)value_);
+    }
+
+    size_type Parameter::intValue() const
+    {
+      check (type_, INT);
+      return *((const size_type*)value_);
+    }
+
+    value_type Parameter::floatValue() const
+    {
+      check (type_, FLOAT);
+      return *((const value_type*)value_);
+    }
+
+    std::string Parameter::stringValue() const
+    {
+      check (type_, STRING);
+      return *((const std::string*)value_);
+    }
+
+    vector_t Parameter::vectorValue() const
+    {
+      check (type_, VECTOR);
+      return *((const vector_t*)value_);
+    }
+
+    matrix_t Parameter::matrixValue() const
+    {
+      check (type_, MATRIX);
+      return *((const matrix_t*)value_);
+    }
+
+    std::string Parameter::typeName(Type type)
+    {
+      switch(type) {
+      case BOOL:
+	return std::string("bool");
+      case INT:
+	return std::string("size_type");
+      case FLOAT:
+	return std::string("value_type");
+      case STRING:
+	return std::string("string");
+      case VECTOR:
+	return std::string("vector");
+      case MATRIX:
+	return std::string("matrix");
+      default:
+	return std::string("unknown");
+      }
+    }
+
+    const Parameter& ParameterDescription::defaultValue () const
+    {
+      if (defaultValue_.type() == type_)
+        return defaultValue_;
+      HPP_THROW(std::logic_error,
+          "Parameter " << name_ << " expected a default value of type "
+          << Parameter::typeName(type_)
+          << " but has invalid default value " << defaultValue_);
+    }
+
+    std::ostream& operator<<(std::ostream& os, const Parameter& value)
+    {
+      os << "Type=" << Parameter::typeName(value.type_)
+	 << ", value=";
+      switch(value.type_) {
+      case Parameter::BOOL:
+	os << value.boolValue();
+	break;
+      case Parameter::INT:
+	os << value.intValue();
+	break;
+      case Parameter::FLOAT:
+	os << value.floatValue();
+	break;
+      case Parameter::STRING:
+	os << value.stringValue();
+	break;
+      case Parameter::VECTOR:
+	os << value.vectorValue();
+	break;
+      case Parameter::MATRIX:
+	os << value.matrixValue();
+	break;
+      default:
+	return os;
+      }
+      return os;
+    }
+
+    // template< typename T >
+      // struct DYNAMIC_GRAPH_DLLAPI ValueHelper
+      // {
+	// static const Parameter::Type TypeID;
+      // };
+    // template<> const Parameter::Type ValueHelper<bool>::TypeID = Parameter::BOOL;
+    // template<> const Parameter::Type ValueHelper<unsigned>::TypeID = Parameter::UNSIGNED;
+    // template<> const Parameter::Type ValueHelper<int>::TypeID = Parameter::INT;
+    // template<> const Parameter::Type ValueHelper<float>::TypeID = Parameter::FLOAT;
+    // template<> const Parameter::Type ValueHelper<double>::TypeID = Parameter::DOUBLE;
+    // template<> const Parameter::Type ValueHelper<std::string>::TypeID = Parameter::STRING;
+    // template<> const Parameter::Type ValueHelper<Vector>::TypeID = Parameter::VECTOR;
+    // template<> const Parameter::Type ValueHelper<Eigen::MatrixXd>::TypeID = Parameter::MATRIX;
+    // template<> const Parameter::Type ValueHelper<Eigen::Matrix4d>::TypeID = Parameter::MATRIX4D;
+
+  } // namespace core
+} // namespace hpp

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

@@ -129,8 +129,8 @@ namespace hpp {
       {
         const value_type infinity = std::numeric_limits<value_type>::infinity();
 
-        const value_type safety = problem().getParameter("SimpleTimeParameterization/safety", (value_type)1);
-        const size_type order = problem().getParameter("SimpleTimeParameterization/order", (size_type)0);
+        const value_type safety = problem().getParameter("SimpleTimeParameterization/safety").floatValue();
+        const size_type order = problem().getParameter("SimpleTimeParameterization/order").intValue();
 
         // Retrieve velocity limits
         const DevicePtr_t& robot = problem().robot();
@@ -205,6 +205,19 @@ namespace hpp {
 
       SimpleTimeParameterization::SimpleTimeParameterization (const Problem& problem):
         PathOptimizer(problem) {}
+
+      // ----------- Declare parameters ------------------------------------- //
+
+      HPP_START_PARAMETER_DECLARATION(SimpleTimeParameterization)
+      Problem::declareParameter(ParameterDescription (Parameter::FLOAT,
+            "SimpleTimeParameterization/safety",
+            "A scaling factor for the joint bounds.",
+            Parameter(1.)));
+      Problem::declareParameter(ParameterDescription (Parameter::INT,
+            "SimpleTimeParameterization/order",
+            "The desired continuity order.",
+            Parameter((size_type)0)));
+      HPP_END_PARAMETER_DECLARATION(SimpleTimeParameterization)
     } // namespace pathOptimization
   } // namespace core
 } // namespace hpp

src/path-optimization/spline-gradient-based.cc

@@ -197,7 +197,7 @@ namespace hpp {
             const constraints::ExplicitSolver& es = hs.explicitSolver();
 
             // Get the active parameter row selection.
-            value_type guessThreshold = problem().getParameter ("SplineGradientBased/guessThreshold", value_type(-1));
+            value_type guessThreshold = problem().getParameter ("SplineGradientBased/guessThreshold").floatValue();
             Eigen::RowBlockIndices select = computeActiveParameters (path, hs, guessThreshold);
 
             const size_type rDof = robot_->numberDof(),
@@ -361,12 +361,12 @@ namespace hpp {
       PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize (const PathVectorPtr_t& path)
       {
         // Get some parameters
-        value_type alphaInit = problem().getParameter ("SplineGradientBased/alphaInit", value_type(0.2));
-        bool alwaysStopAtFirst = problem().getParameter ("SplineGradientBased/alwaysStopAtFirst", true);
-        bool linearizeAtEachStep = problem().getParameter ("SplineGradientBased/linearizeAtEachStep", false);
-        bool checkJointBound = problem().getParameter ("SplineGradientBased/checkJointBound", true);
-        bool returnOptimum = problem().getParameter ("SplineGradientBased/returnOptimum", false);
-        value_type costThreshold = problem().getParameter ("SplineGradientBased/costThreshold", value_type(0.01));
+        value_type alphaInit = problem().getParameter ("SplineGradientBased/alphaInit").floatValue();
+        bool alwaysStopAtFirst = problem().getParameter ("SplineGradientBased/alwaysStopAtFirst").boolValue();
+        bool linearizeAtEachStep = problem().getParameter ("SplineGradientBased/linearizeAtEachStep").boolValue();
+        bool checkJointBound = problem().getParameter ("SplineGradientBased/checkJointBound").boolValue();
+        bool returnOptimum = problem().getParameter ("SplineGradientBased/returnOptimum").boolValue();
+        value_type costThreshold = problem().getParameter ("SplineGradientBased/costThreshold").floatValue();
 
         PathVectorPtr_t tmp = PathVector::create (robot_->configSize(), robot_->numberDof());
         path->flatten(tmp);
@@ -588,6 +588,36 @@ namespace hpp {
       template class SplineGradientBased<path::BernsteinBasis, 1>; // equivalent to StraightPath
       // template class SplineGradientBased<path::BernsteinBasis, 2>;
       template class SplineGradientBased<path::BernsteinBasis, 3>;