Commit 2b29902d authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v4.4.0'

Release of version 4.4.0.
parents d0610ca3 3878b257
Pipeline #3615 failed with stage
in 50 minutes and 31 seconds
......@@ -31,11 +31,14 @@ SET(PROJECT_NAME hpp-core)
SET(PROJECT_DESCRIPTION
"Implement basic classes for canonical path planning for kinematic chains."
)
SET(PROJECT_URL "https://github.com/humanoid-path-planner/hpp-core")
# Where to compile shared objects
SET(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
SET(CMAKE_MODULE_PATH
${CMAKE_SOURCE_DIR}/cmake-modules/
${CMAKE_MODULE_PATH})
SETUP_HPP_PROJECT()
# Declare Headers
......@@ -108,6 +111,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/kinodynamic-path.hh
include/hpp/core/kinodynamic-oriented-path.hh
include/hpp/core/plan-and-optimize.hh
include/hpp/core/plugin.hh
include/hpp/core/problem.hh
include/hpp/core/problem-solver.hh
include/hpp/core/random-shortcut.hh # DEPRECATED
......@@ -155,13 +159,21 @@ SET(BOOST_COMPONENTS system timer unit_test_framework)
SEARCH_FOR_BOOST()
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(plugins)
ADD_SUBDIRECTORY(tests)
# Add dependency toward hpp-core library in pkg-config file.
PKG_CONFIG_APPEND_LIBS("hpp-core")
INSTALL (FILES doc/continuous-collision-checking.pdf
doc/continuous-validation.pdf
DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/doc/${PROJECT_NAME}/doxygen-html)
IF(INSTALL_DOCUMENTATION)
INSTALL (FILES doc/continuous-collision-checking.pdf
doc/continuous-validation.pdf
DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/doc/${PROJECT_NAME}/doxygen-html)
ENDIF(INSTALL_DOCUMENTATION)
SET(PKG_CONFIG_EXTRA "${PKG_CONFIG_EXTRA}\ncmake_plugin=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake/${PROJECT_NAME}-plugin.cmake")
INSTALL(FILES cmake-modules/hpp-core-plugin.cmake
DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake/
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
SETUP_HPP_PROJECT_FINALIZE()
Subproject commit 320c636960b03b3bad7c7a08bd2e104951f42bc3
Subproject commit d128cc2965a7878dc38acf8f4395e1899aa505cb
# Copyright (c) 2019, Joseph Mirabel
# Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
#
# 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/>.
MACRO(ADD_PLUGIN PLUGIN_NAME)
SET(options EXCLUDE_FROM_ALL)
SET(oneValueArgs )
SET(multiValueArgs
SOURCES
LINK_DEPENDENCIES
PKG_CONFIG_DEPENDENCIES)
CMAKE_PARSE_ARGUMENTS(PLUGIN "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
IF(PLUGIN_EXCLUDE_FROM_ALL)
SET(_options ${_options} EXCLUDE_FROM_ALL)
ENDIF()
ADD_LIBRARY(${PLUGIN_NAME} MODULE ${_options} ${PLUGIN_SOURCES})
SET_TARGET_PROPERTIES(${PLUGIN_NAME} PROPERTIES PREFIX "" BUILD_WITH_INSTALL_RPATH TRUE)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${PLUGIN_LINK_DEPENDENCIES})
FOREACH(DEP ${PLUGIN_PKG_CONFIG_DEPENDENCIES})
PKG_CONFIG_USE_DEPENDENCY(${PLUGIN_NAME} ${DEP})
ENDFOREACH()
IF(NOT PLUGIN_EXCLUDE_FROM_ALL)
INSTALL(TARGETS ${PLUGIN_NAME} DESTINATION lib/hppPlugins)
ENDIF()
ENDMACRO()
......@@ -23,8 +23,6 @@
# include <hpp/statistics/success-bin.hh>
# include <hpp/constraints/solver/by-substitution.hh>
# include <hpp/core/config.hh>
# include <hpp/core/constraint.hh>
# include <hpp/core/deprecated.hh>
......@@ -269,6 +267,9 @@ namespace hpp {
/// \return the parameter.
vector_t rightHandSide () const;
/// Update the right hand side using Implicit::rightHandSideAt
void rightHandSideAt (const value_type& s);
/// @}
/// Check whether a configuration statisfies the constraint.
......@@ -289,10 +290,7 @@ namespace hpp {
/// Get the numerical constraints of the config-projector (and so of the
/// Constraint Set)
const NumericalConstraints_t& numericalConstraints () const
{
return solver_->numericalConstraints ();
}
const NumericalConstraints_t& numericalConstraints () const;
const BySubstitution& solver () const
{
......
......@@ -21,8 +21,6 @@
# include <sstream>
# include <pinocchio/algorithm/joint-configuration.hpp>
# include <hpp/pinocchio/device.hh>
# include <hpp/core/configuration-shooter.hh>
......
......@@ -149,6 +149,7 @@ namespace hpp {
typedef constraints::matrixIn_t matrixIn_t;
typedef constraints::matrixOut_t matrixOut_t;
typedef constraints::LiegroupElement LiegroupElement;
typedef constraints::LiegroupElementRef LiegroupElementRef;
typedef constraints::LiegroupSpace LiegroupSpace;
typedef constraints::LiegroupSpacePtr_t LiegroupSpacePtr_t;
typedef pinocchio::size_type size_type;
......
......@@ -116,12 +116,8 @@ namespace hpp {
Configuration_t operator () (const value_type& time) const
HPP_CORE_DEPRECATED
{
Configuration_t result (outputSize ());
impl_compute (result, paramAtTime(time));
if (constraints_) {
constraints_->apply (result);
}
return result;
bool unused;
return (*this) (time, unused);
}
Configuration_t operator () (const value_type& time, bool& success) const
......@@ -132,9 +128,10 @@ namespace hpp {
bool operator () (ConfigurationOut_t result, const value_type& time)
const throw ()
{
bool success = impl_compute (result, paramAtTime(time));
value_type s = paramAtTime (time);
bool success = impl_compute (result, s);
if (!success) return false;
return (!constraints_ || constraints_->apply (result));
return applyConstraints (result, s);
}
/// Get the configuration at a parameter without applying the constraints.
......@@ -318,9 +315,7 @@ namespace hpp {
Configuration_t result (outputSize ());
success = impl_compute (result, param);
if (!success) return result;
if (constraints_) {
success = constraints_->apply (result);
}
success = applyConstraints (result, param);
return result;
}
......@@ -373,6 +368,8 @@ namespace hpp {
return time;
}
bool applyConstraints (ConfigurationOut_t result, const value_type& param) const;
/// Size of the configuration space
size_type outputSize_;
/// Number of degrees of freedom of the robot
......
// Copyright (c) 2019, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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_PLUGIN_HH
#define HPP_CORE_PLUGIN_HH
#include <string>
#include <hpp/core/config.hh>
#include <hpp/core/fwd.hh>
namespace hpp {
namespace core {
/// \addtogroup plugin
/// Plugin mechanism to declare new features in ProblemSolver class
class ProblemSolverPlugin {
public:
const std::string& name () const
{
return name_;
}
const std::string& version () const
{
return version_;
}
bool initialize (ProblemSolverPtr_t ps)
{
if (initialized_) return true;
initialized_ = impl_initialize (ps);
return initialized_;
}
virtual ~ProblemSolverPlugin () {}
protected:
virtual bool impl_initialize (ProblemSolverPtr_t ps) = 0;
ProblemSolverPlugin (const std::string& name, const std::string& version)
: name_ (name), version_ (version), initialized_ (false)
{}
private:
std::string name_, version_;
bool initialized_;
}; // class ProblemSolver
#define HPP_CORE_DEFINE_PLUGIN(PluginClassName) \
extern "C" { \
::hpp::core::ProblemSolverPlugin* createProblemSolverPlugin () \
{ \
return new PluginClassName (); \
} \
}
/// \addtogroup plugin
namespace plugin {
/// Find the absolute path to a library named name.
/// \param name
/// \return name if it is an absolute path. Otherwise, for each path in
/// LD_LIBRARY_PATH environment variable, look for path/hppPlugins/name.
/// \throw std::invalid_argument if not valid file found.
std::string findPluginLibrary (const std::string& name);
bool loadPlugin (const std::string& lib, ProblemSolver* ps);
} // namespace plugin
} // namespace core
} // namespace hpp
#endif // HPP_CORE_PLUGIN_HH
......@@ -19,7 +19,7 @@
#ifndef HPP_CORE_RANDOM_SHORTCUT_HH
# define HPP_CORE_RANDOM_SHORTCUT_HH
# warning "This file is deprecated. Include <hpp/constraints/explicit.hh> instead."
# warning "This file is deprecated. Include <hpp/core/path-optimization/random-shortcut.hh> instead."
# include <hpp/core/path-optimization/random-shortcut.hh>
# include <hpp/core/deprecated.hh>
......
......@@ -53,17 +53,26 @@ namespace hpp {
ConfigurationIn_t end,
value_type length)
{
StraightPath* ptr = new StraightPath (device, init, end, length);
return create (device, init, end, interval_t (0, length));
}
/// Create instance and return shared pointer
/// \param device Robot corresponding to configurations
/// \param init, end Start and end configurations of the path
/// \param interval interval of definition.
static StraightPathPtr_t create (const DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
interval_t interval)
{
StraightPath* ptr = new StraightPath (device, init, end, interval);
StraightPathPtr_t shPtr (ptr);
ptr->init (shPtr);
ptr->checkPath ();
return shPtr;
}
/// Create instance and return shared pointer
/// \param device Robot corresponding to configurations
/// \param init, end Start and end configurations of the path
/// \param length Distance between the configurations.
/// \copydoc create(const DevicePtr_t&, ConfigurationIn_t, ConfigurationIn_t, value_type)
/// \param constraints the path is subject to
static StraightPathPtr_t create (const DevicePtr_t& device,
ConfigurationIn_t init,
......@@ -71,7 +80,18 @@ namespace hpp {
value_type length,
ConstraintSetPtr_t constraints)
{
StraightPath* ptr = new StraightPath (device, init, end, length,
return create (device, init, end, interval_t (0, length), constraints);
}
/// \copydoc create(const DevicePtr_t&, ConfigurationIn_t, ConfigurationIn_t, interval_t)
/// \param constraints the path is subject to
static StraightPathPtr_t create (const DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
interval_t interval,
ConstraintSetPtr_t constraints)
{
StraightPath* ptr = new StraightPath (device, init, end, interval,
constraints);
StraightPathPtr_t shPtr (ptr);
ptr->init (shPtr);
......@@ -166,15 +186,25 @@ namespace hpp {
os << "final configuration: " << end_.transpose () << std::endl;
return os;
}
/// Constructor
StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type length);
/// Constructor
StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, interval_t interval);
/// Constructor with constraints
StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type length,
ConstraintSetPtr_t constraints);
/// Constructor with constraints
StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, interval_t interval,
ConstraintSetPtr_t constraints);
/// Copy constructor
StraightPath (const StraightPath& path);
......
......@@ -20,6 +20,7 @@
# define HPP_CORE_DOF_EXTRACTED_PATH_HH
# include <hpp/core/path.hh>
# include <hpp/constraints/matrix-view.hh>
namespace hpp {
namespace core {
......@@ -51,13 +52,15 @@ namespace hpp {
return createCopy (weak_.lock (), constraints);
}
/// \copydoc SubchainPath::SubchainPath
static SubchainPathPtr_t
create (const PathPtr_t& original, const segments_t& intervals)
create (const PathPtr_t& original, const segments_t& confIntervals,
const segments_t& velIntervals)
{
SubchainPath* ptr = new SubchainPath (original, intervals);
SubchainPathPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
SubchainPath* ptr = new SubchainPath (original, confIntervals, velIntervals);
SubchainPathPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
static SubchainPathPtr_t
......@@ -105,13 +108,7 @@ namespace hpp {
void dofExtract (ConfigurationIn_t qin, ConfigurationOut_t qout) const
{
size_type r = 0;
for (segments_t::const_iterator _rank = intervals_.begin();
_rank != intervals_.end(); ++_rank) {
qout.segment(r, _rank->second) = qin.segment(_rank->first, _rank->second);
r += _rank->second;
}
assert (r == outputSize());
qout = configView_.rview (qin);
}
protected:
......@@ -119,10 +116,7 @@ namespace hpp {
virtual std::ostream& print (std::ostream &os) const
{
os << "Dof Extracted Path:" << std::endl;
os << "intervals: ";
for (segments_t::const_iterator _rank = intervals_.begin();
_rank != intervals_.end(); ++_rank)
os << "[ " << _rank->first << ", " << _rank->second << "], " << std::endl;
os << "intervals: " << configView_ << std::endl;
os << "original path:" << std::endl;
os << *original_ << std::endl;
return os;
......@@ -131,16 +125,20 @@ namespace hpp {
/// Constructor
///
/// \param original Path to extract,
/// \param intervals of the configuration parameters to be extracted
SubchainPath (const PathPtr_t& original, const segments_t& intervals) :
Path (original->timeRange(), intervalsToSize(intervals), outputSize ()),
original_ (original), intervals_ (intervals),
/// \param confIntervals of the configuration parameters to be extracted
/// \param velIntervals of the configuration parameters to be extracted
SubchainPath (const PathPtr_t& original,
const segments_t& confIntervals,
const segments_t& velIntervals) :
Path (original->timeRange(), Eigen::BlockIndex::cardinal(confIntervals),
Eigen::BlockIndex::cardinal(velIntervals)),
original_ (original), configView_ (confIntervals),
q_ (Configuration_t::Zero(original->outputSize()))
{}
SubchainPath (const SubchainPath& path) : Path (path),
original_ (path.original_),
intervals_ (path.intervals_),
configView_ (path.configView_),
q_ (path.q_),
weak_ ()
{
......@@ -149,7 +147,7 @@ namespace hpp {
SubchainPath (const SubchainPath& path,
const ConstraintSetPtr_t& constraints) :
Path (path, constraints), original_ (path.original_),
intervals_ (path.intervals_), weak_ ()
configView_ (path.configView_), weak_ ()
{
}
......@@ -161,18 +159,9 @@ namespace hpp {
private:
PathPtr_t original_;
segments_t intervals_;
Eigen::RowBlockIndices configView_, velView_;
mutable Configuration_t q_;
SubchainPathWkPtr_t weak_;
static size_type intervalsToSize(const segments_t& ints)
{
size_type l = 0;
for (segments_t::const_iterator _rank = ints.begin();
_rank != ints.end(); ++_rank)
l += _rank->second;
return l;
}
}; // SubchainPath
/// \}
} // namespace core
......
......@@ -36,19 +36,22 @@ namespace hpp {
(const Problem& problem);
static WeighedDistancePtr_t create (const DevicePtr_t& robot);
static WeighedDistancePtr_t
createWithWeight (const DevicePtr_t& robot,
const std::vector <value_type>& weights);
createWithWeight (const DevicePtr_t& robot, const vector_t& weights);
static WeighedDistancePtr_t createCopy
(const WeighedDistancePtr_t& distance);
virtual DistancePtr_t clone () const;
/// Get weight of joint at given rank
/// \param rank rank of the joint in robot joint vector
value_type getWeight( std::size_t rank ) const;
value_type getWeight( size_type rank ) const;
/// Set weight of joint at given rank
/// \param rank rank of the joint in robot joint vector
void setWeight(std::size_t rank, value_type weight);
void setWeight(size_type rank, value_type weight);
/// Get weights
const vector_t& weights() const;
/// Set weights
void weights(const vector_t& ws);
/// Get size of weight vector
std::size_t size () const
size_type size () const
{
return weights_.size ();
}
......@@ -61,8 +64,7 @@ namespace hpp {
protected:
WeighedDistance (const Problem& problem);
WeighedDistance (const DevicePtr_t& robot);
WeighedDistance (const DevicePtr_t& robot,
const std::vector <value_type>& weights);
WeighedDistance (const DevicePtr_t& robot, const vector_t& weights);
WeighedDistance (const WeighedDistance& distance);
void init (WeighedDistanceWkPtr_t self);
/// Derived class should implement this function
......@@ -71,7 +73,7 @@ namespace hpp {
private:
void computeWeights ();
DevicePtr_t robot_;
std::vector <value_type> weights_;
vector_t weights_;
WeighedDistanceWkPtr_t weak_;
}; // class WeighedDistance
/// \}
......
# Copyright (c) 2019, Joseph Mirabel
# Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
#
# 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-plugin)
ADD_PLUGIN(spline-gradient-based
SOURCES spline-gradient-based.cc
LINK_DEPENDENCIES ${PROJECT_NAME} ${PROJECT_NAME}-gpl
PKG_CONFIG_DEPENDENCIES hpp-util hpp-pinocchio hpp-statistics hpp-constraints)
// Copyright (c) 2019, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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/plugin.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/core/path-optimization/spline-gradient-based.hh>
namespace hpp {
namespace core {
class SplineGradientBasedPlugin : public ProblemSolverPlugin
{
public:
SplineGradientBasedPlugin ()
: ProblemSolverPlugin ("SplineGradientBasedPlugin", "0.0")
{}
protected:
virtual bool impl_initialize (ProblemSolverPtr_t ps)
{
// ps->pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 1>::create);
// ps->pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 2>::create);
// ps->pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 3>::create);
ps->pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<path::BernsteinBasis, 1>::create);
// ps->pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<path::BernsteinBasis, 2>::create);
ps->pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<path::BernsteinBasis, 3>::create);
return true;
}
};
} // namespace core
} // namespace hpp
HPP_CORE_DEFINE_PLUGIN(hpp::core::SplineGradientBasedPlugin)
......@@ -59,9 +59,7 @@ path-validation/discretized-joint-bound.cc
path-optimizer.cc #
path-optimization/linear-constraint.cc #
path-optimization/spline-gradient-based-abstract.cc #
path-optimization/spline-gradient-based.cc #
path-optimization/partial-shortcut.cc #
path-optimization/quadratic-program.cc
path-optimization/random-shortcut.cc
path-optimization/simple-time-parameterization.cc#
path-planner.cc #
......@@ -70,6 +68,7 @@ path-validation/discretized-joint-bound.cc
path/spline.cc
path/hermite.cc
plan-and-optimize.cc #
plugin.cc # TODO : addObstacle
problem.cc # TODO : addObstacle
problem-solver.cc # TODO : addObstacle
path-projector.cc
......@@ -117,5 +116,19 @@ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-util)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-statistics)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-constraints)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS})
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)