Merge pull request AtsushiSakai#152 from jwdinius/eta3_traj2

polishing up continuous velocity profile overlay on path

Author: AtsushiSakai

PathPlanning/Eta3SplinePath/eta3_spline_path.py

@@ -152,10 +152,10 @@ def __init__(self, start_pose, end_pose, eta=None, kappa=None):
             + (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
             - (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
                kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
-
-        def s_dot(u): return np.linalg.norm(self.coeffs[:, 1:].dot(
-            np.array([1, 2. * u, 3. * u**2, 4. * u**3, 5. * u**4, 6. * u**5, 7. * u**6])))
-        self.segment_length = quad(lambda u: s_dot(u), 0, 1)[0]
+        
+        self.s_dot = lambda u : max(np.linalg.norm(self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))), 1e-6)
+        self.f_length = lambda ue: quad(lambda u: self.s_dot(u), 0, ue)
+        self.segment_length = self.f_length(1)[0]
 
     """
     eta3_path_segment::calc_point
@@ -170,6 +170,21 @@ def calc_point(self, u):
         assert(u >= 0 and u <= 1)
         return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
 
+    """
+    eta3_path_segment::calc_deriv
+
+    input
+        u - parametric representation of a point along the segment, 0 <= u <= 1
+    returns
+        (d^nx/du^n,d^ny/du^n) of point along the segment, for 0 < n <= 2
+    """
+    def calc_deriv(self, u, order=1):
+        assert(u >= 0 and u <= 1)
+        assert(order > 0 and order <= 2)
+        if order == 1:
+            return self.coeffs[:, 1:].dot(np.array([1, 2.*u, 3.*u**2, 4.*u**3, 5.*u**4, 6.*u**5, 7.*u**6]))
+        else:
+            return self.coeffs[:, 2:].dot(np.array([2, 6.*u, 12.*u**2, 20.*u**3, 30.*u**4, 42.*u**5]))
 
 def test1():