From 7f44095a01d3c6eee979926049a8ce6f2c26d358 Mon Sep 17 00:00:00 2001 From: iory Date: Sun, 18 Mar 2018 13:31:35 +0900 Subject: [PATCH] [test] Add interpolator test --- tests/robot_tests/test_interpolator.py | 62 ++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 tests/robot_tests/test_interpolator.py diff --git a/tests/robot_tests/test_interpolator.py b/tests/robot_tests/test_interpolator.py new file mode 100644 index 00000000..7eed237c --- /dev/null +++ b/tests/robot_tests/test_interpolator.py @@ -0,0 +1,62 @@ +import unittest + +from numpy import pi +from numpy import testing +import numpy as np + +from robot.interpolator import LinearInterpolator +from robot.interpolator import MinjerkInterpolator +from robot.math import midpoint + + +class TestInterpolator(unittest.TestCase): + + def test_linear_interpolator(self): + ip = LinearInterpolator() + p0 = np.array([1, 2, 3]) + t0 = 0.10 + p1 = np.array([3, 4, 5]) + t1 = 0.18 + ip.reset(position_list=[p0, p1, p0], + time_list=[t0, t1]) + ip.start_interpolation() + + i = 0 + while t0 > i: + ip.pass_time(0.02) + testing.assert_almost_equal(ip.position, midpoint(i / t0, p0, p1)) + i += 0.02 + + i = t0 + while t1 > i: + ip.pass_time(0.02) + testing.assert_almost_equal( + ip.position, + midpoint((i - t0) / (t1 - t0), p1, p0)) + i += 0.02 + + assert(ip.is_interpolating is False) + + def test_minjerk_interpolator(self): + ip = MinjerkInterpolator() + p0 = np.array([1, 2, 3]) + t0 = 0.10 + p1 = np.array([3, 4, 5]) + t1 = 0.18 + ip.reset(position_list=[p0, p1, p0], + time_list=[t0, t1]) + ip.start_interpolation() + + i = 0 + while t0 >= i: + ip.pass_time(0.02) + i += 0.02 + testing.assert_almost_equal(ip.position, p1) + + i = t0 + while t1 >= i: + ip.pass_time(0.02) + i += 0.02 + testing.assert_almost_equal(ip.position, p0) + + assert(ip.is_interpolating is False)