Make smoothing optional

examples/plot_dmp_scaling.py

@@ -16,27 +16,28 @@
 """
 import numpy as np
 import matplotlib.pyplot as plt
-from movement_primitives.dmp import DMP, CartesianDMP, DualCartesianDMP
+from movement_primitives.dmp import DMP, DMPWithFinalVelocity, CartesianDMP, DualCartesianDMP
 
 
 T = np.linspace(0, 1, 101)
 x = np.sin(T ** 2 * 1.99 * np.pi)
 y = np.cos(T ** 2 * 1.99 * np.pi)
 z = qx = qy = qz = np.zeros_like(x)
 qw = np.ones_like(x)
-Y = np.column_stack((x, y, z, qw, qx, qy, qz))#, x, y, z, qw, qx, qy, qz))
+Y = np.column_stack((x, y, z, qw, qx, qy, qz, x, y, z, qw, qx, qy, qz))
 start = Y[0]
 goal = Y[-1]
-new_start = np.array([0.5, 0.5, 0, 1, 0, 0, 0])#, 0.5, 0.5, 0, 1, 0, 0, 0])
-new_goal = np.array([-0.5, 0.5, 0, 1, 0, 0, 0])#, -0.5, 0.5, 0, 1, 0, 0, 0])
+new_start = np.array([0.5, 0.5, 0, 1, 0, 0, 0, 0.5, 0.5, 0, 1, 0, 0, 0])
+new_goal = np.array([-0.5, 0.5, 0, 1, 0, 0, 0, -0.5, 0.5, 0, 1, 0, 0, 0])
 Y_shifted = Y - goal[np.newaxis] + new_goal[np.newaxis]
 
 #dmp = DMP(n_dims=len(start), execution_time=1.0, dt=0.01, n_weights_per_dim=20, smooth_scaling=False)
-dmp = CartesianDMP(execution_time=1.0, dt=0.01, n_weights_per_dim=20, smooth_scaling=True)
-#dmp = DualCartesianDMP(execution_time=1.0, dt=0.01, n_weights_per_dim=20)
+#dmp = DMPWithFinalVelocity(n_dims=len(start), execution_time=1.0, dt=0.01, n_weights_per_dim=20, smooth_scaling=False)
+#dmp = CartesianDMP(execution_time=1.0, dt=0.01, n_weights_per_dim=20, smooth_scaling=True)
+dmp = DualCartesianDMP(execution_time=1.0, dt=0.01, n_weights_per_dim=20, smooth_scaling=True)
 dmp.imitate(T, Y)
 dmp.configure(start_y=new_start, goal_y=new_goal)
-_, Y_dmp = dmp.open_loop(quaternion_step_function="cython")
+_, Y_dmp = dmp.open_loop(step_function="cython")
 
 plt.plot(Y[:, 0], Y[:, 1], label=r"Demonstration, $g \approx y_0$", ls="--")
 plt.plot(Y_shifted[:, 0], Y_shifted[:, 1], label="Original shape with new goal", ls="--")

movement_primitives/dmp/_cartesian_dmp.py

@@ -116,9 +116,9 @@ def dmp_step_quaternion_python(
         z = forcing_term.phase(current_t, int_dt)
         f = forcing_term.forcing_term(z).squeeze()
 
-        goal_y_minus_start_y = pr.compact_axis_angle_from_quaternion(pr.concatenate_quaternions(goal_y, pr.q_conj(start_y)))
-
         if smooth_scaling:
+            goal_y_minus_start_y = pr.compact_axis_angle_from_quaternion(
+                pr.concatenate_quaternions(goal_y, pr.q_conj(start_y)))
             smoothing = beta_y * z * goal_y_minus_start_y
         else:
             smoothing = 0.0

movement_primitives/dmp/_dmp_with_final_velocity.py

@@ -36,6 +36,11 @@ class DMPWithFinalVelocity(WeightParametersMixin, DMPBase):
         Gain for proportional controller of DMP tracking error.
         The domain is [0, execution_time**2/dt].
 
+    smooth_scaling : bool, optional (default: False)
+        Avoids jumps during the beginning of DMP execution when the goal
+        is changed and the trajectory is scaled by interpolating between
+        the old and new scaling of the trajectory.
+
     Attributes
     ----------
     execution_time_ : float
@@ -46,13 +51,15 @@ class DMPWithFinalVelocity(WeightParametersMixin, DMPBase):
         the frequency.
     """
     def __init__(self, n_dims, execution_time=1.0, dt=0.01,
-                 n_weights_per_dim=10, int_dt=0.001, p_gain=0.0):
+                 n_weights_per_dim=10, int_dt=0.001, p_gain=0.0,
+                 smooth_scaling=False):
         super(DMPWithFinalVelocity, self).__init__(n_dims, n_dims)
         self._execution_time = execution_time
         self.dt_ = dt
         self.n_weights_per_dim = n_weights_per_dim
         self.int_dt = int_dt
         self.p_gain = p_gain
+        self.smooth_scaling = smooth_scaling
 
         self._init_forcing_term()
 
@@ -121,7 +128,8 @@ def step(self, last_y, last_yd, coupling_term=None):
             coupling_term=coupling_term,
             int_dt=self.int_dt,
             p_gain=self.p_gain,
-            tracking_error=tracking_error)
+            tracking_error=tracking_error,
+            smooth_scaling=self.smooth_scaling)
         return np.copy(self.current_y), np.copy(self.current_yd)
 
     def open_loop(self, run_t=None, coupling_term=None):
@@ -152,7 +160,8 @@ def open_loop(self, run_t=None, coupling_term=None):
             run_t, self.int_dt,
             dmp_step_euler_with_constraints,
             start_yd=self.start_yd, start_ydd=self.start_ydd,
-            goal_yd=self.goal_yd, goal_ydd=self.goal_ydd)
+            goal_yd=self.goal_yd, goal_ydd=self.goal_ydd,
+            smooth_scaling=self.smooth_scaling)
 
     def imitate(self, T, Y, regularization_coefficient=0.0):
         """Imitate demonstration.
@@ -309,7 +318,7 @@ def dmp_step_euler_with_constraints(
         last_t, t, current_y, current_yd, goal_y, goal_yd, goal_ydd,
         start_y, start_yd, start_ydd, goal_t, start_t, alpha_y, beta_y,
         forcing_term, coupling_term=None, coupling_term_precomputed=None,
-        int_dt=0.001, p_gain=0.0, tracking_error=0.0):
+        int_dt=0.001, p_gain=0.0, tracking_error=0.0, smooth_scaling=False):
     """Integrate regular DMP for one step with Euler integration.
 
     Parameters
@@ -375,6 +384,11 @@ def dmp_step_euler_with_constraints(
 
     tracking_error : float, optional (default: 0)
         Tracking error from last step.
+
+    smooth_scaling : bool, optional (default: False)
+        Avoids jumps during the beginning of DMP execution when the goal
+        is changed and the trajectory is scaled by interpolating between
+        the old and new scaling of the trajectory.
     """
     if start_t >= goal_t:
         raise ValueError("Goal must be chronologically after start!")
@@ -408,13 +422,18 @@ def dmp_step_euler_with_constraints(
 
         g, gd, gdd = apply_constraints(current_t, goal_y, goal_t, coefficients)
 
+        if smooth_scaling:
+            smoothing = beta_y * (g - start_y) * z
+        else:
+            smoothing = 0.0
+
         coupling_sum = cdd + p_gain * tracking_error / dt
         ydd = (
             alpha_y * (
                 beta_y * (g - current_y)
                 + execution_time * gd
                 - execution_time * current_yd
-                - beta_y * (g - start_y) * z
+                - smoothing
             )
             + gdd * execution_time ** 2
             + f + coupling_sum

movement_primitives/dmp/_dual_cartesian_dmp.py

@@ -18,7 +18,7 @@ def dmp_step_dual_cartesian_python(
         start_y, start_yd, start_ydd,
         goal_t, start_t, alpha_y, beta_y,
         forcing_term, coupling_term=None, int_dt=0.001,
-        p_gain=0.0, tracking_error=None):
+        p_gain=0.0, tracking_error=None, smooth_scaling=False):
     """Integrate bimanual Cartesian DMP for one step with Euler integration.
 
     Parameters
@@ -80,6 +80,11 @@ def dmp_step_dual_cartesian_python(
 
     tracking_error : float, optional (default: 0)
         Tracking error from last step.
+
+    smooth_scaling : bool, optional (default: False)
+        Avoids jumps during the beginning of DMP execution when the goal
+        is changed and the trajectory is scaled by interpolating between
+        the old and new scaling of the trajectory.
     """
     if t <= start_t:
         current_y[:] = start_y
@@ -110,13 +115,18 @@ def dmp_step_dual_cartesian_python(
                 cdd[ovs] += p_gain * pr.compact_axis_angle_from_quaternion(
                     tracking_error[ops]) / dt
 
+        if smooth_scaling:
+            smoothing = beta_y * (goal_y[pps] - start_y[pps]) * z
+        else:
+            smoothing = 0.0
+
         # position components
         current_ydd[pvs] = (
             alpha_y * (
                 beta_y * (goal_y[pps] - current_y[pps])
                 + execution_time * goal_yd[pvs]
                 - execution_time * current_yd[pvs]
-                - beta_y * (goal_y[pps] - start_y[pps]) * z
+                - smoothing
             )
             + goal_ydd[pvs] * execution_time ** 2
             + f[pvs]
@@ -128,15 +138,19 @@ def dmp_step_dual_cartesian_python(
         # orientation components
         for ops, ovs in ((slice(3, 7), slice(3, 6)),
                          (slice(10, 14), slice(9, 12))):
-            goal_y_minus_start_y = pr.compact_axis_angle_from_quaternion(
-                pr.concatenate_quaternions(goal_y[ops], pr.q_conj(start_y[ops])))
+            if smooth_scaling:
+                goal_y_minus_start_y = pr.compact_axis_angle_from_quaternion(
+                    pr.concatenate_quaternions(goal_y[ops], pr.q_conj(start_y[ops])))
+                smoothing = beta_y * z * goal_y_minus_start_y
+            else:
+                smoothing = 0.0
             current_ydd[ovs] = (
                 alpha_y * (
                     beta_y * pr.compact_axis_angle_from_quaternion(pr.concatenate_quaternions(
                         goal_y[ops], pr.q_conj(current_y[ops])))
                     + execution_time * goal_yd[ovs]
                     - execution_time * current_yd[ovs]
-                    - beta_y * z * goal_y_minus_start_y
+                    - smoothing
                 )
                 + goal_ydd[ovs] * execution_time ** 2
                 + f[ovs]
@@ -194,6 +208,11 @@ class DualCartesianDMP(WeightParametersMixin, DMPBase):
         Gain for proportional controller of DMP tracking error.
         The domain is [0, execution_time**2/dt].
 
+    smooth_scaling : bool, optional (default: False)
+        Avoids jumps during the beginning of DMP execution when the goal
+        is changed and the trajectory is scaled by interpolating between
+        the old and new scaling of the trajectory.
+
     Attributes
     ----------
     execution_time_ : float
@@ -204,13 +223,14 @@ class DualCartesianDMP(WeightParametersMixin, DMPBase):
         the frequency.
     """
     def __init__(self, execution_time=1.0, dt=0.01, n_weights_per_dim=10,
-                 int_dt=0.001, p_gain=0.0):
+                 int_dt=0.001, p_gain=0.0, smooth_scaling=False):
         super(DualCartesianDMP, self).__init__(14, 12)
         self._execution_time = execution_time
         self.dt_ = dt
         self.n_weights_per_dim = n_weights_per_dim
         self.int_dt = int_dt
         self.p_gain = p_gain
+        self.smooth_scaling = smooth_scaling
 
         self._init_forcing_term()
 
@@ -286,7 +306,8 @@ def step(self, last_y, last_yd, coupling_term=None,
             self.alpha_y, self.beta_y,
             self.forcing_term, coupling_term,
             self.int_dt,
-            self.p_gain, tracking_error)
+            self.p_gain, tracking_error,
+            self.smooth_scaling)
 
         return np.copy(self.current_y), np.copy(self.current_yd)
 

movement_primitives/dmp_fast.pyx

@@ -494,14 +494,15 @@ cpdef dmp_step_quaternion(
         z = forcing_term.phase(current_t, int_dt)
         f[:] = forcing_term.forcing_term(z).squeeze()
 
-        goal_y_minus_start_y = compact_axis_angle_from_quaternion(concatenate_quaternions(goal_y, q_conj(start_y)))
-
         if smooth_scaling:
+            goal_y_minus_start_y = compact_axis_angle_from_quaternion(
+                concatenate_quaternions(goal_y, q_conj(start_y)))
             smoothing[:] = beta_y * z * goal_y_minus_start_y
 
         current_ydd[:] = (
             alpha_y * (
-                beta_y * compact_axis_angle_from_quaternion(concatenate_quaternions(goal_y, q_conj(current_y)))
+                beta_y * compact_axis_angle_from_quaternion(
+                    concatenate_quaternions(goal_y, q_conj(current_y)))
                 + execution_time * goal_yd
                 - execution_time * current_yd
                 - smoothing
@@ -527,7 +528,8 @@ cpdef dmp_step_dual_cartesian(
         double goal_t, double start_t, double alpha_y, double beta_y,
         forcing_term, coupling_term=None,
         double int_dt=0.001,
-        double p_gain=0.0, np.ndarray tracking_error=None):
+        double p_gain=0.0, np.ndarray tracking_error=None,
+        bint smooth_scaling=False):
     """Integrate bimanual Cartesian DMP for one step with Euler integration.
 
     Parameters
@@ -589,6 +591,11 @@ cpdef dmp_step_dual_cartesian(
 
     tracking_error : float, optional (default: 0)
         Tracking error from last step.
+
+    smooth_scaling : bool, optional (default: False)
+        Avoids jumps during the beginning of DMP execution when the goal
+        is changed and the trajectory is scaled by interpolating between
+        the old and new scaling of the trajectory.
     """
     if t <= start_t:
         current_y[:] = start_y
@@ -600,11 +607,15 @@ cpdef dmp_step_dual_cartesian(
 
     cdef int n_vel_dims = current_yd.shape[0]
 
-    cdef np.ndarray[double, ndim=1] cd = np.empty(n_vel_dims, dtype=np.float64)
-    cdef np.ndarray[double, ndim=1] cdd = np.empty(n_vel_dims, dtype=np.float64)
+    cdef np.ndarray[double, ndim=1] cd = np.empty(n_vel_dims, dtype=float)
+    cdef np.ndarray[double, ndim=1] cdd = np.empty(n_vel_dims, dtype=float)
 
-    cdef np.ndarray[double, ndim=1] f = np.empty(n_vel_dims, dtype=np.float64)
+    cdef np.ndarray[double, ndim=1] f = np.empty(n_vel_dims, dtype=float)
     cdef np.ndarray[double, ndim=1] goal_y_minus_start_y
+    cdef double smoothing_pos
+    cdef np.ndarray[double, ndim=1] smoothing_orn = np.empty(3, dtype=float)
+    if not smooth_scaling:
+        smoothing_orn[:] = 0.0
 
     cdef double z
 
@@ -637,11 +648,15 @@ cpdef dmp_step_dual_cartesian(
 
         # position components
         for pps, pvs in POS_INDICES:
+            if smooth_scaling:
+                smoothing_pos = beta_y * (goal_y[pps] - start_y[pps]) * z
+            else:
+                smoothing_pos = 0.0
             current_ydd[pvs] = (
                 alpha_y * (
                     beta_y * (goal_y[pps] - current_y[pps])
                     + execution_time * (goal_yd[pvs] - current_yd[pvs])
-                    - beta_y * (goal_y[pps] - start_y[pps]) * z
+                    - smoothing_pos
                 )
                 + f[pvs]
                 + cdd[pvs]
@@ -651,14 +666,16 @@ cpdef dmp_step_dual_cartesian(
 
         # orientation components
         for ops, ovs in ((slice(3, 7), slice(3, 6)), (slice(10, 14), slice(9, 12))):
-            goal_y_minus_start_y = compact_axis_angle_from_quaternion(
-                concatenate_quaternions(goal_y[ops], q_conj(start_y[ops])))
+            if smooth_scaling:
+                goal_y_minus_start_y = compact_axis_angle_from_quaternion(
+                    concatenate_quaternions(goal_y[ops], q_conj(start_y[ops])))
+                smoothing_orn[:] = beta_y * z * goal_y_minus_start_y
             current_ydd[ovs] = (
                 alpha_y * (
                     beta_y * compact_axis_angle_from_quaternion(concatenate_quaternions(goal_y[ops], q_conj(current_y[ops])))
                     + execution_time * goal_yd[ovs]
                     - execution_time * current_yd[ovs]
-                    - beta_y * z * goal_y_minus_start_y
+                    - smoothing_orn
                 )
                 + goal_ydd[ovs] * execution_time ** 2
                 + f[ovs]