def test_ik(self):
links = [1,1,1]
m = Model(links)
for i in range(100):
# create end effector random pose
X = np.random.uniform(-1,1,2)
X = np.append(X,0)
# calculate inverse kinematic
q = m.inverse_kinematic(X)
# calculate forward kinematic from obtained q
X2 = m.forward_kinematic(q)
# assert equality between X and X2
np.testing.assert_almost_equal(X,X2,decimal=3)
def test_random_model(self):
for i in range(100):
nb_joints = np.random.randint(10)+3
links = np.random.uniform(1,5,nb_joints)
# create the model and check the ik
m = Model(links)
# create end effector random pose
X = np.random.uniform(-3,3,2)
X = np.append(X,0)
# calculate inverse kinematic
q = m.inverse_kinematic(X)
# calculate forward kinematic from obtained q
X2 = m.forward_kinematic(q)
# plot the model for testing
# m.plot_model(q,X)
# assert equality between X and X2
np.testing.assert_almost_equal(X,X2,decimal=3)
def test_base_transformation(self):
links = [1,1,1]
# shift the base by a random vector
rot = np.eye(3)
trans = np.random.uniform(-5,5,3)
T = tr.transformation(rot,trans)
# create the model
m = Model(links,rot,trans)
# check the forward kinematic 1
Xd = tr.transform_point([2,0,0],T)
X = m.forward_kinematic([0,0])
np.testing.assert_almost_equal(X,Xd)
# check the forward kinematic 2
Xd = tr.transform_point([1,-1,0],T)
X = m.forward_kinematic([0,-math.pi/2.])
np.testing.assert_almost_equal(X,Xd)
# check the inverse kinematic
X = np.random.uniform(-1,1,2)
X = np.append(X,0)
WX = tr.transform_point(X,T)
q = m.inverse_kinematic(WX)
X2 = m.forward_kinematic(q)
np.testing.assert_almost_equal(WX,X2,decimal=3)
