Commit 5cc58785 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

[BasicConfigurationShooter] Move code of shoot to src directory.

  - remove useless variable jv = robot_->getJointVector ();
parent dfdf2d7e
......@@ -44,31 +44,8 @@ namespace hpp {
ptr->init (shPtr);
return shPtr;
}
virtual ConfigurationPtr_t shoot () const
{
JointVector_t jv = robot_->getJointVector ();
size_type extraDim = robot_->extraConfigSpace ().dimension ();
size_type offset = robot_->configSize () - extraDim;
Configuration_t config(robot_->configSize ());
config.head(offset) = se3::randomConfiguration(robot_->model());
virtual ConfigurationPtr_t shoot () const;
// 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;
}
return boost::make_shared<Configuration_t>(config);
}
protected:
/// Uniformly sample configuration space
///
......
......@@ -21,6 +21,7 @@ SET(LIBRARY_NAME ${PROJECT_NAME})
SET(${LIBRARY_NAME}_SOURCES
astar.hh
basic-configuration-shooter.cc
bi-rrt-planner.cc
collision-validation.cc
config-projector.cc
......
//
// Copyright (c) 2018 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/>.
# include <hpp/core/basic-configuration-shooter.hh>
namespace hpp {
namespace core {
ConfigurationPtr_t BasicConfigurationShooter::shoot () const
{
size_type extraDim = robot_->extraConfigSpace ().dimension ();
size_type offset = robot_->configSize () - extraDim;
ConfigurationPtr_t config (new Configuration_t (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 ());
}
(*config) [offset + i] = lower + (upper - lower) * rand ()/RAND_MAX;
}
return config;
}
} // namespace core
} // namespace hpp
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