Commit 8fe8b08e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Move BasicConfigurationShooter to configurationShooter::Uniform and add

configuration::Gaussian
parent d1cfe58c
......@@ -52,7 +52,7 @@ SET(DOXYGEN_TREEVIEW "NO" CACHE STRING "Set to YES to generate a tree view in th
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/hpp/core/basic-configuration-shooter.hh
include/hpp/core/basic-configuration-shooter.hh # DEPRECATED
include/hpp/core/bi-rrt-planner.hh
include/hpp/core/collision-path-validation-report.hh
include/hpp/core/collision-validation.hh
......@@ -60,6 +60,8 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/collision-validation-report.hh
include/hpp/core/projection-error.hh
include/hpp/core/configuration-shooter.hh
include/hpp/core/configuration-shooter/uniform.hh
include/hpp/core/configuration-shooter/gaussian.hh
include/hpp/core/config-projector.hh
include/hpp/core/config-validation.hh
include/hpp/core/config-validations.hh
......
......@@ -19,50 +19,16 @@
#ifndef HPP_CORE_BASIC_CONFIGURATION_SHOOTER_HH
# define HPP_CORE_BASIC_CONFIGURATION_SHOOTER_HH
# include <sstream>
# warning "This file is deprecated. You should only include hpp/core/configuration-shooter/uniform.hh"
# include <pinocchio/algorithm/joint-configuration.hpp>
# include <hpp/pinocchio/device.hh>
# include <hpp/core/configuration-shooter.hh>
# include <hpp/core/configuration-shooter/uniform.hh>
namespace hpp {
namespace core {
/// \addtogroup configuration_sampling
/// \{
/// Uniformly sample with bounds of degrees of freedom.
class HPP_CORE_DLLAPI BasicConfigurationShooter :
public ConfigurationShooter
{
public:
static BasicConfigurationShooterPtr_t create (const DevicePtr_t& robot)
{
BasicConfigurationShooter* ptr = new BasicConfigurationShooter (robot);
BasicConfigurationShooterPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
virtual ConfigurationPtr_t shoot () const;
protected:
/// Uniformly sample configuration space
///
/// Note that translation joints have to be bounded.
BasicConfigurationShooter (const DevicePtr_t& robot) : robot_ (robot)
{
}
void init (const BasicConfigurationShooterPtr_t& self)
{
ConfigurationShooter::init (self);
weak_ = self;
}
private:
DevicePtr_t robot_;
BasicConfigurationShooterWkPtr_t weak_;
}; // class BasicConfigurationShooter
typedef configurationShooter::Uniform BasicConfigurationShooter;
typedef configurationShooter::UniformPtr_t BasicConfigurationShooterPtr_t;
/// \}
} // namespace core
} // namespace hpp
......
//
// Copyright (c) 2018 CNRS
// Authors: 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_CONFIGURATION_SHOOTER_GAUSSIAN_HH
# define HPP_CORE_CONFIGURATION_SHOOTER_GAUSSIAN_HH
# include <sstream>
# include <hpp/pinocchio/device.hh>
# include <hpp/core/configuration-shooter.hh>
namespace hpp {
namespace core {
namespace configurationShooter {
/// \addtogroup configuration_sampling
/// \{
/// Sample configuration using a gaussian distribution around a
/// configuration.
class HPP_CORE_DLLAPI Gaussian :
public ConfigurationShooter
{
public:
static GaussianPtr_t create (const DevicePtr_t& robot)
{
Gaussian* ptr = new Gaussian (robot);
GaussianPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
virtual ConfigurationPtr_t shoot () const;
void center (ConfigurationIn_t c)
{
center_ = c;
}
const Configuration_t& center () const
{
return center_;
}
/// Set the standard deviation proportional to a default value
///
/// The default value is:
/// \li for vector spaces, the difference between the upper and the
/// lower bounds,
/// \li for SO(n), \f$\frac{2\pi}{\sqrt{2n-3}}\f$ on each of the
/// \f$ 2n-3 \f$ dimensions,
/// \li SE(n) is treated as \f$ R^n \times SO(n) \f$.
void sigma (const value_type& factor);
void sigmas (vectorIn_t s)
{
assert (s.size() == robot_->numberDof());
sigmas_ = s;
}
const vector_t& sigmas () const
{
return sigmas_;
}
protected:
/// Create a gaussian distribution centered in the robot current
/// configuration. The standard deviation is computed as \c sigma(0.25)
/// \sa Gaussian::sigma
Gaussian (const DevicePtr_t& robot)
: robot_ (robot)
, center_ (robot->currentConfiguration())
, sigmas_ (robot->numberDof())
{
sigma(1./4.);
}
void init (const GaussianPtr_t& self)
{
ConfigurationShooter::init (self);
weak_ = self;
}
private:
const DevicePtr_t& robot_;
/// The mean value
Configuration_t center_;
/// The standard deviation
vector_t sigmas_;
GaussianWkPtr_t weak_;
}; // class Gaussian
/// \}
} // namespace configurationShooter
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONFIGURATION_SHOOTER_GAUSSIAN_HH
//
// Copyright (c) 2014 CNRS
// Authors: 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/>.
#ifndef HPP_CORE_CONFIGURATION_SHOOTER_BASIC_HH
# define HPP_CORE_CONFIGURATION_SHOOTER_BASIC_HH
# include <sstream>
# include <pinocchio/algorithm/joint-configuration.hpp>
# include <hpp/pinocchio/device.hh>
# include <hpp/core/configuration-shooter.hh>
namespace hpp {
namespace core {
namespace configurationShooter {
/// \addtogroup configuration_sampling
/// \{
/// Uniformly sample with bounds of degrees of freedom.
class HPP_CORE_DLLAPI Uniform : public ConfigurationShooter
{
public:
static UniformPtr_t create (const DevicePtr_t& robot)
{
Uniform* ptr = new Uniform (robot);
UniformPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
virtual ConfigurationPtr_t shoot () const;
protected:
/// Uniformly sample configuration space
///
/// Note that translation joints have to be bounded.
Uniform (const DevicePtr_t& robot) : robot_ (robot)
{
}
void init (const UniformPtr_t& self)
{
ConfigurationShooter::init (self);
weak_ = self;
}
private:
DevicePtr_t robot_;
UniformWkPtr_t weak_;
}; // class Uniform
/// \}
} // namespace configurationShooter
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONFIGURATION_SHOOTER_BASIC_HH
......@@ -30,7 +30,6 @@
namespace hpp {
namespace core {
HPP_PREDEF_CLASS (BasicConfigurationShooter);
HPP_PREDEF_CLASS (BiRRTPlanner);
HPP_PREDEF_CLASS (CollisionPathValidation);
struct CollisionPathValidationReport;
......@@ -87,8 +86,6 @@ namespace hpp {
typedef constraints::ComparisonTypes_t ComparisonTypes_t;
typedef constraints::ComparisonType ComparisonType;
typedef boost::shared_ptr < BasicConfigurationShooter >
BasicConfigurationShooterPtr_t;
typedef boost::shared_ptr <BiRRTPlanner> BiRRTPlannerPtr_t;
typedef hpp::pinocchio::Body Body;
typedef hpp::pinocchio::BodyPtr_t BodyPtr_t;
......@@ -309,6 +306,13 @@ namespace hpp {
typedef boost::shared_ptr <TaskTarget> TaskTargetPtr_t;
} // namespace problemTarget
namespace configurationShooter {
HPP_PREDEF_CLASS (Uniform);
typedef boost::shared_ptr < Uniform > UniformPtr_t;
HPP_PREDEF_CLASS (Gaussian);
typedef boost::shared_ptr < Gaussian > GaussianPtr_t;
} // namespace configurationShooter
typedef std::vector<core::vector3_t> Shape_t;
typedef std::pair <JointPtr_t, Shape_t> JointAndShape_t;
typedef std::list <JointAndShape_t> JointAndShapes_t;
......
......@@ -21,9 +21,10 @@ SET(LIBRARY_NAME ${PROJECT_NAME})
SET(${LIBRARY_NAME}_SOURCES
astar.hh
basic-configuration-shooter.cc
bi-rrt-planner.cc
collision-validation.cc
configuration-shooter/uniform.cc
configuration-shooter/gaussian.cc
config-projector.cc
config-validations.cc
connected-component.cc
......
......@@ -29,7 +29,7 @@
#include <hpp/core/problem.hh>
#include <hpp/core/roadmap.hh>
#include <hpp/core/steering-method.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/configuration-shooter.hh>
namespace hpp {
namespace core {
......
// Copyright (c) 2018, 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/configuration-shooter/gaussian.hh>
#include <math.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/multibody/model.hpp>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/liegroup.hh>
namespace hpp {
namespace core {
namespace configurationShooter {
namespace liegroup = pinocchio::liegroup;
template <typename LG1, typename LG2>
void computeSigmasAlgo (liegroup::CartesianProductOperation<LG1, LG2> lg,
vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower);
template <int N>
void computeSigmasAlgo (liegroup::SpecialOrthogonalOperation<N>,
vectorOut_t sigmas, vectorIn_t, vectorIn_t)
{
sigmas.setConstant (2*M_PI/std::sqrt((value_type)N));
}
template <int N>
void computeSigmasAlgo (liegroup::SpecialEuclideanOperation<N>,
vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower)
{
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<N,true>,
liegroup::SpecialOrthogonalOperation<N>
> LG_t;
computeSigmasAlgo (LG_t(), sigmas, upper, lower);
}
template <int N, bool rot>
void computeSigmasAlgo (liegroup::VectorSpaceOperation<N, rot>,
vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower)
{
// TODO isFinite was added after 3.2.4
// sigmas.array() =
// (upper.array().isFinite() && lower.array().isFinite())
// .select (upper - lower, 1);
for (size_type i = 0; i < sigmas.size(); ++i) {
if (Eigen::numext::isfinite (upper(i))
&& Eigen::numext::isfinite (lower(i)))
sigmas(i) = upper(i)-lower(i);
else
sigmas(i) = 1.;
}
}
template <typename LG1, typename LG2>
void computeSigmasAlgo (liegroup::CartesianProductOperation<LG1, LG2>,
vectorOut_t sigmas, vectorIn_t upper, vectorIn_t lower)
{
computeSigmasAlgo (LG1(), sigmas.head(LG1::NV), upper.head(LG1::NQ), lower.head(LG1::NQ));
computeSigmasAlgo (LG2(), sigmas.tail(LG2::NV), upper.tail(LG2::NQ), lower.tail(LG2::NQ));
}
struct ComputeSigmasStep : public se3::fusion::JointModelVisitor<ComputeSigmasStep>
{
typedef boost::fusion::vector<const se3::Model&, vector_t&> ArgsType;
JOINT_MODEL_VISITOR_INIT(ComputeSigmasStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
const se3::Model& model,
vector_t& sigmas)
{
typedef typename pinocchio::DefaultLieGroupMap::operation<JointModel>::type LG_t;
computeSigmasAlgo (LG_t(),
jmodel.jointVelocitySelector (sigmas),
jmodel.jointConfigSelector (model.upperPositionLimit),
jmodel.jointConfigSelector (model.lowerPositionLimit));
}
};
template<>
void ComputeSigmasStep::algo<se3::JointModelComposite>(const se3::JointModelBase<se3::JointModelComposite> & jmodel,
const se3::Model& model,
vector_t& sigmas)
{
se3::details::Dispatch<ComputeSigmasStep>::run(jmodel, ComputeSigmasStep::ArgsType(model, sigmas));
}
ConfigurationPtr_t Gaussian::shoot () const
{
boost::random::mt19937 eng;
vector_t velocity (robot_->numberDof());
for (size_type i = 0; i < velocity.size(); ++i)
{
boost::random::normal_distribution<value_type> distrib(0, sigmas_[i]);
velocity[i] = distrib (eng);
}
ConfigurationPtr_t config(new Configuration_t(robot_->configSize ()));
::hpp::pinocchio::integrate (robot_, center_, velocity, *config);
::hpp::pinocchio::saturate (robot_, *config);
return config;
}
void Gaussian::sigma(const value_type& factor)
{
const se3::Model& model = robot_->model();
ComputeSigmasStep::ArgsType args (model, sigmas_);
for(std::size_t i = 1; i < model.joints.size(); ++i)
ComputeSigmasStep::run (model.joints[i], args);
sigmas_ *= factor;
}
} // namespace configurationShooter
} // namespace core
} // namespace hpp
......@@ -16,35 +16,37 @@
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
# include <hpp/core/basic-configuration-shooter.hh>
# include <hpp/core/configuration-shooter/uniform.hh>
namespace hpp {
namespace core {
namespace configurationShooter {
ConfigurationPtr_t BasicConfigurationShooter::shoot () const
{
size_type extraDim = robot_->extraConfigSpace ().dimension ();
size_type offset = robot_->configSize () - extraDim;
ConfigurationPtr_t Uniform::shoot () const
{
size_type extraDim = robot_->extraConfigSpace ().dimension ();
size_type offset = robot_->configSize () - extraDim;
Configuration_t config(robot_->configSize ());
config.head (offset) = se3::randomConfiguration(robot_->model());
Configuration_t config(robot_->configSize ());
config.head (offset) = se3::randomConfiguration(robot_->model());
// Shoot extra configuration variables
for (size_type i=0; i<extraDim; ++i) {
value_type lower = robot_->extraConfigSpace ().lower (i);
value_type upper = robot_->extraConfigSpace ().upper (i);
value_type range = upper - lower;
if ((range < 0) ||
(range == std::numeric_limits<double>::infinity())) {
std::ostringstream oss
("Cannot uniformy sample extra config variable ");
oss << i << ". min = " <<lower<< ", max = " << upper << std::endl;
throw std::runtime_error (oss.str ());
// Shoot extra configuration variables
for (size_type i=0; i<extraDim; ++i) {
value_type lower = robot_->extraConfigSpace ().lower (i);
value_type upper = robot_->extraConfigSpace ().upper (i);
value_type range = upper - lower;
if ((range < 0) ||
(range == std::numeric_limits<double>::infinity())) {
std::ostringstream oss
("Cannot uniformy sample extra config variable ");
oss << i << ". min = " <<lower<< ", max = " << upper << std::endl;
throw std::runtime_error (oss.str ());
}
config [offset + i] = lower + (upper - lower) * rand ()/RAND_MAX;
}
config [offset + i] = lower + (upper - lower) * rand ()/RAND_MAX;
return boost::make_shared<Configuration_t> (config);
}
return boost::make_shared<Configuration_t> (config);
}
} // namespace core
} // namespace configurationShooter
} // namespace core
} // namespace hpp
......@@ -19,6 +19,8 @@
#include <hpp/core/diffusing-planner.hh>
#include <boost/tuple/tuple.hpp>
#include <boost/next_prior.hpp>
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <hpp/pinocchio/configuration.hh>
......@@ -33,7 +35,7 @@
#include <hpp/core/problem.hh>
#include <hpp/core/roadmap.hh>
#include <hpp/core/steering-method.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/configuration-shooter.hh>
namespace hpp {
namespace core {
......
......@@ -33,8 +33,9 @@
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/configuration-shooter/uniform.hh>
#include <hpp/core/bi-rrt-planner.hh>
#include <hpp/core/configuration-shooter/gaussian.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/continuous-collision-checking/dichotomy.hh>
......@@ -152,7 +153,7 @@ namespace hpp {
target_ (problemTarget::GoalConfigurations::create(NULL)),
initConf_ (), goalConfigurations_ (),
robotType_ ("hpp::pinocchio::Device"),
configurationShooterType_ ("BasicConfigurationShooter"),
configurationShooterType_ ("Uniform"),
distanceType_("WeighedDistance"),
steeringMethodType_ ("Straight"),
pathOptimizerTypes_ (), pathOptimizers_ (),
......@@ -173,7 +174,8 @@ namespace hpp {
pathPlanners.add ("BiRRTPlanner", BiRRTPlanner::createWithRoadmap);
pathPlanners.add ("kPRM*", pathPlanner::kPrmStar::createWithRoadmap);
configurationShooters.add ("BasicConfigurationShooter", BasicConfigurationShooter::create);
configurationShooters.add ("Uniform" , configurationShooter::Uniform ::create);
configurationShooters.add ("Gaussian", configurationShooter::Gaussian::create);
distances.add ("Weighed", WeighedDistance::createFromProblem);
distances.add ("ReedsShepp", bind (distance::ReedsShepp::create, _1));
......
......@@ -31,7 +31,7 @@
#include <hpp/core/discretized-collision-checking.hh>
#include <hpp/core/continuous-collision-checking/dichotomy.hh>
#include <hpp/core/continuous-collision-checking/progressive.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/configuration-shooter/uniform.hh>
namespace hpp {
namespace core {
......@@ -47,7 +47,7 @@ namespace hpp {
pathValidation_ (DiscretizedCollisionChecking::create
(robot, 0.05)),
collisionObstacles_ (), constraints_ (),
configurationShooter_(BasicConfigurationShooter::create (robot))
configurationShooter_(configurationShooter::Uniform::create (robot))
{
resetConfigValidations();
......
......@@ -26,7 +26,7 @@
#include <hpp/core/problem.hh>
#include <hpp/core/roadmap.hh>
#include <hpp/core/visibility-prm-planner.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/configuration-shooter.hh>
#include <stdio.h>
#include <time.h>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment