Skip to content

Commit

Permalink
Test stepping through DMP with velocity
Browse files Browse the repository at this point in the history
  • Loading branch information
AlexanderFabisch committed Nov 17, 2021
1 parent aaa1d8c commit 5ac2866
Showing 1 changed file with 19 additions and 0 deletions.
19 changes: 19 additions & 0 deletions test/test_dmp_with_final_velocity.py
Original file line number Diff line number Diff line change
Expand Up @@ -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])

0 comments on commit 5ac2866

Please sign in to comment.