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

Merge remote-tracking branch 'jmirabel/devel' into devel

parents 6be357c4 9108a8c7
......@@ -44,7 +44,11 @@ namespace hpp {
/// Shoot a random configuration
/// \param q the configuration (resized if necessary).
virtual void shoot (Configuration_t& q) const = 0;
///
/// \deprecated This method is virtual for backward compatibility. It will
/// become non-virtual in the future. Child classes should rather implement
/// \ref impl_shoot so that both prototype of method shoot remain available.
virtual void shoot (Configuration_t& q) const { impl_shoot(q); }
virtual ~ConfigurationShooter () {};
protected:
......@@ -56,6 +60,8 @@ namespace hpp {
{
weakPtr_ = weak;
}
virtual void impl_shoot (Configuration_t& q) const = 0;
private:
ConfigurationShooterWkPtr_t weakPtr_;
}; // class
......
......@@ -44,7 +44,6 @@ namespace hpp {
ptr->init (shPtr);
return shPtr;
}
virtual void shoot (Configuration_t& q) const;
void center (ConfigurationIn_t c)
{
......@@ -92,6 +91,7 @@ namespace hpp {
weak_ = self;
}
virtual void impl_shoot (Configuration_t& q) const;
private:
const DevicePtr_t& robot_;
/// The mean value
......
......@@ -42,7 +42,6 @@ namespace hpp {
ptr->init (shPtr);
return shPtr;
}
virtual void shoot (Configuration_t& q) const;
void sampleExtraDOF(bool sampleExtraDOF){
sampleExtraDOF_=sampleExtraDOF;
......@@ -61,6 +60,7 @@ namespace hpp {
weak_ = self;
}
virtual void impl_shoot (Configuration_t& q) const;
private:
DevicePtr_t robot_;
bool sampleExtraDOF_;
......
......@@ -500,6 +500,11 @@ namespace hpp {
{
return distanceBetweenObjects_;
}
pinocchio::GeomModelPtr_t obstacleGeomModel () const
{ return obstacleModel_; }
pinocchio::GeomDataPtr_t obstacleGeomData () const
{ return obstacleData_; }
/// \}
/// Local vector of objects considered for collision detection
......
......@@ -108,7 +108,7 @@ namespace hpp {
tn *= t;
res += a[i] * tn;
}
assert (!std::isnan(res));
assert (res == res);
return res;
}
......
......@@ -106,7 +106,7 @@ namespace hpp {
::pinocchio::details::Dispatch<ComputeSigmasStep>::run(jmodel.derived(), ComputeSigmasStep::ArgsType(model, sigmas));
}
void Gaussian::shoot (Configuration_t& config) const
void Gaussian::impl_shoot (Configuration_t& config) const
{
static boost::random::mt19937 eng;
vector_t velocity (robot_->numberDof());
......
......@@ -26,7 +26,7 @@ namespace hpp {
namespace core {
namespace configurationShooter {
void Uniform::shoot (Configuration_t& config) const
void Uniform::impl_shoot (Configuration_t& config) const
{
size_type extraDim = robot_->extraConfigSpace ().dimension ();
size_type offset = robot_->configSize () - extraDim;
......
......@@ -45,6 +45,10 @@ namespace hpp {
a[1] = B;
hppDout (info, "Time parametrization returned " << a.transpose()
<< ", " << T);
if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
HPP_THROW(std::logic_error, "Invalid time parameterization "
"coefficients: " << a.transpose());
}
return TimeParameterizationPtr_t (new Polynomial (a));
}
......@@ -60,6 +64,10 @@ namespace hpp {
a[3] = - 2 * a[2] / (3 * T);
hppDout (info, "Time parametrization returned " << a.transpose()
<< ", " << T);
if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
HPP_THROW(std::logic_error, "Invalid time parameterization "
"coefficients: " << a.transpose());
}
return TimeParameterizationPtr_t (new Polynomial (a));
}
......@@ -88,6 +96,10 @@ namespace hpp {
a[5] = 6 * (s1 - s0) / Tpow;
hppDout (info, "Time parametrization returned " << a.transpose()
<< ", " << T);
if (!isfinite(T) || !a.allFinite() || a.hasNaN()) {
HPP_THROW(std::logic_error, "Invalid time parameterization "
"coefficients: " << a.transpose());
}
return TimeParameterizationPtr_t (new Polynomial (a));
}
......@@ -98,7 +110,9 @@ namespace hpp {
const value_type thr = std::sqrt(Eigen::NumTraits<value_type>::dummy_precision());
if ( fabs(tp->value(0) - sr.first) >= thr
|| fabs(tp->value(T) - sr.second) >= thr) {
throw std::logic_error("Boundaries of TimeParameterization are not correct.");
HPP_THROW(std::logic_error, "Interval of TimeParameterization result"
" is not correct. Expected " << sr.first << ", " << sr.second <<
". Got " << tp->value(0) << ", " << tp->value(T));
}
if (order >= 1
&& (fabs(tp->derivative(0, 1)) > thr
......@@ -153,12 +167,11 @@ namespace hpp {
cb ((ub + lb) / 2);
for (size_type i=0; i < cb.size(); ++i) {
if (std::isnan (cb [i])) {
std::ostringstream oss;
oss << "in SimpleTimeParameterization::optimize:\n";
oss << " the velocities of the input device should be bounded\n";
oss << " velocity bounds at rank " << i << " are [" << lb[i]
<< ", " << ub[i] << "].";
throw std::runtime_error(oss.str());
HPP_THROW(std::runtime_error,
"in SimpleTimeParameterization::optimize:\n"
<< " the velocities of the input device should be bounded\n"
<< " velocity bounds at rank " << i << " are [" << lb[i]
<< ", " << ub[i] << "].");
}
}
assert (cb.size() + robot->extraConfigSpace().dimension()
......@@ -198,7 +211,10 @@ namespace hpp {
const value_type B = std::min(
( ub.cwiseProduct(v_inv)).minCoeff(),
(-lb.cwiseProduct(v_inv)).minCoeff());
assert (B > 0);
if (B <= 0 || B != B) {
HPP_THROW(std::runtime_error,"Invalid parametrization derivative "
"velocity bound: " << B);
}
// Compute the polynom and total time
value_type T;
......
Markdown is supported
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