def test_with_naive_fi(self):
fk = Forward_kinematics('Duty_fi_theta_dot_naive', 'Duty_fi_v_naive',
np.matrix([-1]), np.matrix([1]))
theta_dot, v = fk.evaluate(np.matrix([0]), np.matrix([0]))
self.assertAlmostEqual(theta_dot, 0)
self.assertAlmostEqual(v, 0)
theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([1]))
self.assertAlmostEqual(theta_dot, 0)
self.assertAlmostEqual(v, 2)
theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([-1]))
self.assertAlmostEqual(theta_dot, -2)
self.assertAlmostEqual(v, 0)
theta_dot, v = fk.evaluate(np.matrix([-1]), np.matrix([1]))
self.assertAlmostEqual(theta_dot, 2)
self.assertAlmostEqual(v, 0)
def test_with_linear_fi(self):
fk = Forward_kinematics('Duty_fi_linear_no_constant',
'Duty_fi_linear_no_constant',
np.matrix([-1, 1]), np.matrix([1, 1]))
theta_dot, v = fk.evaluate(np.matrix([0]), np.matrix([0]))
self.assertAlmostEqual(theta_dot, 0)
self.assertAlmostEqual(v, 0)
theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([1]))
self.assertAlmostEqual(theta_dot, 0)
self.assertAlmostEqual(v, 2)
theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([-1]))
self.assertAlmostEqual(theta_dot, -2)
self.assertAlmostEqual(v, 0)
theta_dot, v = fk.evaluate(np.matrix([-1]), np.matrix([1]))
self.assertAlmostEqual(theta_dot, 2)
self.assertAlmostEqual(v, 0)
theta_dot, v = fk.evaluate(np.matrix([0]), np.matrix([2]))
self.assertAlmostEqual(theta_dot, 2)
self.assertAlmostEqual(v, 2)
