Commit 01876e31 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Minor] Implement ExtractedPath::impl_velocityBound + bug in time param.

parent 7ebafa59
Pipeline #13338 failed with stage
in 1 minute and 5 seconds
......@@ -94,6 +94,14 @@ namespace hpp {
}
}
void impl_velocityBound (vectorOut_t result, const value_type& t0, const value_type& t1) const override
{
value_type tmin = sInOriginalPath (t0),
tmax = sInOriginalPath (t1);
if (tmin > tmax) std::swap (tmin, tmax);
original_->velocityBound(result, tmin, tmax);
}
virtual PathPtr_t impl_extract (const interval_t& subInterval) const
{
ExtractedPathPtr_t path = createCopy (weak_.lock ());
......
......@@ -214,6 +214,11 @@ namespace hpp {
interval_t paramRange = p->paramRange();
p->timeParameterization (TimeParameterizationPtr_t(), paramRange);
PathPtr_t pp = p->copy();
if (p->length() > 0) {
output->appendPath (pp);
continue;
}
// Compute B
p->velocityBound (v, paramRange.first, paramRange.second);
v_inv = (v.array() == 0).select(infinity, v.cwiseInverse());
......@@ -245,9 +250,7 @@ namespace hpp {
checkTimeParameterization (tp, order, paramRange, B, maxAcc, T);
PathPtr_t pp = p->copy();
pp->timeParameterization (tp, interval_t (0, T));
output->appendPath (pp);
}
return output;
......
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