def test_fk(self): links = [1,1] m = Model(links) # test joint 1-1 X = m.forward_kinematic([0,0]) np.testing.assert_almost_equal(X,[2,0,0]) # test joint 1-2 X = m.forward_kinematic([math.pi/2.,0]) np.testing.assert_almost_equal(X,[0,2,0]) # test joint 1-3 X = m.forward_kinematic([-math.pi/2.,0]) np.testing.assert_almost_equal(X,[0,-2,0]) #test joint 2-1 X = m.forward_kinematic([0,math.pi/2.]) np.testing.assert_almost_equal(X,[1,1,0]) #test joint 2-1 X = m.forward_kinematic([0,-math.pi/2.]) np.testing.assert_almost_equal(X,[1,-1,0]) #test coupled 1 X = m.forward_kinematic([math.pi/2.,math.pi/2.]) np.testing.assert_almost_equal(X,[-1,1,0]) #test coupled 2 X = m.forward_kinematic([-math.pi/2.,math.pi/2.]) np.testing.assert_almost_equal(X,[1,-1,0]) #test coupled 3 X = m.forward_kinematic([math.pi/2.,-math.pi/2.]) np.testing.assert_almost_equal(X,[1,1,0]) #test coupled 4 X = m.forward_kinematic([-math.pi/2.,-math.pi/2.]) np.testing.assert_almost_equal(X,[-1,-1,0])
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_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)
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)