def test_zero_theta_dot(self):
fk = Forward_kinematics('Duty_fi_linear_no_constant',
'Duty_fi_linear_no_constant',
np.matrix([-1, 1]), np.matrix([1, 1]))
theta, x, y = 0, 0, 0
theta_dot = 0
v = 1
dt = 1
theta_res, x_res, y_res = fk.integrate_propagate(
theta, x, y, theta_dot, v, dt)
self.assertAlmostEqual(theta_res, 0)
self.assertAlmostEqual(x_res, 1)
self.assertAlmostEqual(y_res, 0)
