Commit d97bc8c3 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add class Parameter and update parameter usage.

parent 67c12097
......@@ -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
......
//
// 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
......@@ -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
......@@ -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 #
......
// 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
......@@ -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
......@@ -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>;