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

src/extracted-path.hh

@@ -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 ());

src/path-optimization/simple-time-parameterization.cc

@@ -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;