Merge pull request #51 from dfki-ric/refactor/time_to_phase_mapping

[MRG] Simplify mapping from time to phase

benchmarks/benchmark_dmp_phase.py

@@ -8,9 +8,8 @@
 
 goal_t = 1.0
 start_t = 0.0
-int_dt = 0.001
-alpha = canonical_system_alpha(0.01, goal_t, start_t, int_dt)
-times = timeit.repeat(partial(phase_python, 0.5, alpha, goal_t, start_t, int_dt), repeat=1000, number=1000)
+alpha = canonical_system_alpha(0.01, goal_t, start_t)
+times = timeit.repeat(partial(phase_python, 0.5, alpha, goal_t, start_t), repeat=1000, number=1000)
 print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times)))
-times = timeit.repeat(partial(phase_cython, 0.5, alpha, goal_t, start_t, int_dt), repeat=1000, number=1000)
+times = timeit.repeat(partial(phase_cython, 0.5, alpha, goal_t, start_t), repeat=1000, number=1000)
 print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times)))

movement_primitives/dmp/_canonical_system.py

@@ -1,4 +1,7 @@
-def canonical_system_alpha(goal_z, goal_t, start_t, int_dt=0.001):
+import numpy as np
+
+
+def canonical_system_alpha(goal_z, goal_t, start_t):
     r"""Compute parameter alpha of canonical system.
 
     The parameter alpha is computed such that a specific phase value goal_z
@@ -17,9 +20,6 @@ def canonical_system_alpha(goal_z, goal_t, start_t, int_dt=0.001):
     start_t : float
         Time at which the execution should start.
 
-    int_dt : float, optional (default: 0.001)
-        Time delta that is used internally for integration.
-
     Returns
     -------
     alpha : float
@@ -43,14 +43,10 @@ def canonical_system_alpha(goal_z, goal_t, start_t, int_dt=0.001):
     if start_t >= goal_t:
         raise ValueError("Goal must be chronologically after start!")
 
-    execution_time = goal_t - start_t
-    n_phases = int(execution_time / int_dt) + 1
-    # assert that the execution_time is approximately divisible by int_dt
-    assert abs(((n_phases - 1) * int_dt) - execution_time) < 0.05
-    return (1.0 - goal_z ** (1.0 / (n_phases - 1))) * (n_phases - 1)
+    return float(-np.log(goal_z))
 
 
-def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10):
+def phase(t, alpha, goal_t, start_t):
     r"""Map time to phase.
 
     According to [1]_, the differential Equation
@@ -62,11 +58,11 @@ def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10):
     describes the evolution of the phase variable z. Starting from the initial
     position :math:`z_0 = 1`, the phase value converges monotonically to 0.
     Instead of using an iterative procedure to calculate the current value of
-    z, it is computed directly for a fixed :math:`\Delta t` through
+    z, it is computed directly through
 
     .. math::
 
-        (1 - \alpha_z \frac{\Delta t}{\tau})^{\frac{t}{\Delta t}}
+        z(t) = \exp - \frac{\alpha_z}{\tau} t
 
     Parameters
     ----------
@@ -82,12 +78,6 @@ def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10):
     start_t : float
         Time at which the execution should start.
 
-    int_dt : float, optional (default: 0.001)
-        Time delta that is used internally for integration.
-
-    eps : float, optional (default: 1e-10)
-        Small number used to avoid numerical issues.
-
     Returns
     -------
     z : float
@@ -102,5 +92,4 @@ def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10):
        https://homes.cs.washington.edu/~todorov/courses/amath579/reading/DynamicPrimitives.pdf
     """
     execution_time = goal_t - start_t
-    b = max(1.0 - alpha * int_dt / execution_time, eps)
-    return b ** ((t - start_t) / int_dt)
+    return np.exp(-alpha * (t - start_t) / execution_time)

movement_primitives/dmp/_cartesian_dmp.py

@@ -113,7 +113,7 @@ def dmp_step_quaternion_python(
             cd += coupling_term_precomputed[0]
             cdd += coupling_term_precomputed[1]
 
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f = forcing_term.forcing_term(z).squeeze()
 
         if smooth_scaling:
@@ -239,8 +239,7 @@ def __init__(
         self.beta_y = self.alpha_y / 4.0
 
     def _init_forcing_term(self):
-        alpha_z = canonical_system_alpha(
-            0.01, self.execution_time_, 0.0, self.int_dt)
+        alpha_z = canonical_system_alpha(0.01, self.execution_time_, 0.0)
         self.forcing_term_pos = ForcingTerm(
             3, self.n_weights_per_dim, self.execution_time_, 0.0, 0.8,
             alpha_z)

movement_primitives/dmp/_dmp.py

@@ -98,7 +98,7 @@ def dmp_step_rk4(
     dt = t - last_t
     dt_2 = 0.5 * dt
     T = np.array([t, t + dt_2, t + dt])
-    Z = forcing_term.phase(T, int_dt=int_dt)
+    Z = forcing_term.phase(T)
     F = forcing_term.forcing_term(Z)
     tdd = p_gain * tracking_error / dt
 
@@ -308,7 +308,7 @@ def dmp_step_euler(
                 cdd += coupling_term_precomputed[1]
         else:
             cd, cdd = None, None
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f = forcing_term.forcing_term(z).squeeze()
         tdd = p_gain * tracking_error / dt
 
@@ -420,8 +420,7 @@ def __init__(self, n_dims, execution_time=1.0, dt=0.01,
         self.beta_y = self.alpha_y / 4.0
 
     def _init_forcing_term(self):
-        alpha_z = canonical_system_alpha(
-            0.01, self.execution_time_, 0.0, self.int_dt)
+        alpha_z = canonical_system_alpha(0.01, self.execution_time_, 0.0)
         self.forcing_term = ForcingTerm(
             self.n_dims, self.n_weights_per_dim, self.execution_time_,
             0.0, 0.8, alpha_z)

movement_primitives/dmp/_dmp_with_final_velocity.py

@@ -67,8 +67,7 @@ def __init__(self, n_dims, execution_time=1.0, dt=0.01,
         self.beta_y = self.alpha_y / 4.0
 
     def _init_forcing_term(self):
-        alpha_z = canonical_system_alpha(0.01, self.execution_time_, 0.0,
-                                         self.int_dt)
+        alpha_z = canonical_system_alpha(0.01, self.execution_time_, 0.0)
         self.forcing_term = ForcingTerm(
             self.n_dims, self.n_weights_per_dim, self.execution_time_,
             0.0, 0.8, alpha_z)
@@ -433,7 +432,7 @@ def dmp_step_euler_with_constraints(
             cd += coupling_term_precomputed[0]
             cdd += coupling_term_precomputed[1]
 
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f = forcing_term.forcing_term(z).squeeze()
 
         g, gd, gdd = apply_constraints(current_t, goal_y, goal_t, coefficients)

movement_primitives/dmp/_dual_cartesian_dmp.py

@@ -106,7 +106,7 @@ def dmp_step_dual_cartesian_python(
         if coupling_term is not None:
             cd[:], cdd[:] = coupling_term.coupling(current_y, current_yd)
 
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f = forcing_term.forcing_term(z).squeeze()
         if tracking_error is not None:
             cdd[pvs] += p_gain * tracking_error[pps] / dt
@@ -243,8 +243,7 @@ def __init__(self, execution_time=1.0, dt=0.01, n_weights_per_dim=10,
         self.beta_y = self.alpha_y / 4.0
 
     def _init_forcing_term(self):
-        alpha_z = canonical_system_alpha(
-            0.01, self.execution_time_, 0.0, self.int_dt)
+        alpha_z = canonical_system_alpha(0.01, self.execution_time_, 0.0)
         self.forcing_term = ForcingTerm(
             12, self.n_weights_per_dim, self.execution_time_, 0.0, 0.8,
             alpha_z)

movement_primitives/dmp/_forcing_term.py

@@ -79,22 +79,21 @@ def _activations(self, z):
         activations /= activations.sum(axis=0)  # normalize
         return activations
 
-    def design_matrix(self, T, int_dt=0.001):  # returns: n_weights_per_dim x n_steps
-        Z = phase(T, alpha=self.alpha_z, goal_t=T[-1], start_t=T[0],
-                  int_dt=int_dt)
+    def design_matrix(self, T):  # returns: n_weights_per_dim x n_steps
+        Z = phase(T, alpha=self.alpha_z, goal_t=T[-1], start_t=T[0])
         return Z[np.newaxis, :] * self._activations(Z)
 
-    def phase(self, t, int_dt=0.001):
+    def phase(self, t):
         return phase(t, alpha=self.alpha_z, goal_t=self.goal_t,
-                     start_t=self.start_t, int_dt=int_dt)
+                     start_t=self.start_t)
 
     def forcing_term(self, z):
         z = np.atleast_1d(z)
         activations = self._activations(z)
         return z[np.newaxis, :] * self.weights_.dot(activations)
 
-    def __call__(self, t, int_dt=0.001):
-        return self.forcing_term(self.phase(t, int_dt))
+    def __call__(self, t):
+        return self.forcing_term(self.phase(t))
 
     @property
     def shape(self):

movement_primitives/dmp/_state_following_dmp.py

@@ -32,8 +32,7 @@ def __init__(self, n_dims, execution_time, dt=0.01, n_viapoints=10,
         self.n_viapoints = n_viapoints
         self.int_dt = int_dt
 
-        alpha_z = canonical_system_alpha(
-            0.01, self.execution_time, 0.0, self.int_dt)
+        alpha_z = canonical_system_alpha(0.01, self.execution_time, 0.0)
 
         self.alpha_y = 25.0
         self.beta_y = self.alpha_y / 4.0
@@ -122,7 +121,7 @@ def _activations(self, z, normalized):
 
     def __call__(self, t, int_dt=0.001):
         z = phase(t, alpha=self.alpha_z, goal_t=self.goal_t,
-                  start_t=self.start_t, int_dt=int_dt)
+                  start_t=self.start_t)
         z = np.atleast_1d(z)
         return self._activations(z, normalized=True).T
 

movement_primitives/dmp_fast.pyx

@@ -18,7 +18,7 @@ cdef double EPSILON = 1e-10
 @cython.wraparound(False)
 @cython.nonecheck(False)
 @cython.cdivision(True)
-cpdef phase(t, double alpha, double goal_t, double start_t, double int_dt=0.001, double eps=1e-10):
+cpdef phase(t, double alpha, double goal_t, double start_t):
     """Map time to phase.
 
     Parameters
@@ -35,20 +35,13 @@ cpdef phase(t, double alpha, double goal_t, double start_t, double int_dt=0.001,
     start_t : float
         Time at which the execution should start.
 
-    int_dt : float, optional (default: 0.001)
-        Time delta that is used internally for integration.
-
-    eps : float, optional (default: 1e-10)
-        Small number used to avoid numerical issues.
-
     Returns
     -------
     z : float
         Value of phase variable.
     """
     cdef double execution_time = goal_t - start_t
-    cdef double b = max(1.0 - alpha * int_dt / execution_time, eps)
-    return b ** ((t - start_t) / int_dt)
+    return np.exp(-alpha * (t - start_t) / execution_time)
 
 
 @cython.boundscheck(False)
@@ -179,7 +172,7 @@ cpdef dmp_step(
         if tracking_error is not None:
             cdd += p_gain * tracking_error / dt
 
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f[:] = forcing_term.forcing_term(z).squeeze()
 
         for d in range(n_dims):
@@ -300,7 +293,7 @@ cpdef dmp_step_rk4(
     cdef double dt = t - last_t
     cdef double dt_2 = 0.5 * dt
     cdef np.ndarray[double, ndim=1] T = np.array([t, t + dt_2, t + dt])
-    cdef np.ndarray[double, ndim=1] Z = forcing_term.phase(T, int_dt=int_dt)
+    cdef np.ndarray[double, ndim=1] Z = forcing_term.phase(T)
     cdef np.ndarray[double, ndim=2] F = forcing_term.forcing_term(Z)
     cdef np.ndarray[double, ndim=1] tdd
     if tracking_error is not None:
@@ -489,7 +482,7 @@ cpdef dmp_step_quaternion(
             cd[:] = coupling_term_precomputed[0]
             cdd[:] = coupling_term_precomputed[1]
 
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f[:] = forcing_term.forcing_term(z).squeeze()
 
         if smooth_scaling:
@@ -634,7 +627,7 @@ cpdef dmp_step_dual_cartesian(
         else:
             cd[:], cdd[:] = coupling_term.coupling(current_y, current_yd)
 
-        z = forcing_term.phase(current_t, int_dt)
+        z = forcing_term.phase(current_t)
         f[:] = forcing_term.forcing_term(z).squeeze()
         if tracking_error is not None:
             for pps, pvs in POS_INDICES:

movement_primitives/dmp_potential_field.py

@@ -54,7 +54,7 @@ def potential_field_2d(dmp, x_range, y_range, n_ticks, obstacle=None):
     Yd = np.empty_like(Y)
     Yd[:, :] = dmp.current_yd
 
-    z = dmp.forcing_term.phase(dmp.t, dmp.int_dt)
+    z = dmp.forcing_term.phase(dmp.t)
     ts = dmp_transformation_system(
         Y, Yd, dmp.alpha_y, dmp.beta_y, dmp.goal_y, dmp.goal_yd, dmp.goal_ydd,
         dmp.start_y, z, dmp.execution_time_)

movement_primitives/testing/simulation.py

@@ -161,7 +161,7 @@ def draw_trajectory(A2Bs, client_id, n_key_frames=10, s=1.0, lw=1):
         Line width
     """
     key_frames_indices = np.linspace(
-        0, len(A2Bs) - 1, n_key_frames, dtype=np.int)
+        0, len(A2Bs) - 1, n_key_frames, dtype=np.int64)
     for idx in key_frames_indices:
         draw_transform(A2Bs[idx], s=s, client_id=client_id)
     for idx in range(len(A2Bs) - 1):

test/test_canonical_system.py

@@ -9,8 +9,8 @@ def test_phase_cython():
     goal_t = 1.0
     start_t = 0.0
     int_dt = 0.001
-    alpha = canonical_system_alpha(0.01, goal_t, start_t, int_dt)
+    alpha = canonical_system_alpha(0.01, goal_t, start_t)
     for t in np.linspace(0, 1, 101):
-        z_python = phase_python(t, alpha, goal_t, start_t, int_dt)
-        z_cython = phase_cython(t, alpha, goal_t, start_t, int_dt)
+        z_python = phase_python(t, alpha, goal_t, start_t)
+        z_cython = phase_cython(t, alpha, goal_t, start_t)
         assert z_cython == pytest.approx(z_python)

test/test_cartesian_dmp.py

@@ -68,7 +68,7 @@ def test_compare_python_cython():
     from copy import deepcopy
     from movement_primitives.dmp._cartesian_dmp import dmp_step_quaternion_python
     from movement_primitives.dmp_fast import dmp_step_quaternion as dmp_step_quaternion_cython
-    alpha_z = canonical_system_alpha(0.01, 2.0, 0.0, 0.001)
+    alpha_z = canonical_system_alpha(0.01, 2.0, 0.0)
     forcing_term = ForcingTerm(3, 10, 2.0, 0.0, 0.8, alpha_z)
     kwargs = dict(
         last_t=1.999, t=2.0,

test/test_step_dual_cartesian.py

@@ -8,7 +8,7 @@
 def test_compare_python_cython():
     from movement_primitives.dmp._dual_cartesian_dmp import dmp_step_dual_cartesian_python
     from movement_primitives.dmp_fast import dmp_step_dual_cartesian as dmp_step_dual_cartesian_cython
-    alpha_z = canonical_system_alpha(0.01, 2.0, 0.0, 0.001)
+    alpha_z = canonical_system_alpha(0.01, 2.0, 0.0)
     forcing_term = ForcingTerm(12, 10, 2.0, 0.0, 0.8, alpha_z)
     kwargs = dict(
         last_t=1.999, t=2.0,