Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-core
Commits
d97bc8c3
Commit
d97bc8c3
authored
May 31, 2018
by
Joseph Mirabel
Committed by
Joseph Mirabel
May 31, 2018
Browse files
Add class Parameter and update parameter usage.
parent
67c12097
Changes
14
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
d97bc8c3
...
...
@@ -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
0 → 100644
View file @
d97bc8c3
//
// 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
View file @
d97bc8c3
...
...
@@ -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
View file @
d97bc8c3
...
...
@@ -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
0 → 100644
View file @
d97bc8c3
// 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
View file @
d97bc8c3
...
...
@@ -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
View file @
d97bc8c3
...
...
@@ -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"
,
tr
ue
);
bool
linearizeAtEachStep
=
problem
().
getParameter
(
"SplineGradientBased/linearizeAtEachStep"
,
false
);
bool
checkJointBound
=
problem
().
getParameter
(
"SplineGradientBased/checkJointBound"
,
tr
ue
);
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"
).
boolVal
ue
(
);
bool
linearizeAtEachStep
=
problem
().
getParameter
(
"SplineGradientBased/linearizeAtEachStep"
).
boolValue
(
);
bool
checkJointBound
=
problem
().
getParameter
(
"SplineGradientBased/checkJointBound"
).
boolVal
ue
(
);
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
>;