From 3ffd0806418ff8c9fd75a81da9db6443a5ad8da0 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 21 Dec 2021 22:05:03 +0100 Subject: [PATCH 01/16] Compare phase implementations --- test/test_canonical_system.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 test/test_canonical_system.py diff --git a/test/test_canonical_system.py b/test/test_canonical_system.py new file mode 100644 index 0000000..5b7df19 --- /dev/null +++ b/test/test_canonical_system.py @@ -0,0 +1,16 @@ +import numpy as np +from movement_primitives.dmp._canonical_system import canonical_system_alpha +from movement_primitives.dmp._canonical_system import phase as phase_python +from nose.tools import assert_almost_equal + + +def test_phase_cython(): + from movement_primitives.dmp_fast import phase as 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) + 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) + assert_almost_equal(z_cython, z_python) From 11fab26d16fc35419a72421e2e22b8bfdb719edc Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 21 Dec 2021 22:10:42 +0100 Subject: [PATCH 02/16] Benchmark phase --- benchmarks/benchmark_dmp_phase.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 benchmarks/benchmark_dmp_phase.py diff --git a/benchmarks/benchmark_dmp_phase.py b/benchmarks/benchmark_dmp_phase.py new file mode 100644 index 0000000..e5d903b --- /dev/null +++ b/benchmarks/benchmark_dmp_phase.py @@ -0,0 +1,16 @@ +from functools import partial +import numpy as np +from movement_primitives.dmp._canonical_system import canonical_system_alpha +from movement_primitives.dmp._canonical_system import phase as phase_python +from movement_primitives.dmp_fast import phase as phase_cython +import timeit + + +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) +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) +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) From a6fb002a7ba56cc9738b6e35609fd3895cc2359b Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 21 Dec 2021 22:15:48 +0100 Subject: [PATCH 03/16] Use Cython implementation of phase if available --- movement_primitives/dmp/_canonical_system.py | 4 ---- movement_primitives/dmp/_forcing_term.py | 5 ++++- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/movement_primitives/dmp/_canonical_system.py b/movement_primitives/dmp/_canonical_system.py index fc76e10..8895f86 100644 --- a/movement_primitives/dmp/_canonical_system.py +++ b/movement_primitives/dmp/_canonical_system.py @@ -69,7 +69,3 @@ def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10): execution_time = goal_t - start_t b = max(1.0 - alpha * int_dt / execution_time, eps) return b ** ((t - start_t) / int_dt) - - -# uncomment to overwrite with Cython implementation -#from ..dmp_fast import phase diff --git a/movement_primitives/dmp/_forcing_term.py b/movement_primitives/dmp/_forcing_term.py index 308dd53..840da65 100644 --- a/movement_primitives/dmp/_forcing_term.py +++ b/movement_primitives/dmp/_forcing_term.py @@ -1,6 +1,9 @@ import math import numpy as np -from ._canonical_system import phase +try: + from movement_primitives.dmp_fast import phase +except ImportError: + from ._canonical_system import phase class ForcingTerm: From b3ac309948cb4c08138b8a832f62902af6aeb671 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 21 Dec 2021 22:37:05 +0100 Subject: [PATCH 04/16] Benchmark of DMP step functions --- benchmarks/benchmark_dmp.py | 9 +++++++++ movement_primitives/dmp/_dmp.py | 30 +++++++++++++++++++----------- 2 files changed, 28 insertions(+), 11 deletions(-) diff --git a/benchmarks/benchmark_dmp.py b/benchmarks/benchmark_dmp.py index 8490ce4..bf1875a 100644 --- a/benchmarks/benchmark_dmp.py +++ b/benchmarks/benchmark_dmp.py @@ -15,4 +15,13 @@ dmp.forcing_term.weights = 1000 * np.random.randn(*dmp.forcing_term.weights.shape) times = timeit.repeat(partial(dmp.open_loop, step_function="rk4"), repeat=10, number=1) +print("RK4") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) + +times = timeit.repeat(partial(dmp.open_loop, step_function="euler"), repeat=10, number=1) +print("Euler (Python)") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) + +times = timeit.repeat(partial(dmp.open_loop, step_function="euler-cython"), repeat=10, number=1) +print("Euler (Cython)") print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) diff --git a/movement_primitives/dmp/_dmp.py b/movement_primitives/dmp/_dmp.py index 5a45efd..4ac3381 100644 --- a/movement_primitives/dmp/_dmp.py +++ b/movement_primitives/dmp/_dmp.py @@ -117,7 +117,8 @@ def open_loop(self, run_t=None, coupling_term=None, step_function="rk4"): Coupling term that will be added to velocity. step_function : str, optional (default: 'rk4') - DMP integration function. Possible options: 'rk4', 'euler'. + DMP integration function. Possible options: 'rk4', 'euler', + 'euler-cython'. Returns ------- @@ -132,12 +133,11 @@ def open_loop(self, run_t=None, coupling_term=None, step_function="rk4"): ValueError If step function is unknown. """ - if step_function == "rk4": - step_function = dmp_step_rk4 - elif step_function == "euler": - step_function = dmp_step_euler - else: - raise ValueError("Step function must be 'rk4' or 'euler'.") + try: + step_function = DMP_STEP_FUNCTIONS[step_function] + except KeyError: + raise ValueError( + f"Step function must be in {DMP_STEP_FUNCTIONS.keys()}.") return dmp_open_loop( self.execution_time, 0.0, self.dt_, @@ -484,10 +484,6 @@ def dmp_step_euler( current_y += dt * current_yd -# uncomment to overwrite with cython implementation: -#from ..dmp_fast import dmp_step as dmp_step_euler - - def determine_forces(T, Y, alpha_y, beta_y, allow_final_velocity): """Determine forces that the forcing term should generate. @@ -761,3 +757,15 @@ def dmp_open_loop( Y.append(np.copy(current_y)) return np.array(T), np.array(Y) + + +DMP_STEP_FUNCTIONS = { + "rk4": dmp_step_rk4, + "euler": dmp_step_euler +} + +try: + from ..dmp_fast import dmp_step as dmp_step_euler_cython + DMP_STEP_FUNCTIONS["euler-cython"] = dmp_step_euler_cython +except ImportError: + pass From 260ae992b095d2539420c52bf0e73ab0d1fab57d Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 21 Dec 2021 23:31:01 +0100 Subject: [PATCH 05/16] Benchmark dual Cartesian DMP --- benchmarks/benchmark_dual_dmp.py | 14 +- .../dmp/_dual_cartesian_dmp.py | 304 ++++++++++-------- 2 files changed, 170 insertions(+), 148 deletions(-) diff --git a/benchmarks/benchmark_dual_dmp.py b/benchmarks/benchmark_dual_dmp.py index 639dad6..2a26a49 100644 --- a/benchmarks/benchmark_dual_dmp.py +++ b/benchmarks/benchmark_dual_dmp.py @@ -1,3 +1,4 @@ +from functools import partial import numpy as np from movement_primitives.dmp import DualCartesianDMP import timeit @@ -5,14 +6,17 @@ start_y = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) goal_y = np.array([1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0]) -dt = 0.001 -int_dt = 0.0001 +dt = 0.01 +int_dt = 0.001 dmp = DualCartesianDMP(execution_time=1.0, dt=dt, n_weights_per_dim=6, int_dt=int_dt) dmp.configure(start_y=start_y, goal_y=goal_y) dmp.forcing_term.weights = 1000 * np.random.randn(*dmp.forcing_term.weights.shape) -times = timeit.repeat(dmp.open_loop, repeat=10, number=1) +times = timeit.repeat(partial(dmp.open_loop, step_function="cython"), repeat=10, number=1) +print("Cython") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) + +times = timeit.repeat(partial(dmp.open_loop, step_function="python"), repeat=10, number=1) +print("Python") print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) -# Pure python -# Mean: 0.58188; Std. dev.: 0.00225 \ No newline at end of file diff --git a/movement_primitives/dmp/_dual_cartesian_dmp.py b/movement_primitives/dmp/_dual_cartesian_dmp.py index 3d4e26d..a8b4acb 100644 --- a/movement_primitives/dmp/_dual_cartesian_dmp.py +++ b/movement_primitives/dmp/_dual_cartesian_dmp.py @@ -8,6 +8,142 @@ from ._cartesian_dmp import dmp_quaternion_imitation +pps = [0, 1, 2, 7, 8, 9] +pvs = [0, 1, 2, 6, 7, 8] + + +def dmp_step_dual_cartesian_python( + 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, int_dt=0.001, + p_gain=0.0, tracking_error=None): + """Integrate bimanual Cartesian DMP for one step with Euler integration. + + Parameters + ---------- + last_t : float + Time at last step. + + t : float + Time at current step. + + current_y : array, shape (14,) + Current position. Will be modified. + + current_yd : array, shape (12,) + Current velocity. Will be modified. + + goal_y : array, shape (14,) + Goal position. + + goal_yd : array, shape (12,) + Goal velocity. + + goal_ydd : array, shape (12,) + Goal acceleration. + + start_y : array, shape (14,) + Start position. + + start_yd : array, shape (12,) + Start velocity. + + start_ydd : array, shape (12,) + Start acceleration. + + goal_t : float + Time at the end. + + start_t : float + Time at the start. + + alpha_y : float + Constant in transformation system. + + beta_y : float + Constant in transformation system. + + forcing_term : ForcingTerm + Forcing term. + + coupling_term : CouplingTerm, optional (default: None) + Coupling term. Must have a function coupling(y, yd) that returns + additional velocity and acceleration. + + int_dt : float, optional (default: 0.001) + Time delta used internally for integration. + + p_gain : float, optional (default: 0) + Proportional gain for tracking error. + + tracking_error : float, optional (default: 0) + Tracking error from last step. + """ + if t <= start_t: + current_y[:] = start_y + current_yd[:] = start_yd + + execution_time = goal_t - start_t + + current_ydd = np.empty_like(current_yd) + + cd, cdd = np.zeros_like(current_yd), np.zeros_like(current_ydd) + + current_t = last_t + while current_t < t: + dt = int_dt + if t - current_t < int_dt: + dt = t - current_t + current_t += dt + + if coupling_term is not None: + cd[:], cdd[:] = coupling_term.coupling(current_y, current_yd) + + f = forcing_term(current_t).squeeze() + if tracking_error is not None: + cdd[pvs] += p_gain * tracking_error[pps] / dt + for ops, ovs in ((slice(3, 7), slice(3, 6)), + (slice(10, 14), slice(9, 12))): + cdd[ovs] += p_gain * pr.compact_axis_angle_from_quaternion( + tracking_error[ops]) / dt + + # 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]) + + goal_ydd[pvs] * execution_time ** 2 + f[pvs] + cdd[pvs]) / execution_time ** 2 + current_yd[pvs] += dt * current_ydd[pvs] + cd[pvs] / execution_time + current_y[pps] += dt * current_yd[pvs] + + # orientation components + for ops, ovs in ((slice(3, 7), slice(3, 6)), + (slice(10, 14), slice(9, 12))): + 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 * current_yd[ovs]) + f[ovs] + cdd[ovs]) / execution_time ** 2 + current_yd[ovs] += dt * current_ydd[ovs] + cd[ovs] / execution_time + current_y[ops] = pr.concatenate_quaternions( + pr.quaternion_from_compact_axis_angle(dt * current_yd[ovs]), current_y[ops]) + + +DUAL_CARTESIAN_DMP_STEP_FUNCTIONS = { + "python": dmp_step_dual_cartesian_python +} + + +try: + from ..dmp_fast import dmp_step_dual_cartesian + DUAL_CARTESIAN_DMP_STEP_FUNCTIONS["cython"] = dmp_step_dual_cartesian + DEFAULT_DUAL_CARTESIAN_DMP_STEP_FUNCTION = "cython" +except ImportError: + DEFAULT_DUAL_CARTESIAN_DMP_STEP_FUNCTION = "python" + + class DualCartesianDMP(DMPBase): """Dual cartesian dynamical movement primitive. @@ -48,8 +184,8 @@ class DualCartesianDMP(DMPBase): Time difference between DMP steps. This value can be changed to adapt the frequency. """ - def __init__(self, execution_time, dt=0.01, - n_weights_per_dim=10, int_dt=0.001, p_gain=0.0): + def __init__(self, execution_time, dt=0.01, n_weights_per_dim=10, + int_dt=0.001, p_gain=0.0): super(DualCartesianDMP, self).__init__(14, 12) self.execution_time = execution_time self.dt_ = dt @@ -65,7 +201,9 @@ def __init__(self, execution_time, dt=0.01, self.alpha_y = 25.0 self.beta_y = self.alpha_y / 4.0 - def step(self, last_y, last_yd, coupling_term=None): + def step(self, last_y, last_yd, coupling_term=None, + step_function=DUAL_CARTESIAN_DMP_STEP_FUNCTIONS[ + DEFAULT_DUAL_CARTESIAN_DMP_STEP_FUNCTION]): """DMP step. Parameters @@ -79,6 +217,9 @@ def step(self, last_y, last_yd, coupling_term=None): coupling_term : object, optional (default: None) Coupling term that will be added to velocity. + step_function : str, optional (default: 'cython' if available) + DMP integration function. Possible options: 'python', 'cython'. + Returns ------- y : array, shape (14,) @@ -103,7 +244,7 @@ def step(self, last_y, last_yd, coupling_term=None): tracking_error[ops] = pr.concatenate_quaternions( self.current_y[ops], pr.q_conj(last_y[ops])) self.current_y[:], self.current_yd[:] = last_y, last_yd - dmp_step_dual_cartesian( + step_function( self.last_t, self.t, self.current_y, self.current_yd, self.goal_y, self.goal_yd, self.goal_ydd, self.start_y, self.start_yd, self.start_ydd, @@ -115,7 +256,8 @@ def step(self, last_y, last_yd, coupling_term=None): return np.copy(self.current_y), np.copy(self.current_yd) - def open_loop(self, run_t=None, coupling_term=None): + def open_loop(self, run_t=None, coupling_term=None, + step_function=DEFAULT_DUAL_CARTESIAN_DMP_STEP_FUNCTION): """Run DMP open loop. Parameters @@ -126,6 +268,9 @@ def open_loop(self, run_t=None, coupling_term=None): coupling_term : object, optional (default: None) Coupling term that will be added to velocity. + step_function : str, optional (default: 'cython' if available) + DMP integration function. Possible options: 'python', 'cython'. + Returns ------- T : array, shape (n_steps,) @@ -134,16 +279,25 @@ def open_loop(self, run_t=None, coupling_term=None): Y : array, shape (n_steps, 14) State at each step. """ + try: + step_function = DUAL_CARTESIAN_DMP_STEP_FUNCTIONS[step_function] + except KeyError: + raise ValueError( + f"Step function must be in " + f"{DUAL_CARTESIAN_DMP_STEP_FUNCTIONS.keys()}.") + if run_t is None: run_t = self.execution_time - T = [0.0] + self.t = 0.0 + T = [self.t] Y = [np.copy(self.start_y)] y = np.copy(self.start_y) yd = np.copy(self.start_yd) while self.t < run_t: - y, yd = self.step(y, yd, coupling_term) + y, yd = self.step(y, yd, coupling_term, step_function) T.append(self.t) Y.append(np.copy(self.current_y)) + self.t = 0.0 return np.array(T), np.vstack(Y) def imitate(self, T, Y, regularization_coefficient=0.0, @@ -219,139 +373,3 @@ def set_weights(self, weights): """ self.forcing_term.weights[:, :] = weights.reshape( -1, self.n_weights_per_dim) - - -pps = [0, 1, 2, 7, 8, 9] -pvs = [0, 1, 2, 6, 7, 8] - - -def dmp_step_dual_cartesian_python( - 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, int_dt=0.001, - p_gain=0.0, tracking_error=None): - """Integrate bimanual Cartesian DMP for one step with Euler integration. - - Parameters - ---------- - last_t : float - Time at last step. - - t : float - Time at current step. - - current_y : array, shape (14,) - Current position. Will be modified. - - current_yd : array, shape (12,) - Current velocity. Will be modified. - - goal_y : array, shape (14,) - Goal position. - - goal_yd : array, shape (12,) - Goal velocity. - - goal_ydd : array, shape (12,) - Goal acceleration. - - start_y : array, shape (14,) - Start position. - - start_yd : array, shape (12,) - Start velocity. - - start_ydd : array, shape (12,) - Start acceleration. - - goal_t : float - Time at the end. - - start_t : float - Time at the start. - - alpha_y : float - Constant in transformation system. - - beta_y : float - Constant in transformation system. - - forcing_term : ForcingTerm - Forcing term. - - coupling_term : CouplingTerm, optional (default: None) - Coupling term. Must have a function coupling(y, yd) that returns - additional velocity and acceleration. - - int_dt : float, optional (default: 0.001) - Time delta used internally for integration. - - p_gain : float, optional (default: 0) - Proportional gain for tracking error. - - tracking_error : float, optional (default: 0) - Tracking error from last step. - """ - if t <= start_t: - current_y[:] = start_y - current_yd[:] = start_yd - - execution_time = goal_t - start_t - - current_ydd = np.empty_like(current_yd) - - cd, cdd = np.zeros_like(current_yd), np.zeros_like(current_ydd) - - current_t = last_t - while current_t < t: - dt = int_dt - if t - current_t < int_dt: - dt = t - current_t - current_t += dt - - if coupling_term is not None: - cd[:], cdd[:] = coupling_term.coupling(current_y, current_yd) - - f = forcing_term(current_t).squeeze() - if tracking_error is not None: - cdd[pvs] += p_gain * tracking_error[pps] / dt - for ops, ovs in ((slice(3, 7), slice(3, 6)), - (slice(10, 14), slice(9, 12))): - cdd[ovs] += p_gain * pr.compact_axis_angle_from_quaternion( - tracking_error[ops]) / dt - - # 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]) - + goal_ydd[pvs] * execution_time ** 2 + f[pvs] + cdd[pvs]) / execution_time ** 2 - current_yd[pvs] += dt * current_ydd[pvs] + cd[pvs] / execution_time - current_y[pps] += dt * current_yd[pvs] - - # orientation components - for ops, ovs in ((slice(3, 7), slice(3, 6)), - (slice(10, 14), slice(9, 12))): - 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 * current_yd[ovs]) + f[ovs] + cdd[ovs]) / execution_time ** 2 - current_yd[ovs] += dt * current_ydd[ovs] + cd[ovs] / execution_time - current_y[ops] = pr.concatenate_quaternions( - pr.quaternion_from_compact_axis_angle(dt * current_yd[ovs]), current_y[ops]) - - -try: - from ..dmp_fast import dmp_step_dual_cartesian -except ImportError: - warnings.warn( - "Could not import fast dual cartesian DMP. " - "Build Cython extension if you want it.", - UserWarning) - dmp_step_dual_cartesian = dmp_step_dual_cartesian_python - -# uncomment to overwrite cython version with python version: -#dmp_step_dual_cartesian = dmp_step_dual_cartesian_python From 82bb2e9963e526971371b1354e2e28ccc356af9c Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 21 Dec 2021 23:59:58 +0100 Subject: [PATCH 06/16] Benchmark Cartesian DMP --- benchmarks/benchmark_cartesian_dmp.py | 26 ++ movement_primitives/dmp/_cartesian_dmp.py | 327 +++++++------ movement_primitives/dmp/_dmp.py | 428 +++++++++--------- .../dmp/_dual_cartesian_dmp.py | 5 +- 4 files changed, 425 insertions(+), 361 deletions(-) create mode 100644 benchmarks/benchmark_cartesian_dmp.py diff --git a/benchmarks/benchmark_cartesian_dmp.py b/benchmarks/benchmark_cartesian_dmp.py new file mode 100644 index 0000000..25279a9 --- /dev/null +++ b/benchmarks/benchmark_cartesian_dmp.py @@ -0,0 +1,26 @@ +from functools import partial +import numpy as np +from movement_primitives.dmp import CartesianDMP +import timeit + + +start_y = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) +goal_y = np.array([1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0]) +dt = 0.01 +int_dt = 0.001 + +dmp = CartesianDMP(execution_time=1.0, dt=dt, n_weights_per_dim=6, int_dt=int_dt) +dmp.configure(start_y=start_y, goal_y=goal_y) +dmp.set_weights(1000 * np.random.randn(*dmp.get_weights().shape)) + +times = timeit.repeat(partial(dmp.open_loop, quaternion_step_function="cython"), repeat=10, number=1) +print("RK4 + Cython") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) + +times = timeit.repeat(partial(dmp.open_loop, quaternion_step_function="python"), repeat=10, number=1) +print("RK4 + Python") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) + +times = timeit.repeat(partial(dmp.open_loop, step_function="euler", quaternion_step_function="python"), repeat=10, number=1) +print("Euler + Python") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) diff --git a/movement_primitives/dmp/_cartesian_dmp.py b/movement_primitives/dmp/_cartesian_dmp.py index 7d17b15..34b167e 100644 --- a/movement_primitives/dmp/_cartesian_dmp.py +++ b/movement_primitives/dmp/_cartesian_dmp.py @@ -1,10 +1,136 @@ -import warnings import numpy as np import pytransform3d.rotations as pr from ._base import DMPBase from ._forcing_term import ForcingTerm from ._canonical_system import canonical_system_alpha -from ._dmp import dmp_step_rk4, dmp_open_loop, dmp_imitate, ridge_regression +from ._dmp import (dmp_step_rk4, dmp_open_loop, dmp_imitate, ridge_regression, + DMP_STEP_FUNCTIONS, DEFAULT_DMP_STEP_FUNCTION) + + +def dmp_step_quaternion_python( + 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): + """Integrate quaternion DMP for one step with Euler integration. + + Parameters + ---------- + last_t : float + Time at last step. + + t : float + Time at current step. + + current_y : array, shape (7,) + Current position. Will be modified. + + current_yd : array, shape (6,) + Current velocity. Will be modified. + + goal_y : array, shape (7,) + Goal position. + + goal_yd : array, shape (6,) + Goal velocity. + + goal_ydd : array, shape (6,) + Goal acceleration. + + start_y : array, shape (7,) + Start position. + + start_yd : array, shape (6,) + Start velocity. + + start_ydd : array, shape (6,) + Start acceleration. + + goal_t : float + Time at the end. + + start_t : float + Time at the start. + + alpha_y : float + Constant in transformation system. + + beta_y : float + Constant in transformation system. + + forcing_term : ForcingTerm + Forcing term. + + coupling_term : CouplingTerm, optional (default: None) + Coupling term. Must have a function coupling(y, yd) that returns + additional velocity and acceleration. + + coupling_term_precomputed : tuple + A precomputed coupling term, i.e., additional velocity and + acceleration. + + int_dt : float, optional (default: 0.001) + Time delta used internally for integration. + + Raises + ------ + ValueError + If goal time is before start time. + """ + if start_t >= goal_t: + raise ValueError("Goal must be chronologically after start!") + + if t <= start_t: + return np.copy(start_y), np.copy(start_yd), np.copy(start_ydd) + + execution_time = goal_t - start_t + + current_ydd = np.empty_like(current_yd) + + current_t = last_t + while current_t < t: + dt = int_dt + if t - current_t < int_dt: + dt = t - current_t + current_t += dt + + if coupling_term is not None: + cd, cdd = coupling_term.coupling(current_y, current_yd) + else: + cd, cdd = np.zeros(3), np.zeros(3) + if coupling_term_precomputed is not None: + cd += coupling_term_precomputed[0] + cdd += coupling_term_precomputed[1] + + f = forcing_term(current_t).squeeze() + + current_ydd[:] = ( + alpha_y * (beta_y * pr.compact_axis_angle_from_quaternion( + pr.concatenate_quaternions( + goal_y, pr.q_conj(current_y))) + - execution_time * current_yd) + + f + cdd) / execution_time ** 2 + current_yd += dt * current_ydd + cd / execution_time + current_y[:] = pr.concatenate_quaternions( + pr.quaternion_from_compact_axis_angle(dt * current_yd), current_y) + + +CARTESIAN_DMP_STEP_FUNCTIONS = { + "python": dmp_step_quaternion_python +} + + +try: + from ..dmp_fast import dmp_step_quaternion + CARTESIAN_DMP_STEP_FUNCTIONS["cython"] = dmp_step_quaternion + DEFAULT_CARTESIAN_DMP_STEP_FUNCTION = "cython" +except ImportError: + DEFAULT_CARTESIAN_DMP_STEP_FUNCTION = "python" class CartesianDMP(DMPBase): @@ -62,7 +188,10 @@ def __init__( self.alpha_y = 25.0 self.beta_y = self.alpha_y / 4.0 - def step(self, last_y, last_yd, coupling_term=None): + def step(self, last_y, last_yd, coupling_term=None, + step_function=DMP_STEP_FUNCTIONS[DEFAULT_DMP_STEP_FUNCTION], + quaternion_step_function=CARTESIAN_DMP_STEP_FUNCTIONS[ + DEFAULT_CARTESIAN_DMP_STEP_FUNCTION]): """DMP step. Parameters @@ -76,6 +205,12 @@ def step(self, last_y, last_yd, coupling_term=None): coupling_term : object, optional (default: None) Coupling term that will be added to velocity. + step_function : callable, optional (default: RK4) + DMP integration function. + + quaternion_step_function : callable, optional (default: cython code if available) + DMP integration function. + Returns ------- y : array, shape (14,) @@ -93,7 +228,7 @@ def step(self, last_y, last_yd, coupling_term=None): # TODO tracking error self.current_y[:], self.current_yd[:] = last_y, last_yd - dmp_step_rk4( + step_function( self.last_t, self.t, self.current_y[:3], self.current_yd[:3], self.goal_y[:3], self.goal_yd[:3], self.goal_ydd[:3], @@ -103,7 +238,7 @@ def step(self, last_y, last_yd, coupling_term=None): self.forcing_term_pos, coupling_term=coupling_term, int_dt=self.int_dt) - dmp_step_quaternion( + quaternion_step_function( self.last_t, self.t, self.current_y[3:], self.current_yd[3:], self.goal_y[3:], self.goal_yd[3:], self.goal_ydd[3:], @@ -115,7 +250,9 @@ def step(self, last_y, last_yd, coupling_term=None): int_dt=self.int_dt) return np.copy(self.current_y), np.copy(self.current_yd) - def open_loop(self, run_t=None, coupling_term=None): + def open_loop(self, run_t=None, coupling_term=None, + step_function=DEFAULT_DMP_STEP_FUNCTION, + quaternion_step_function=DEFAULT_CARTESIAN_DMP_STEP_FUNCTION): """Run DMP open loop. Parameters @@ -126,6 +263,13 @@ def open_loop(self, run_t=None, coupling_term=None): coupling_term : object, optional (default: None) Coupling term that will be added to velocity. + step_function : str, optional (default: 'rk4') + DMP integration function. Possible options: 'rk4', 'euler', + 'euler-cython'. + + quaternion_step_function : str, optional (default: 'cython' if available) + DMP integration function. Possible options: 'python', 'cython'. + Returns ------- T : array, shape (n_steps,) @@ -134,20 +278,35 @@ def open_loop(self, run_t=None, coupling_term=None): Y : array, shape (n_steps, 7) State at each step. """ + try: + step_function = DMP_STEP_FUNCTIONS[step_function] + except KeyError: + raise ValueError( + f"Step function must be in " + f"{DMP_STEP_FUNCTIONS.keys()}.") T, Yp = dmp_open_loop( - self.execution_time, 0.0, self.dt_, - self.start_y[:3], self.goal_y[:3], - self.alpha_y, self.beta_y, - self.forcing_term_pos, - coupling_term, - run_t, self.int_dt) + self.execution_time, 0.0, self.dt_, + self.start_y[:3], self.goal_y[:3], + self.alpha_y, self.beta_y, + self.forcing_term_pos, + coupling_term, + run_t, self.int_dt, + step_function=step_function) + try: + quaternion_step_function = CARTESIAN_DMP_STEP_FUNCTIONS[ + quaternion_step_function] + except KeyError: + raise ValueError( + f"Step function must be in " + f"{CARTESIAN_DMP_STEP_FUNCTIONS.keys()}.") _, Yr = dmp_open_loop_quaternion( - self.execution_time, 0.0, self.dt_, - self.start_y[3:], self.goal_y[3:], - self.alpha_y, self.beta_y, - self.forcing_term_rot, - coupling_term, - run_t, self.int_dt) + self.execution_time, 0.0, self.dt_, + self.start_y[3:], self.goal_y[3:], + self.alpha_y, self.beta_y, + self.forcing_term_rot, + coupling_term, + run_t, self.int_dt, + quaternion_step_function) return T, np.hstack((Yp, Yr)) def imitate(self, T, Y, regularization_coefficient=0.0, @@ -213,129 +372,6 @@ def set_weights(self, weights): -1, self.n_weights_per_dim) -def dmp_step_quaternion_python( - 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): - """Integrate quaternion DMP for one step with Euler integration. - - Parameters - ---------- - last_t : float - Time at last step. - - t : float - Time at current step. - - current_y : array, shape (7,) - Current position. Will be modified. - - current_yd : array, shape (6,) - Current velocity. Will be modified. - - goal_y : array, shape (7,) - Goal position. - - goal_yd : array, shape (6,) - Goal velocity. - - goal_ydd : array, shape (6,) - Goal acceleration. - - start_y : array, shape (7,) - Start position. - - start_yd : array, shape (6,) - Start velocity. - - start_ydd : array, shape (6,) - Start acceleration. - - goal_t : float - Time at the end. - - start_t : float - Time at the start. - - alpha_y : float - Constant in transformation system. - - beta_y : float - Constant in transformation system. - - forcing_term : ForcingTerm - Forcing term. - - coupling_term : CouplingTerm, optional (default: None) - Coupling term. Must have a function coupling(y, yd) that returns - additional velocity and acceleration. - - coupling_term_precomputed : tuple - A precomputed coupling term, i.e., additional velocity and - acceleration. - - int_dt : float, optional (default: 0.001) - Time delta used internally for integration. - - Raises - ------ - ValueError - If goal time is before start time. - """ - if start_t >= goal_t: - raise ValueError("Goal must be chronologically after start!") - - if t <= start_t: - return np.copy(start_y), np.copy(start_yd), np.copy(start_ydd) - - execution_time = goal_t - start_t - - current_ydd = np.empty_like(current_yd) - - current_t = last_t - while current_t < t: - dt = int_dt - if t - current_t < int_dt: - dt = t - current_t - current_t += dt - - if coupling_term is not None: - cd, cdd = coupling_term.coupling(current_y, current_yd) - else: - cd, cdd = np.zeros(3), np.zeros(3) - if coupling_term_precomputed is not None: - cd += coupling_term_precomputed[0] - cdd += coupling_term_precomputed[1] - - f = forcing_term(current_t).squeeze() - - current_ydd[:] = ( - alpha_y * (beta_y * pr.compact_axis_angle_from_quaternion( - pr.concatenate_quaternions( - goal_y, pr.q_conj(current_y))) - - execution_time * current_yd) - + f + cdd) / execution_time ** 2 - current_yd += dt * current_ydd + cd / execution_time - current_y[:] = pr.concatenate_quaternions( - pr.quaternion_from_compact_axis_angle(dt * current_yd), current_y) - - -try: - from ..dmp_fast import dmp_step_quaternion -except ImportError: - warnings.warn( - "Could not import fast quaternion DMP. " - "Build Cython extension if you want it.", - UserWarning) - dmp_step_quaternion = dmp_step_quaternion_python - - def dmp_quaternion_imitation( T, Y, n_weights_per_dim, regularization_coefficient, alpha_y, beta_y, overlap, alpha_z, allow_final_velocity): @@ -480,7 +516,9 @@ def determine_forces_quaternion(T, Y, alpha_y, beta_y, allow_final_velocity): def dmp_open_loop_quaternion( goal_t, start_t, dt, start_y, goal_y, alpha_y, beta_y, forcing_term, - coupling_term=None, run_t=None, int_dt=0.001): + coupling_term=None, run_t=None, int_dt=0.001, + quaternion_step_function=CARTESIAN_DMP_STEP_FUNCTIONS[ + DEFAULT_CARTESIAN_DMP_STEP_FUNCTION]): """Run Cartesian DMP without external feedback. Parameters @@ -518,6 +556,9 @@ def dmp_open_loop_quaternion( int_dt : float, optional (default: 0.001) Time delta used internally for integration. + + quaternion_step_function : callable, optional (default: cython code if available) + DMP integration function. """ t = start_t y = np.copy(start_y) @@ -529,7 +570,7 @@ def dmp_open_loop_quaternion( while t < run_t: last_t = t t += dt - dmp_step_quaternion( + quaternion_step_function( last_t, t, y, yd, goal_y=goal_y, goal_yd=np.zeros_like(yd), goal_ydd=np.zeros_like(yd), diff --git a/movement_primitives/dmp/_dmp.py b/movement_primitives/dmp/_dmp.py index 4ac3381..d036d26 100644 --- a/movement_primitives/dmp/_dmp.py +++ b/movement_primitives/dmp/_dmp.py @@ -4,198 +4,6 @@ from ._canonical_system import canonical_system_alpha -class DMP(DMPBase): - """Dynamical movement primitive (DMP). - - Implementation according to - - A.J. Ijspeert, J. Nakanishi, H. Hoffmann, P. Pastor, S. Schaal: - Dynamical Movement Primitives: Learning Attractor Models for Motor - Behaviors (2013), Neural Computation 25(2), pp. 328-373, doi: - 10.1162/NECO_a_00393, https://ieeexplore.ieee.org/document/6797340 - - Parameters - ---------- - n_dims : int - State space dimensions. - - execution_time : float - Execution time of the DMP. - - dt : float, optional (default: 0.01) - Time difference between DMP steps. - - n_weights_per_dim : int, optional (default: 10) - Number of weights of the function approximator per dimension. - - int_dt : float, optional (default: 0.001) - Time difference for Euler integration. - - p_gain : float, optional (default: 0) - Gain for proportional controller of DMP tracking error. - The domain is [0, execution_time**2/dt]. - - Attributes - ---------- - dt_ : float - Time difference between DMP steps. This value can be changed to adapt - the frequency. - """ - def __init__(self, n_dims, execution_time, dt=0.01, n_weights_per_dim=10, - int_dt=0.001, p_gain=0.0): - super(DMP, 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 - - alpha_z = canonical_system_alpha( - 0.01, self.execution_time, 0.0, self.int_dt) - self.forcing_term = ForcingTerm(self.n_dims, self.n_weights_per_dim, - self.execution_time, 0.0, 0.8, alpha_z) - - self.alpha_y = 25.0 - self.beta_y = self.alpha_y / 4.0 - - def step(self, last_y, last_yd, coupling_term=None): - """DMP step. - - Parameters - ---------- - last_y : array, shape (n_dims,) - Last state. - - last_yd : array, shape (n_dims,) - Last time derivative of state (e.g., velocity). - - coupling_term : object, optional (default: None) - Coupling term that will be added to velocity. - - Returns - ------- - y : array, shape (n_dims,) - Next state. - - yd : array, shape (n_dims,) - Next time derivative of state (e.g., velocity). - """ - self.last_t = self.t - self.t += self.dt_ - - if not self.initialized: - self.current_y = np.copy(self.start_y) - self.current_yd = np.copy(self.start_yd) - self.initialized = True - - # https://github.com/studywolf/pydmps/blob/master/pydmps/cs.py - tracking_error = self.current_y - last_y - - dmp_step_rk4( - self.last_t, self.t, - self.current_y, self.current_yd, - self.goal_y, self.goal_yd, self.goal_ydd, - self.start_y, self.start_yd, self.start_ydd, - self.execution_time, 0.0, - self.alpha_y, self.beta_y, - self.forcing_term, - coupling_term=coupling_term, - int_dt=self.int_dt, - p_gain=self.p_gain, - tracking_error=tracking_error) - return np.copy(self.current_y), np.copy(self.current_yd) - - def open_loop(self, run_t=None, coupling_term=None, step_function="rk4"): - """Run DMP open loop. - - Parameters - ---------- - run_t : float, optional (default: execution_time) - Run time of DMP. Can be shorter or longer than execution_time. - - coupling_term : object, optional (default: None) - Coupling term that will be added to velocity. - - step_function : str, optional (default: 'rk4') - DMP integration function. Possible options: 'rk4', 'euler', - 'euler-cython'. - - Returns - ------- - T : array, shape (n_steps,) - Time for each step. - - Y : array, shape (n_steps, n_dims) - State at each step. - - Raises - ------ - ValueError - If step function is unknown. - """ - try: - step_function = DMP_STEP_FUNCTIONS[step_function] - except KeyError: - raise ValueError( - f"Step function must be in {DMP_STEP_FUNCTIONS.keys()}.") - - return dmp_open_loop( - self.execution_time, 0.0, self.dt_, - self.start_y, self.goal_y, - self.alpha_y, self.beta_y, - self.forcing_term, - coupling_term, - run_t, self.int_dt, - step_function) - - def imitate(self, T, Y, regularization_coefficient=0.0, - allow_final_velocity=False): - """Imitate demonstration. - - Parameters - ---------- - T : array, shape (n_steps,) - Time for each step. - - Y : array, shape (n_steps, n_dims) - State at each step. - - regularization_coefficient : float, optional (default: 0) - Regularization coefficient for regression. - - allow_final_velocity : bool, optional (default: False) - Allow a final velocity. - """ - self.forcing_term.weights[:, :], start_y, _, _, goal_y, _, _ = dmp_imitate( - T, Y, - n_weights_per_dim=self.n_weights_per_dim, - regularization_coefficient=regularization_coefficient, - alpha_y=self.alpha_y, beta_y=self.beta_y, overlap=self.forcing_term.overlap, - alpha_z=self.forcing_term.alpha_z, allow_final_velocity=allow_final_velocity) - self.configure(start_y=start_y, goal_y=goal_y) - - def get_weights(self): - """Get weight vector of DMP. - - Returns - ------- - weights : array, shape (n_dims * n_weights_per_dim,) - Current weights of the DMP. - """ - return self.forcing_term.weights.ravel() - - def set_weights(self, weights): - """Set weight vector of DMP. - - Parameters - ---------- - weights : array, shape (n_dims * n_weights_per_dim,) - New weights of the DMP. - """ - self.forcing_term.weights[:, :] = weights.reshape( - -1, self.n_weights_per_dim) - - def dmp_step_rk4( 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, @@ -311,10 +119,6 @@ def dmp_step_rk4( current_yd += cd / execution_time -# uncomment to overwrite with cython implementation: -#from ..dmp_fast import dmp_step_rk4 - - def _dmp_acc(Y, V, cdd, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, f, coupling_term, tdd): """DMP acceleration. @@ -371,13 +175,6 @@ def _dmp_acc(Y, V, cdd, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, + goal_ydd) -def dmp_transformation_system(Y, V, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, - execution_time): - """Compute acceleration generated by transformation system of DMP.""" - return (alpha_y * (beta_y * (goal_y - Y) + execution_time * (goal_yd - V)) - ) / execution_time ** 2 + goal_ydd - - def dmp_step_euler( 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, @@ -484,6 +281,219 @@ def dmp_step_euler( current_y += dt * current_yd +DMP_STEP_FUNCTIONS = { + "rk4": dmp_step_rk4, + "euler": dmp_step_euler +} +DEFAULT_DMP_STEP_FUNCTION = "rk4" + +try: + from ..dmp_fast import dmp_step as dmp_step_euler_cython + DMP_STEP_FUNCTIONS["euler-cython"] = dmp_step_euler_cython +except ImportError: + pass + + +class DMP(DMPBase): + """Dynamical movement primitive (DMP). + + Implementation according to + + A.J. Ijspeert, J. Nakanishi, H. Hoffmann, P. Pastor, S. Schaal: + Dynamical Movement Primitives: Learning Attractor Models for Motor + Behaviors (2013), Neural Computation 25(2), pp. 328-373, doi: + 10.1162/NECO_a_00393, https://ieeexplore.ieee.org/document/6797340 + + Parameters + ---------- + n_dims : int + State space dimensions. + + execution_time : float + Execution time of the DMP. + + dt : float, optional (default: 0.01) + Time difference between DMP steps. + + n_weights_per_dim : int, optional (default: 10) + Number of weights of the function approximator per dimension. + + int_dt : float, optional (default: 0.001) + Time difference for Euler integration. + + p_gain : float, optional (default: 0) + Gain for proportional controller of DMP tracking error. + The domain is [0, execution_time**2/dt]. + + Attributes + ---------- + dt_ : float + Time difference between DMP steps. This value can be changed to adapt + the frequency. + """ + def __init__(self, n_dims, execution_time, dt=0.01, n_weights_per_dim=10, + int_dt=0.001, p_gain=0.0): + super(DMP, 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 + + alpha_z = canonical_system_alpha( + 0.01, self.execution_time, 0.0, self.int_dt) + self.forcing_term = ForcingTerm(self.n_dims, self.n_weights_per_dim, + self.execution_time, 0.0, 0.8, alpha_z) + + self.alpha_y = 25.0 + self.beta_y = self.alpha_y / 4.0 + + def step(self, last_y, last_yd, coupling_term=None): + """DMP step. + + Parameters + ---------- + last_y : array, shape (n_dims,) + Last state. + + last_yd : array, shape (n_dims,) + Last time derivative of state (e.g., velocity). + + coupling_term : object, optional (default: None) + Coupling term that will be added to velocity. + + Returns + ------- + y : array, shape (n_dims,) + Next state. + + yd : array, shape (n_dims,) + Next time derivative of state (e.g., velocity). + """ + self.last_t = self.t + self.t += self.dt_ + + if not self.initialized: + self.current_y = np.copy(self.start_y) + self.current_yd = np.copy(self.start_yd) + self.initialized = True + + # https://github.com/studywolf/pydmps/blob/master/pydmps/cs.py + tracking_error = self.current_y - last_y + + dmp_step_rk4( + self.last_t, self.t, + self.current_y, self.current_yd, + self.goal_y, self.goal_yd, self.goal_ydd, + self.start_y, self.start_yd, self.start_ydd, + self.execution_time, 0.0, + self.alpha_y, self.beta_y, + self.forcing_term, + coupling_term=coupling_term, + int_dt=self.int_dt, + p_gain=self.p_gain, + tracking_error=tracking_error) + return np.copy(self.current_y), np.copy(self.current_yd) + + def open_loop(self, run_t=None, coupling_term=None, + step_function=DEFAULT_DMP_STEP_FUNCTION): + """Run DMP open loop. + + Parameters + ---------- + run_t : float, optional (default: execution_time) + Run time of DMP. Can be shorter or longer than execution_time. + + coupling_term : object, optional (default: None) + Coupling term that will be added to velocity. + + step_function : str, optional (default: 'rk4') + DMP integration function. Possible options: 'rk4', 'euler', + 'euler-cython'. + + Returns + ------- + T : array, shape (n_steps,) + Time for each step. + + Y : array, shape (n_steps, n_dims) + State at each step. + + Raises + ------ + ValueError + If step function is unknown. + """ + try: + step_function = DMP_STEP_FUNCTIONS[step_function] + except KeyError: + raise ValueError( + f"Step function must be in {DMP_STEP_FUNCTIONS.keys()}.") + + return dmp_open_loop( + self.execution_time, 0.0, self.dt_, + self.start_y, self.goal_y, + self.alpha_y, self.beta_y, + self.forcing_term, + coupling_term, + run_t, self.int_dt, + step_function) + + def imitate(self, T, Y, regularization_coefficient=0.0, + allow_final_velocity=False): + """Imitate demonstration. + + Parameters + ---------- + T : array, shape (n_steps,) + Time for each step. + + Y : array, shape (n_steps, n_dims) + State at each step. + + regularization_coefficient : float, optional (default: 0) + Regularization coefficient for regression. + + allow_final_velocity : bool, optional (default: False) + Allow a final velocity. + """ + self.forcing_term.weights[:, :], start_y, _, _, goal_y, _, _ = dmp_imitate( + T, Y, + n_weights_per_dim=self.n_weights_per_dim, + regularization_coefficient=regularization_coefficient, + alpha_y=self.alpha_y, beta_y=self.beta_y, overlap=self.forcing_term.overlap, + alpha_z=self.forcing_term.alpha_z, allow_final_velocity=allow_final_velocity) + self.configure(start_y=start_y, goal_y=goal_y) + + def get_weights(self): + """Get weight vector of DMP. + + Returns + ------- + weights : array, shape (n_dims * n_weights_per_dim,) + Current weights of the DMP. + """ + return self.forcing_term.weights.ravel() + + def set_weights(self, weights): + """Set weight vector of DMP. + + Parameters + ---------- + weights : array, shape (n_dims * n_weights_per_dim,) + New weights of the DMP. + """ + self.forcing_term.weights[:, :] = weights.reshape( + -1, self.n_weights_per_dim) + + +def dmp_transformation_system(Y, V, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, + execution_time): + """Compute acceleration generated by transformation system of DMP.""" + return (alpha_y * (beta_y * (goal_y - Y) + execution_time * (goal_yd - V)) + ) / execution_time ** 2 + goal_ydd + + def determine_forces(T, Y, alpha_y, beta_y, allow_final_velocity): """Determine forces that the forcing term should generate. @@ -757,15 +767,3 @@ def dmp_open_loop( Y.append(np.copy(current_y)) return np.array(T), np.array(Y) - - -DMP_STEP_FUNCTIONS = { - "rk4": dmp_step_rk4, - "euler": dmp_step_euler -} - -try: - from ..dmp_fast import dmp_step as dmp_step_euler_cython - DMP_STEP_FUNCTIONS["euler-cython"] = dmp_step_euler_cython -except ImportError: - pass diff --git a/movement_primitives/dmp/_dual_cartesian_dmp.py b/movement_primitives/dmp/_dual_cartesian_dmp.py index a8b4acb..c08a450 100644 --- a/movement_primitives/dmp/_dual_cartesian_dmp.py +++ b/movement_primitives/dmp/_dual_cartesian_dmp.py @@ -1,4 +1,3 @@ -import warnings import numpy as np import pytransform3d.rotations as pr from ._base import DMPBase @@ -217,8 +216,8 @@ def step(self, last_y, last_yd, coupling_term=None, coupling_term : object, optional (default: None) Coupling term that will be added to velocity. - step_function : str, optional (default: 'cython' if available) - DMP integration function. Possible options: 'python', 'cython'. + step_function : callable, optional (default: cython code if available) + DMP integration function. Returns ------- From 5cd17ea5f46daf25a0d01d2b3eff589ea214189a Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 00:15:21 +0100 Subject: [PATCH 07/16] Test invalid step functions --- test/test_cartesian_dmp.py | 12 +++++++++++- test/test_dmp.py | 10 +++++++++- test/test_dual_cartesian_dmp.py | 9 +++++++++ 3 files changed, 29 insertions(+), 2 deletions(-) create mode 100644 test/test_dual_cartesian_dmp.py diff --git a/test/test_cartesian_dmp.py b/test/test_cartesian_dmp.py index 4f20932..8301b44 100644 --- a/test/test_cartesian_dmp.py +++ b/test/test_cartesian_dmp.py @@ -1,7 +1,7 @@ import numpy as np from movement_primitives.dmp import CartesianDMP from pytransform3d import rotations as pr -from numpy.testing import assert_array_almost_equal +from numpy.testing import assert_array_almost_equal, assert_raises_regex def test_imitate_cartesian_dmp(): @@ -103,3 +103,13 @@ def test_get_set_weights(): T3, Y3 = dmp.open_loop() assert_array_almost_equal(T, T3) assert_array_almost_equal(Y, Y3, decimal=2) + + +def test_invalid_step_function(): + dmp = CartesianDMP( + execution_time=1.0, dt=0.01, + n_weights_per_dim=10, int_dt=0.001) + assert_raises_regex(ValueError, "Step function", dmp.open_loop, + step_function="invalid") + assert_raises_regex(ValueError, "Step function", dmp.open_loop, + quaternion_step_function="invalid") diff --git a/test/test_dmp.py b/test/test_dmp.py index f5e11d0..f9ba944 100644 --- a/test/test_dmp.py +++ b/test/test_dmp.py @@ -3,7 +3,7 @@ from movement_primitives.dmp import DMP from nose.tools import (assert_almost_equal, assert_equal, assert_less, assert_raises_regexp) -from numpy.testing import assert_array_almost_equal +from numpy.testing import assert_array_almost_equal, assert_raises_regex def test_dmp_step_function_unknown(): @@ -198,6 +198,14 @@ def test_get_set_weights(): assert_array_almost_equal(Y, Y3, decimal=2) +def test_invalid_step_function(): + dmp = DMP( + execution_time=1.0, n_dims=3, dt=0.01, + n_weights_per_dim=10, int_dt=0.001) + assert_raises_regex(ValueError, "Step function", dmp.open_loop, + step_function="invalid") + + if __name__ == "__main__": import matplotlib.pyplot as plt start_y = np.array([0.0]) diff --git a/test/test_dual_cartesian_dmp.py b/test/test_dual_cartesian_dmp.py new file mode 100644 index 0000000..21001c8 --- /dev/null +++ b/test/test_dual_cartesian_dmp.py @@ -0,0 +1,9 @@ +from movement_primitives.dmp import DualCartesianDMP +from nose.tools import assert_raises_regexp + + +def test_invalid_step_function(): + dmp = DualCartesianDMP( + execution_time=1.0, dt=0.01, n_weights_per_dim=10, int_dt=0.001) + assert_raises_regexp(ValueError, "Step function", dmp.open_loop, + step_function="invalid") From 808f69f65093ca9502329bae410fd92390f56e9e Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 00:18:58 +0100 Subject: [PATCH 08/16] Use public functions --- benchmarks/benchmark_dmp.py | 2 +- benchmarks/benchmark_dual_dmp.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/benchmarks/benchmark_dmp.py b/benchmarks/benchmark_dmp.py index bf1875a..d808305 100644 --- a/benchmarks/benchmark_dmp.py +++ b/benchmarks/benchmark_dmp.py @@ -12,7 +12,7 @@ dmp = DMP(n_dims=n_dims, execution_time=1.0, dt=dt, n_weights_per_dim=6, int_dt=int_dt) dmp.configure(start_y=start_y, goal_y=goal_y) -dmp.forcing_term.weights = 1000 * np.random.randn(*dmp.forcing_term.weights.shape) +dmp.set_weights(1000 * np.random.randn(*dmp.get_weights().shape)) times = timeit.repeat(partial(dmp.open_loop, step_function="rk4"), repeat=10, number=1) print("RK4") diff --git a/benchmarks/benchmark_dual_dmp.py b/benchmarks/benchmark_dual_dmp.py index 2a26a49..9369f7b 100644 --- a/benchmarks/benchmark_dual_dmp.py +++ b/benchmarks/benchmark_dual_dmp.py @@ -11,7 +11,7 @@ dmp = DualCartesianDMP(execution_time=1.0, dt=dt, n_weights_per_dim=6, int_dt=int_dt) dmp.configure(start_y=start_y, goal_y=goal_y) -dmp.forcing_term.weights = 1000 * np.random.randn(*dmp.forcing_term.weights.shape) +dmp.set_weights(1000 * np.random.randn(*dmp.get_weights().shape)) times = timeit.repeat(partial(dmp.open_loop, step_function="cython"), repeat=10, number=1) print("Cython") From 498be02b2aa92ad96e673a36aa69c91bda3b4401 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 00:33:16 +0100 Subject: [PATCH 09/16] Don't initialize coupling terms if not necessary --- movement_primitives/dmp_fast.pyx | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx index 6c594fa..a49679f 100644 --- a/movement_primitives/dmp_fast.pyx +++ b/movement_primitives/dmp_fast.pyx @@ -50,8 +50,8 @@ cpdef dmp_step( cdef np.ndarray[double, ndim=1] current_ydd = np.empty(n_dims, dtype=np.float64) - cdef np.ndarray[double, ndim=1] cd = np.zeros(n_dims, dtype=np.float64) - cdef np.ndarray[double, ndim=1] cdd = np.zeros(n_dims, dtype=np.float64) + cdef np.ndarray[double, ndim=1] cd = np.empty(n_dims, dtype=np.float64) + cdef np.ndarray[double, ndim=1] cdd = np.empty(n_dims, dtype=np.float64) cdef np.ndarray[double, ndim=1] f = np.empty(n_dims, dtype=np.float64) @@ -71,13 +71,22 @@ cpdef dmp_step( elif coupling_term_precomputed is not None: cd[:] = coupling_term_precomputed[0] cdd[:] = coupling_term_precomputed[1] + else: + cd[:] = 0.0 + cdd[:] = 0.0 if tracking_error is not None: cdd += p_gain * tracking_error / dt f[:] = forcing_term(current_t).squeeze() for d in range(n_dims): - current_ydd[d] = (alpha_y * (beta_y * (goal_y[d] - current_y[d]) + execution_time * goal_yd[d] - execution_time * current_yd[d]) + goal_ydd[d] * execution_time ** 2 + f[d] + cdd[d]) / execution_time ** 2 + current_ydd[d] = ( + alpha_y * (beta_y * (goal_y[d] - current_y[d]) + + execution_time * goal_yd[d] + - execution_time * current_yd[d]) + + goal_ydd[d] * execution_time ** 2 + f[d] + + cdd[d] + ) / execution_time ** 2 current_yd[d] += dt * current_ydd[d] + cd[d] / execution_time current_y[d] += dt * current_yd[d] From b42564e4f7e4510ad7969f793a23ed0c1be4efea Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 00:43:20 +0100 Subject: [PATCH 10/16] Layout code --- movement_primitives/dmp_fast.pyx | 32 +++++++++++++++++++++++++------- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx index a49679f..b1c2a90 100644 --- a/movement_primitives/dmp_fast.pyx +++ b/movement_primitives/dmp_fast.pyx @@ -234,14 +234,15 @@ cpdef dmp_step_dual_cartesian( cdef int n_vel_dims = current_yd.shape[0] - cdef np.ndarray[double, ndim=1] cd = np.zeros(n_vel_dims, dtype=np.float64) - cdef np.ndarray[double, ndim=1] cdd = np.zeros(n_vel_dims, dtype=np.float64) + 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] f = np.empty(n_vel_dims, dtype=np.float64) cdef int pps cdef int pvs - cdef np.ndarray[long, ndim=2] POS_INDICES = np.array([[0, 0], [1, 1], [2, 2], [7, 6], [8, 7], [9, 8]], dtype=long) + cdef np.ndarray[long, ndim=2] POS_INDICES = np.array( + [[0, 0], [1, 1], [2, 2], [7, 6], [8, 7], [9, 8]], dtype=long) cdef double dt cdef double current_t = last_t @@ -251,7 +252,10 @@ cpdef dmp_step_dual_cartesian( dt = t - current_t current_t += dt - if coupling_term is not None: + if coupling_term is None: + cd[:] = 0.0 + cdd[:] = 0.0 + else: cd[:], cdd[:] = coupling_term.coupling(current_y, current_yd) f[:] = forcing_term(current_t).squeeze() @@ -263,15 +267,29 @@ cpdef dmp_step_dual_cartesian( # position components for pps, pvs in POS_INDICES: - current_ydd[pvs] = (alpha_y * (beta_y * (goal_y[pps] - current_y[pps]) + execution_time * (goal_yd[pvs] - current_yd[pvs])) + f[pvs] + cdd[pvs]) / execution_time ** 2 + goal_ydd[pvs] + current_ydd[pvs] = ( + alpha_y * (beta_y * (goal_y[pps] - current_y[pps]) + + execution_time * (goal_yd[pvs] - current_yd[pvs])) + + f[pvs] + cdd[pvs] + ) / execution_time ** 2 + goal_ydd[pvs] current_yd[pvs] += dt * current_ydd[pvs] + cd[pvs] / execution_time current_y[pps] += dt * current_yd[pvs] # orientation components for ops, ovs in ((slice(3, 7), slice(3, 6)), (slice(10, 14), slice(9, 12))): - current_ydd[ovs] = (alpha_y * (beta_y * compact_axis_angle_from_quaternion(concatenate_quaternions(goal_y[ops], q_conj(current_y[ops]))) - execution_time * current_yd[ovs]) + f[ovs] + cdd[ovs]) / execution_time ** 2 + current_ydd[ovs] = ( + alpha_y * ( + beta_y * compact_axis_angle_from_quaternion( + concatenate_quaternions( + goal_y[ops], q_conj(current_y[ops])) + ) + - execution_time * current_yd[ovs] + ) + f[ovs] + cdd[ovs] + ) / execution_time ** 2 current_yd[ovs] += dt * current_ydd[ovs] + cd[ovs] / execution_time - current_y[ops] = concatenate_quaternions(quaternion_from_compact_axis_angle(dt * current_yd[ovs]), current_y[ops]) + current_y[ops] = concatenate_quaternions( + quaternion_from_compact_axis_angle(dt * current_yd[ovs]), + current_y[ops]) @cython.boundscheck(False) From fd2a7095e5fd354e9b140a987fb8ff4c5bdcdc89 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 14:50:51 +0100 Subject: [PATCH 11/16] Make RK4 implementation in Cython faster --- benchmarks/benchmark_dmp.py | 4 ++ movement_primitives/dmp/_dmp.py | 3 +- movement_primitives/dmp_fast.pyx | 69 ++++++++++++++++++-------------- test/test_dmp.py | 19 +++++++++ 4 files changed, 64 insertions(+), 31 deletions(-) diff --git a/benchmarks/benchmark_dmp.py b/benchmarks/benchmark_dmp.py index d808305..3f4c034 100644 --- a/benchmarks/benchmark_dmp.py +++ b/benchmarks/benchmark_dmp.py @@ -25,3 +25,7 @@ times = timeit.repeat(partial(dmp.open_loop, step_function="euler-cython"), repeat=10, number=1) print("Euler (Cython)") print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) + +times = timeit.repeat(partial(dmp.open_loop, step_function="rk4-cython"), repeat=10, number=1) +print("RK4 (Cython)") +print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times))) diff --git a/movement_primitives/dmp/_dmp.py b/movement_primitives/dmp/_dmp.py index d036d26..69b2671 100644 --- a/movement_primitives/dmp/_dmp.py +++ b/movement_primitives/dmp/_dmp.py @@ -288,8 +288,9 @@ def dmp_step_euler( DEFAULT_DMP_STEP_FUNCTION = "rk4" try: - from ..dmp_fast import dmp_step as dmp_step_euler_cython + from ..dmp_fast import dmp_step as dmp_step_euler_cython, dmp_step_rk4 as dmp_step_rk4_cython DMP_STEP_FUNCTIONS["euler-cython"] = dmp_step_euler_cython + DMP_STEP_FUNCTIONS["rk4-cython"] = dmp_step_rk4_cython except ImportError: pass diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx index b1c2a90..aee0977 100644 --- a/movement_primitives/dmp_fast.pyx +++ b/movement_primitives/dmp_fast.pyx @@ -105,15 +105,6 @@ cpdef dmp_step_rk4( tuple coupling_term_precomputed=None, double int_dt=0.001, double p_gain=0.0, np.ndarray tracking_error=None): - - if start_t >= goal_t: - raise ValueError("Goal must be chronologically after start!") - - if t <= start_t: - return np.copy(start_y), np.copy(start_yd), np.copy(start_ydd) - - cdef double execution_time = goal_t - start_t - cdef int n_dims = current_y.shape[0] cdef np.ndarray[double, ndim=1] cd = np.zeros(n_dims, dtype=np.float64) @@ -122,41 +113,59 @@ cpdef dmp_step_rk4( cd += coupling_term_precomputed[0] cdd += coupling_term_precomputed[1] + # precompute constants for following queries + cdef double execution_time = goal_t - start_t cdef double dt = t - last_t + cdef double dt_2 = 0.5 * dt + cdef np.ndarray[double, ndim=2] F = forcing_term(np.array([t, t + dt_2, t + dt])) + cdef np.ndarray[double, ndim=1] tdd + if tracking_error is not None: + tdd = p_gain * tracking_error / dt + else: + tdd = np.zeros(n_dims, dtype=np.float64) cdef np.ndarray[double, ndim=1] Y = current_y cdef np.ndarray[double, ndim=1] V = current_yd cdef np.ndarray[double, ndim=1] C0 = current_yd - cdef np.ndarray[double, ndim=1] K0 = _dmp_acc(Y, C0, t, cd, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, forcing_term, coupling_term, p_gain, tracking_error) - cdef np.ndarray[double, ndim=1] C1 = V + 0.5 * dt * K0 - cdef np.ndarray[double, ndim=1] K1 = _dmp_acc(Y + 0.5 * dt * C0, C1, t + 0.5 * dt, cd, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, forcing_term, coupling_term, p_gain, tracking_error) - cdef np.ndarray[double, ndim=1] C2 = V + 0.5 * dt * K1 - cdef np.ndarray[double, ndim=1] K2 = _dmp_acc(Y + 0.5 * dt * C1, C2, t + 0.5 * dt, cd, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, forcing_term, coupling_term, p_gain, tracking_error) + cdef np.ndarray[double, ndim=1] K0 = _dmp_acc( + Y, C0, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, + execution_time, F[:, 0], coupling_term, p_gain, tdd) + cdef np.ndarray[double, ndim=1] C1 = V + dt_2 * K0 + cdef np.ndarray[double, ndim=1] K1 = _dmp_acc( + Y + dt_2 * C0, C1, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + goal_ydd, execution_time, F[:, 1], coupling_term, p_gain, tdd) + cdef np.ndarray[double, ndim=1] C2 = V + dt_2 * K1 + cdef np.ndarray[double, ndim=1] K2 = _dmp_acc( + Y + dt_2 * C1, C2, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + goal_ydd, execution_time, F[:, 1], coupling_term, p_gain, tdd) cdef np.ndarray[double, ndim=1] C3 = V + dt * K2 - cdef np.ndarray[double, ndim=1] K3 = _dmp_acc(Y + dt * C2, C3, t + 0.5 * dt, cd, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, forcing_term, coupling_term, p_gain, tracking_error) - - cdef np.ndarray[double, ndim=1] Y_step = dt * (C0 + 2 * C1 + 2 * C2 + C3) / 6.0 - cdef np.ndarray[double, ndim=1] V_step = dt * (K0 + 2 * K1 + 2 * K2 + K3) / 6.0 + cdef np.ndarray[double, ndim=1] K3 = _dmp_acc( + Y + dt * C2, C3, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + goal_ydd, execution_time, F[:, 2], coupling_term, p_gain, tdd) - current_y += Y_step - current_yd += V_step + current_y += dt * (C0 + 2 * C1 + 2 * C2 + C3) / 6.0 + current_yd += dt * (K0 + 2 * K1 + 2 * K2 + K3) / 6.0 if coupling_term is not None: cd[:], _ = coupling_term.coupling(Y, V) current_yd += cd / execution_time -cpdef _dmp_acc( - np.ndarray[double, ndim=1] Y, np.ndarray[double, ndim=1] V, double t, np.ndarray[double, ndim=1] cd, - np.ndarray[double, ndim=1] cdd, double dt, double alpha_y, double beta_y, np.ndarray[double, ndim=1] goal_y, - np.ndarray[double, ndim=1] goal_yd, np.ndarray[double, ndim=1] goal_ydd, double execution_time, - object forcing_term, object coupling_term, double p_gain, np.ndarray[double, ndim=1] tracking_error): +cdef _dmp_acc( + np.ndarray[double, ndim=1] Y, np.ndarray[double, ndim=1] V, + np.ndarray[double, ndim=1] cdd, double dt, double alpha_y, double beta_y, + np.ndarray[double, ndim=1] goal_y, np.ndarray[double, ndim=1] goal_yd, + np.ndarray[double, ndim=1] goal_ydd, double execution_time, + np.ndarray[double, ndim=1] f, object coupling_term, double p_gain, + np.ndarray[double, ndim=1] tdd): if coupling_term is not None: - cd[:], cdd[:] = coupling_term.coupling(Y, V) - if tracking_error is not None: - cdd += p_gain * tracking_error / dt - f = forcing_term(t).squeeze() - return (alpha_y * (beta_y * (goal_y - Y) + execution_time * goal_yd - execution_time * V) + goal_ydd * execution_time ** 2 + f + cdd) / execution_time ** 2 + _, cdd[:] = coupling_term.coupling(Y, V) + + return (( + alpha_y * (beta_y * (goal_y - Y) + + execution_time * goal_yd - execution_time * V) + + f + cdd + tdd + ) / execution_time ** 2) + goal_ydd @cython.boundscheck(False) diff --git a/test/test_dmp.py b/test/test_dmp.py index f9ba944..55a801b 100644 --- a/test/test_dmp.py +++ b/test/test_dmp.py @@ -143,6 +143,25 @@ def test_compare_integrators(): assert_less(error_rk4, error_euler) +def test_compare_rk4_python_cython(): + execution_time = 1.0 + dt = 0.001 + + dmp = DMP(n_dims=2, execution_time=execution_time, dt=dt, + n_weights_per_dim=100) + + T = np.arange(0.0, execution_time + dt, dt) + Y_demo = np.empty((len(T), 2)) + Y_demo[:, 0] = np.cos(2.5 * np.pi * T) + Y_demo[:, 1] = 0.5 + np.cos(1.5 * np.pi * T) + dmp.imitate(T, Y_demo) + + dmp.configure(start_y=Y_demo[0], goal_y=Y_demo[-1]) + T_python, Y_python = dmp.open_loop(step_function="rk4") + T_cython, Y_cython = dmp.open_loop(step_function="rk4-cython") + assert_array_almost_equal(Y_cython, Y_python) + + def test_set_current_time(): start_y = np.array([0.0]) goal_y = np.array([1.0]) From da61c8d205f13d4428f9dfa4ec89f6db37ed0b32 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 15:08:11 +0100 Subject: [PATCH 12/16] Less copying of arrays --- movement_primitives/dmp_fast.pyx | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx index aee0977..14ea19e 100644 --- a/movement_primitives/dmp_fast.pyx +++ b/movement_primitives/dmp_fast.pyx @@ -120,34 +120,31 @@ cpdef dmp_step_rk4( cdef np.ndarray[double, ndim=2] F = forcing_term(np.array([t, t + dt_2, t + dt])) cdef np.ndarray[double, ndim=1] tdd if tracking_error is not None: - tdd = p_gain * tracking_error / dt + tdd = p_gain / dt * tracking_error else: tdd = np.zeros(n_dims, dtype=np.float64) - cdef np.ndarray[double, ndim=1] Y = current_y - cdef np.ndarray[double, ndim=1] V = current_yd - cdef np.ndarray[double, ndim=1] C0 = current_yd cdef np.ndarray[double, ndim=1] K0 = _dmp_acc( - Y, C0, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, - execution_time, F[:, 0], coupling_term, p_gain, tdd) - cdef np.ndarray[double, ndim=1] C1 = V + dt_2 * K0 + current_y, current_yd, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + goal_ydd, execution_time, F[:, 0], coupling_term, p_gain, tdd) + cdef np.ndarray[double, ndim=1] C1 = current_yd + dt_2 * K0 cdef np.ndarray[double, ndim=1] K1 = _dmp_acc( - Y + dt_2 * C0, C1, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + current_y + dt_2 * current_yd, C1, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, F[:, 1], coupling_term, p_gain, tdd) - cdef np.ndarray[double, ndim=1] C2 = V + dt_2 * K1 + cdef np.ndarray[double, ndim=1] C2 = current_yd + dt_2 * K1 cdef np.ndarray[double, ndim=1] K2 = _dmp_acc( - Y + dt_2 * C1, C2, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + current_y + dt_2 * C1, C2, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, F[:, 1], coupling_term, p_gain, tdd) - cdef np.ndarray[double, ndim=1] C3 = V + dt * K2 + cdef np.ndarray[double, ndim=1] C3 = current_yd + dt * K2 cdef np.ndarray[double, ndim=1] K3 = _dmp_acc( - Y + dt * C2, C3, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, + current_y + dt * C2, C3, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, F[:, 2], coupling_term, p_gain, tdd) - current_y += dt * (C0 + 2 * C1 + 2 * C2 + C3) / 6.0 + current_y += dt * (current_yd + 2 * C1 + 2 * C2 + C3) / 6.0 current_yd += dt * (K0 + 2 * K1 + 2 * K2 + K3) / 6.0 if coupling_term is not None: - cd[:], _ = coupling_term.coupling(Y, V) + cd[:], _ = coupling_term.coupling(current_y, current_yd) current_yd += cd / execution_time From 2e9041425d585546f681747f68e8864b8e6e02aa Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 15:18:51 +0100 Subject: [PATCH 13/16] Speed up Cython implementation with loops --- movement_primitives/dmp_fast.pyx | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx index 14ea19e..4e9cca0 100644 --- a/movement_primitives/dmp_fast.pyx +++ b/movement_primitives/dmp_fast.pyx @@ -140,8 +140,10 @@ cpdef dmp_step_rk4( current_y + dt * C2, C3, cdd, dt, alpha_y, beta_y, goal_y, goal_yd, goal_ydd, execution_time, F[:, 2], coupling_term, p_gain, tdd) - current_y += dt * (current_yd + 2 * C1 + 2 * C2 + C3) / 6.0 - current_yd += dt * (K0 + 2 * K1 + 2 * K2 + K3) / 6.0 + cdef int i + for i in range(n_dims): + current_y[i] += dt * (current_yd[i] + 2 * C1[i] + 2 * C2[i] + C3[i]) / 6.0 + current_yd[i] += dt * (K0[i] + 2 * K1[i] + 2 * K2[i] + K3[i]) / 6.0 if coupling_term is not None: cd[:], _ = coupling_term.coupling(current_y, current_yd) @@ -158,11 +160,16 @@ cdef _dmp_acc( if coupling_term is not None: _, cdd[:] = coupling_term.coupling(Y, V) - return (( - alpha_y * (beta_y * (goal_y - Y) - + execution_time * goal_yd - execution_time * V) - + f + cdd + tdd - ) / execution_time ** 2) + goal_ydd + cdef int n_dims = Y.shape[0] + cdef np.ndarray[double, ndim=1] Ydd = np.empty(n_dims, dtype=np.float64) + cdef int i + for i in range(n_dims): + Ydd[i] = (( + alpha_y * (beta_y * (goal_y[i] - Y[i]) + + execution_time * goal_yd[i] - execution_time * V[i]) + + f[i] + cdd[i] + tdd[i] + ) / execution_time ** 2) + goal_ydd[i] + return Ydd @cython.boundscheck(False) From da570c0fb312d26344079989145262d0e87e42f5 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 18:29:33 +0100 Subject: [PATCH 14/16] Use faster RK4 implementation --- movement_primitives/dmp/_cartesian_dmp.py | 2 +- movement_primitives/dmp/_dmp.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/movement_primitives/dmp/_cartesian_dmp.py b/movement_primitives/dmp/_cartesian_dmp.py index 34b167e..29dbce4 100644 --- a/movement_primitives/dmp/_cartesian_dmp.py +++ b/movement_primitives/dmp/_cartesian_dmp.py @@ -265,7 +265,7 @@ def open_loop(self, run_t=None, coupling_term=None, step_function : str, optional (default: 'rk4') DMP integration function. Possible options: 'rk4', 'euler', - 'euler-cython'. + 'euler-cython', 'rk4-cython'. quaternion_step_function : str, optional (default: 'cython' if available) DMP integration function. Possible options: 'python', 'cython'. diff --git a/movement_primitives/dmp/_dmp.py b/movement_primitives/dmp/_dmp.py index 69b2671..2f7ad60 100644 --- a/movement_primitives/dmp/_dmp.py +++ b/movement_primitives/dmp/_dmp.py @@ -291,6 +291,7 @@ def dmp_step_euler( from ..dmp_fast import dmp_step as dmp_step_euler_cython, dmp_step_rk4 as dmp_step_rk4_cython DMP_STEP_FUNCTIONS["euler-cython"] = dmp_step_euler_cython DMP_STEP_FUNCTIONS["rk4-cython"] = dmp_step_rk4_cython + DEFAULT_DMP_STEP_FUNCTION = "rk4-cython" except ImportError: pass @@ -410,7 +411,7 @@ def open_loop(self, run_t=None, coupling_term=None, step_function : str, optional (default: 'rk4') DMP integration function. Possible options: 'rk4', 'euler', - 'euler-cython'. + 'euler-cython', 'rk4-cython'. Returns ------- From 23a288bda79a32af3381d036a91dbe83e31d367b Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 18:35:35 +0100 Subject: [PATCH 15/16] Remove unused import --- movement_primitives/dmp/_cartesian_dmp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/movement_primitives/dmp/_cartesian_dmp.py b/movement_primitives/dmp/_cartesian_dmp.py index 29dbce4..329b0b1 100644 --- a/movement_primitives/dmp/_cartesian_dmp.py +++ b/movement_primitives/dmp/_cartesian_dmp.py @@ -3,7 +3,7 @@ from ._base import DMPBase from ._forcing_term import ForcingTerm from ._canonical_system import canonical_system_alpha -from ._dmp import (dmp_step_rk4, dmp_open_loop, dmp_imitate, ridge_regression, +from ._dmp import (dmp_open_loop, dmp_imitate, ridge_regression, DMP_STEP_FUNCTIONS, DEFAULT_DMP_STEP_FUNCTION) From 78c94a4b9e12c8f6998fe9d0b5f2027d84999c95 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 22 Dec 2021 18:35:45 +0100 Subject: [PATCH 16/16] Add docstrings --- movement_primitives/dmp_fast.pyx | 310 ++++++++++++++++++++++++++++++- 1 file changed, 303 insertions(+), 7 deletions(-) diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx index 4e9cca0..3a01877 100644 --- a/movement_primitives/dmp_fast.pyx +++ b/movement_primitives/dmp_fast.pyx @@ -17,6 +17,33 @@ cdef double M_2PI = 2.0 * pi @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): + """Map time to phase. + + Parameters + ---------- + t : float + Current time. + + alpha : float + Value of the alpha parameter of the canonical system. + + goal_t : float + Time at which the execution should be done. + + 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) @@ -36,7 +63,72 @@ cpdef dmp_step( tuple coupling_term_precomputed=None, double int_dt=0.001, double p_gain=0.0, np.ndarray tracking_error=None): + """Integrate regular DMP for one step with Euler integration. + + Parameters + ---------- + last_t : float + Time at last step. + + t : float + Time at current step. + + current_y : array, shape (n_dims,) + Current position. Will be modified. + + current_yd : array, shape (n_dims,) + Current velocity. Will be modified. + + goal_y : array, shape (n_dims,) + Goal position. + + goal_yd : array, shape (n_dims,) + Goal velocity. + + goal_ydd : array, shape (n_dims,) + Goal acceleration. + + start_y : array, shape (n_dims,) + Start position. + + start_yd : array, shape (n_dims,) + Start velocity. + + start_ydd : array, shape (n_dims,) + Start acceleration. + + goal_t : float + Time at the end. + + start_t : float + Time at the start. + + alpha_y : float + Constant in transformation system. + + beta_y : float + Constant in transformation system. + + forcing_term : ForcingTerm + Forcing term. + coupling_term : CouplingTerm, optional (default: None) + Coupling term. Must have a function coupling(y, yd) that returns + additional velocity and acceleration. + + coupling_term_precomputed : tuple + A precomputed coupling term, i.e., additional velocity and + acceleration. + + int_dt : float, optional (default: 0.001) + Time delta used internally for integration. + + p_gain : float, optional (default: 0) + Proportional gain for tracking error. + + tracking_error : float, optional (default: 0) + Tracking error from last step. + """ if start_t >= goal_t: raise ValueError("Goal must be chronologically after start!") @@ -105,6 +197,72 @@ cpdef dmp_step_rk4( tuple coupling_term_precomputed=None, double int_dt=0.001, double p_gain=0.0, np.ndarray tracking_error=None): + """Integrate regular DMP for one step with RK4 integration. + + Parameters + ---------- + last_t : float + Time at last step. + + t : float + Time at current step. + + current_y : array, shape (n_dims,) + Current position. Will be modified. + + current_yd : array, shape (n_dims,) + Current velocity. Will be modified. + + goal_y : array, shape (n_dims,) + Goal position. + + goal_yd : array, shape (n_dims,) + Goal velocity. + + goal_ydd : array, shape (n_dims,) + Goal acceleration. + + start_y : array, shape (n_dims,) + Start position. + + start_yd : array, shape (n_dims,) + Start velocity. + + start_ydd : array, shape (n_dims,) + Start acceleration. + + goal_t : float + Time at the end. + + start_t : float + Time at the start. + + alpha_y : float + Constant in transformation system. + + beta_y : float + Constant in transformation system. + + forcing_term : ForcingTerm + Forcing term. + + coupling_term : CouplingTerm, optional (default: None) + Coupling term. Must have a function coupling(y, yd) that returns + additional velocity and acceleration. + + coupling_term_precomputed : tuple + A precomputed coupling term, i.e., additional velocity and + acceleration. + + int_dt : float, optional (default: 0.001) + Time delta used internally for integration. + + p_gain : float, optional (default: 0) + Proportional gain for tracking error. + + tracking_error : float, optional (default: 0) + Tracking error from last step. + """ cdef int n_dims = current_y.shape[0] cdef np.ndarray[double, ndim=1] cd = np.zeros(n_dims, dtype=np.float64) @@ -178,15 +336,77 @@ cdef _dmp_acc( @cython.cdivision(True) cpdef dmp_step_quaternion( double last_t, double t, - np.ndarray[double, ndim=1] current_y, np.ndarray[double, ndim=1] current_yd, - np.ndarray[double, ndim=1] goal_y, np.ndarray[double, ndim=1] goal_yd, np.ndarray[double, ndim=1] goal_ydd, - np.ndarray[double, ndim=1] start_y, np.ndarray[double, ndim=1] start_yd, np.ndarray[double, ndim=1] start_ydd, + np.ndarray[double, ndim=1] current_y, + np.ndarray[double, ndim=1] current_yd, + np.ndarray[double, ndim=1] goal_y, + np.ndarray[double, ndim=1] goal_yd, + np.ndarray[double, ndim=1] goal_ydd, + np.ndarray[double, ndim=1] start_y, + np.ndarray[double, ndim=1] start_yd, + np.ndarray[double, ndim=1] start_ydd, double goal_t, double start_t, double alpha_y, double beta_y, - forcing_term, - coupling_term=None, - coupling_term_precomputed=None, + forcing_term, coupling_term=None, coupling_term_precomputed=None, double int_dt=0.001): + """Integrate quaternion DMP for one step with Euler integration. + + Parameters + ---------- + last_t : float + Time at last step. + + t : float + Time at current step. + + current_y : array, shape (7,) + Current position. Will be modified. + + current_yd : array, shape (6,) + Current velocity. Will be modified. + + goal_y : array, shape (7,) + Goal position. + + goal_yd : array, shape (6,) + Goal velocity. + + goal_ydd : array, shape (6,) + Goal acceleration. + + start_y : array, shape (7,) + Start position. + + start_yd : array, shape (6,) + Start velocity. + + start_ydd : array, shape (6,) + Start acceleration. + + goal_t : float + Time at the end. + start_t : float + Time at the start. + + alpha_y : float + Constant in transformation system. + + beta_y : float + Constant in transformation system. + + forcing_term : ForcingTerm + Forcing term. + + coupling_term : CouplingTerm, optional (default: None) + Coupling term. Must have a function coupling(y, yd) that returns + additional velocity and acceleration. + + coupling_term_precomputed : tuple + A precomputed coupling term, i.e., additional velocity and + acceleration. + + int_dt : float, optional (default: 0.001) + Time delta used internally for integration. + """ if t <= start_t: current_y[:] = start_y current_yd[:] = start_yd @@ -237,6 +457,68 @@ cpdef dmp_step_dual_cartesian( forcing_term, coupling_term=None, double int_dt=0.001, double p_gain=0.0, np.ndarray tracking_error=None): + """Integrate bimanual Cartesian DMP for one step with Euler integration. + + Parameters + ---------- + last_t : float + Time at last step. + + t : float + Time at current step. + + current_y : array, shape (14,) + Current position. Will be modified. + + current_yd : array, shape (12,) + Current velocity. Will be modified. + + goal_y : array, shape (14,) + Goal position. + + goal_yd : array, shape (12,) + Goal velocity. + + goal_ydd : array, shape (12,) + Goal acceleration. + + start_y : array, shape (14,) + Start position. + + start_yd : array, shape (12,) + Start velocity. + + start_ydd : array, shape (12,) + Start acceleration. + + goal_t : float + Time at the end. + + start_t : float + Time at the start. + + alpha_y : float + Constant in transformation system. + + beta_y : float + Constant in transformation system. + + forcing_term : ForcingTerm + Forcing term. + + coupling_term : CouplingTerm, optional (default: None) + Coupling term. Must have a function coupling(y, yd) that returns + additional velocity and acceleration. + + int_dt : float, optional (default: 0.001) + Time delta used internally for integration. + + p_gain : float, optional (default: 0) + Proportional gain for tracking error. + + tracking_error : float, optional (default: 0) + Tracking error from last step. + """ if t <= start_t: current_y[:] = start_y current_yd[:] = start_yd @@ -309,15 +591,20 @@ cpdef dmp_step_dual_cartesian( @cython.wraparound(False) @cython.nonecheck(False) @cython.cdivision(True) -cpdef concatenate_quaternions(np.ndarray[double, ndim=1] q1, np.ndarray[double, ndim=1] q2): +cpdef concatenate_quaternions( + np.ndarray[double, ndim=1] q1, np.ndarray[double, ndim=1] q2): """Concatenate two quaternions. + We use Hamilton's quaternion multiplication. + Parameters ---------- q1 : array-like, shape (4,) First quaternion + q2 : array-like, shape (4,) Second quaternion + Returns ------- q12 : array-like, shape (4,) @@ -346,11 +633,14 @@ cpdef concatenate_quaternions(np.ndarray[double, ndim=1] q1, np.ndarray[double, @cython.cdivision(True) cdef quaternion_from_compact_axis_angle(np.ndarray[double, ndim=1] a): """Compute quaternion from compact axis-angle (exponential map). + We usually assume active rotations. + Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: angle * (x, y, z) + Returns ------- q : array-like, shape (4,) @@ -379,13 +669,16 @@ cdef quaternion_from_compact_axis_angle(np.ndarray[double, ndim=1] a): @cython.cdivision(True) cdef q_conj(np.ndarray[double, ndim=1] q): """Conjugate of quaternion. + The conjugate of a unit quaternion inverts the rotation represented by this unit quaternion. The conjugate of a quaternion q is often denoted as q*. + Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) + Returns ------- q_c : array-like, shape (4,) @@ -400,11 +693,14 @@ cdef q_conj(np.ndarray[double, ndim=1] q): @cython.cdivision(True) cpdef compact_axis_angle_from_quaternion(np.ndarray[double, ndim=1] q): """Compute compact axis-angle from quaternion (logarithmic map). + We usually assume active rotations. + Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) + Returns ------- a : array-like, shape (3,)