test_analytical_ik.py
Go to the documentation of this file.
00001 import numpy as np
00002 import sys
00003 import roslib
00004 roslib.load_manifest("ur_kinematics")
00005 from ur_kin_py import forward, inverse
00006 
00007 def best_sol(sols, q_guess, weights):
00008     valid_sols = []
00009     for sol in sols:
00010         test_sol = np.ones(6)*9999.
00011         for i in range(6):
00012             for add_ang in [-2.*np.pi, 0, 2.*np.pi]:
00013                 test_ang = sol[i] + add_ang
00014                 if (abs(test_ang) <= 2.*np.pi and 
00015                     abs(test_ang - q_guess[i]) < abs(test_sol[i] - q_guess[i])):
00016                     test_sol[i] = test_ang
00017         if np.all(test_sol != 9999.):
00018             valid_sols.append(test_sol)
00019     if len(valid_sols) == 0:
00020         return None
00021     best_sol_ind = np.argmin(np.sum((weights*(valid_sols - np.array(q_guess)))**2,1))
00022     return valid_sols[best_sol_ind]
00023 
00024 def test_q(q):
00025     x = forward(q)
00026     sols = inverse(np.array(x), float(q[5]))
00027 
00028     qsol = best_sol(sols, q, [1.]*6)
00029     if qsol is None:
00030         qsol = [999.]*6
00031     diff = np.sum(np.abs(np.array(qsol) - q))
00032     if diff > 0.001:
00033         print np.array(sols)
00034         print 'Best q:', qsol
00035         print 'Actual:', np.array(q)
00036         print 'Diff:  ', q - qsol
00037         print 'Difdiv:', (q - qsol)/np.pi
00038         print i1-3, i2-3, i3-3, i4-3, i5-3, i6-3
00039         if raw_input() == 'q':
00040             sys.exit()
00041 
00042 def main():
00043     np.set_printoptions(precision=3)
00044     print "Testing multiples of pi/2..."
00045     for i1 in range(0,5):
00046         for i2 in range(0,5):
00047             print i1, i2
00048             for i3 in range(0,5):
00049                 for i4 in range(0,5):
00050                     for i5 in range(0,5):
00051                         for i6 in range(0,5):
00052                             q = np.array([i1*np.pi/2., i2*np.pi/2., i3*np.pi/2., 
00053                                           i4*np.pi/2., i5*np.pi/2., i6*np.pi/2.])
00054                             test_q(q)
00055     print "Testing random configurations..."
00056     for i in range(10000):
00057         q = (np.random.rand(6)-.5)*4*np.pi
00058         test_q(q)
00059     print "Done!"
00060 
00061 if __name__ == "__main__":
00062     if False:
00063         import cProfile
00064         cProfile.run('main()', 'ik_prof')
00065     else:
00066         main()


ur_kinematics
Author(s): Kelsey Hawkins
autogenerated on Fri Aug 28 2015 13:31:18