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

[SimpleTimeParameterization] Throw if velocities of device are not bounded.

  add also a few assertions.
parent 1477d8f9
......@@ -31,7 +31,13 @@ namespace hpp {
class HPP_CORE_DLLAPI Polynomial : public TimeParameterization
{
public:
Polynomial (const vector_t& param) : a (param) {}
Polynomial (const vector_t& param) : a (param)
{
for (size_type i=0; i<a.size(); ++i){
assert(a[i] < std::numeric_limits<value_type>::infinity());
assert(a[i] > -std::numeric_limits<value_type>::infinity());
}
}
const vector_t& parameters () const
{
......@@ -102,6 +108,7 @@ namespace hpp {
tn *= t;
res += a[i] * tn;
}
assert (!std::isnan(res));
return res;
}
......
......@@ -151,6 +151,16 @@ namespace hpp {
vector_t ub ( robot->model().velocityLimit),
lb (-robot->model().velocityLimit),
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());
}
}
assert (cb.size() + robot->extraConfigSpace().dimension()
== robot->numberDof());
......
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