DobotModel.py 文件源码

def test():
    s2 = np.sqrt(2)
    check = npt.assert_array_almost_equal

    # test forward kinematics
    check(forward_kinematics((0,0,0)),[l2+d,0,l1])
    check(forward_kinematics((0,90,0)),[l1+l2+d,0,0])
    check(forward_kinematics((0,45,-45)), np.array([l1+l2,0,l1+l2])/np.sqrt(2) + [d,0,0])
    check(forward_kinematics((0,0,90)),[d,0,l1-l2])
    check(forward_kinematics((0,0,45)), [d+l2/np.sqrt(2),0,l1-l2/np.sqrt(2)])
    check(forward_kinematics((90,90,0)),[0,l1+l2+d,0])
    check(forward_kinematics((45,90,0)), np.array([l1+l2+d,l1+l2+d,0])/np.sqrt(2))

    # test inverse kinematics
    check(inverse_kinematics(forward_kinematics((0,0.01,0.01))),(0,0.01,0.01))
    check(inverse_kinematics(forward_kinematics((0,22.5,0.01))),(0,22.5,0.01))
    check(inverse_kinematics(forward_kinematics((0,45,45))),(0,45,45))
    check(inverse_kinematics(forward_kinematics((0,0.01,45))),(0,0.01,45))
    check(inverse_kinematics(forward_kinematics((0,0.01,22.5))),(0,0.01,22.5))
    check(inverse_kinematics(forward_kinematics((90,45,22.5))),(90,45,22.5))
    check(inverse_kinematics(forward_kinematics((45,45,22.5))),(45,45,22.5))

    # test jacobian
    check(jacobian((0,0,0)), np.matrix([[0,l1,0],[l2+d,0,0],[0,0,-l2]]))
    check(jacobian((90,90,0)), np.matrix([[-l1-l2-d,0,0],[0,0,0],[0,-l1,-l2]]))
    check(jacobian((0,45,45)), np.matrix([[0,l1/s2,-l2/s2],[(l1+l2)/s2+d,0,0],[0,-l1/s2,-l2/s2]]))