From 5ac28663fd0f33af14c6a970a814c2396aefc890 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 17 Nov 2021 16:30:33 +0100 Subject: [PATCH] Test stepping through DMP with velocity --- test/test_dmp_with_final_velocity.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/test/test_dmp_with_final_velocity.py b/test/test_dmp_with_final_velocity.py index 1167e6f..baf7b75 100644 --- a/test/test_dmp_with_final_velocity.py +++ b/test/test_dmp_with_final_velocity.py @@ -18,3 +18,22 @@ def test_final_velocity(): _, Y = dmp.open_loop(run_t=execution_time) goal_yd_actual = (Y[-1] - Y[-2]) / dt assert_array_almost_equal(goal_yd_expected, goal_yd_actual, decimal=2) + + +def test_step_through_dmp_with_final_velocity(): + dt = 0.01 + execution_time = 1.0 + T = np.arange(0, execution_time + dt, dt) + Y = np.column_stack((np.cos(np.pi * T), -np.cos(np.pi * T))) + + dmp = DMPWithFinalVelocity(n_dims=2, execution_time=execution_time) + dmp.imitate(T, Y) + dmp.configure(start_y=np.array([0, 0], dtype=float), + goal_y=np.array([1, 1], dtype=float)) + current_y = np.copy(dmp.start_y) + current_yd = np.copy(dmp.start_yd) + path = [np.copy(current_y)] + while dmp.t <= dmp.execution_time: + current_y, current_yd = dmp.step(current_y, current_yd) + path.append(np.copy(current_y)) + assert_array_almost_equal(np.vstack(path), dmp.open_loop()[1])