def test_integrate(self):
fk = Forward_kinematics('Duty_fi_linear_no_constant',
'Duty_fi_linear_no_constant',
np.matrix([-1, 1]), np.matrix([1, 1]))
theta_dot = np.pi / 2
v = np.pi / 2
dt = 1
theta_res, x_res, y_res = fk.integrate(theta_dot, v, dt)
self.assertAlmostEqual(theta_res, np.pi / 2)
self.assertAlmostEqual(x_res, 1)
self.assertAlmostEqual(y_res, 1)
