Add simple test

traj/src/traj/discrete_time_parameterization.py

@@ -9,10 +9,10 @@
 POSITION_THRESHOLD = 0.01
 VELOCITY_THRESHOLD = 0.01
 ACCELERATION_THRESHOLD = 0.01
+JERK_THRESHOLD = 0.01
 
 
-def compute_stopping_trajectory(
-        p_start, p_end, v_start, a_start, is_valid, j_max, delta_t):
+def compute_stopping_trajectory(p_start, v_start, a_start, is_valid, j_max, delta_t):
     positions = np.zeros(MAX_TIME_STEPS)
     velocities = np.zeros(MAX_TIME_STEPS)
     accelerations = np.zeros(MAX_TIME_STEPS)
@@ -68,7 +68,7 @@ def compute_stopping_trajectory(
             if accelerations[time_i - 1] > -ACCELERATION_THRESHOLD:
                 return (positions[1:time_i], velocities[1:time_i], accelerations[1:time_i],
                         jerks[1:time_i])
-            elif velocities[time_i - 1] < 0.0:
+            elif velocities[time_i - 1] < -VELOCITY_THRESHOLD:
                 # We weren't reduce acceleration magnitude to zero before velocity hit zero.
                 break
 
@@ -121,7 +121,7 @@ def parameterize_path_discrete(p_start, p_end, is_valid, j_max, delta_t):
                 continue
 
             stopping_trajectory = compute_stopping_trajectory(
-                next_position, p_end, next_velocity, next_acceleration,
+                next_position, next_velocity, next_acceleration,
                 is_valid, j_max, delta_t)
             if stopping_trajectory is None:
                 # There will be no valid way to stop if we apply this jerk.

traj/test/test_discrete_parameterization.py

@@ -1,3 +1,26 @@
 import numpy as np
 
-def test_stopping_
+import traj.discrete_time_parameterization
+
+def simple_is_valid(position, velocity, acceleration, jerk):
+    """
+    Checks values against constant bounds.
+    """
+    p_end = 10.0
+    v_max = 10.0
+    a_max = 10.0
+    j_max = 10.0
+    if position < 0.0 or position > p_end + traj.discrete_time_parameterization.POSITION_THRESHOLD:
+        return False
+    if velocity > v_max + traj.discrete_time_parameterization.VELOCITY_THRESHOLD:
+        return False
+    if np.abs(acceleration) > a_max + traj.discrete_time_parameterization.ACCELERATION_THRESHOLD:
+        return False
+    if np.abs(jerk) > j_max + traj.discrete_time_parameterization.JERK_THRESHOLD:
+        return False
+    return True
+
+
+def test_stop_from_stopped():
+    assert traj.discrete_time_parameterization.compute_stopping_trajectory(
+        0.0, 0.0, 0.0, simple_is_valid, j_max=10.0, delta_t=0.001) is not None