00001
00002
00003 from trac_ik_python.trac_ik import IK
00004 from numpy.random import random
00005 import time
00006
00007
00008 if __name__ == '__main__':
00009
00010
00011
00012 ik_solver = IK("torso_lift_link",
00013 "r_wrist_roll_link")
00014
00015 print("IK solver uses link chain:")
00016 print(ik_solver.link_names)
00017
00018 print("IK solver base frame:")
00019 print(ik_solver.base_link)
00020
00021 print("IK solver tip link:")
00022 print(ik_solver.tip_link)
00023
00024 print("IK solver for joints:")
00025 print(ik_solver.joint_names)
00026
00027 print("IK solver using joint limits:")
00028 lb, up = ik_solver.get_joint_limits()
00029 print("Lower bound: " + str(lb))
00030 print("Upper bound: " + str(up))
00031
00032 qinit = [0.] * ik_solver.number_of_joints
00033 x = y = z = 0.0
00034 rx = ry = rz = 0.0
00035 rw = 1.0
00036 bx = by = bz = 0.001
00037 brx = bry = brz = 0.1
00038
00039
00040 NUM_COORDS = 200
00041 rand_coords = []
00042 for _ in range(NUM_COORDS):
00043 x = random() * 0.5
00044 y = random() * 0.6 + -0.3
00045 z = random() * 0.7 + -0.35
00046 rand_coords.append((x, y, z))
00047
00048
00049 avg_time = 0.0
00050 num_solutions_found = 0
00051 for x, y, z in rand_coords:
00052 ini_t = time.time()
00053 sol = ik_solver.get_ik(qinit,
00054 x, y, z,
00055 rx, ry, rz, rw,
00056 bx, by, bz,
00057 brx, bry, brz)
00058 fin_t = time.time()
00059 call_time = fin_t - ini_t
00060
00061 avg_time += call_time
00062 if sol:
00063
00064
00065 num_solutions_found += 1
00066 avg_time = avg_time / NUM_COORDS
00067
00068 print
00069 print "Found " + str(num_solutions_found) + " of 200 random coords"
00070 print "Average IK call time: " + str(avg_time)
00071 print
00072
00073
00074 avg_time = 0.0
00075 num_solutions_found = 0
00076 brx = bry = brz = 9999.0
00077 for x, y, z in rand_coords:
00078 ini_t = time.time()
00079 sol = ik_solver.get_ik(qinit,
00080 x, y, z,
00081 rx, ry, rz, rw,
00082 bx, by, bz,
00083 brx, bry, brz)
00084 fin_t = time.time()
00085 call_time = fin_t - ini_t
00086
00087 avg_time += call_time
00088 if sol:
00089
00090
00091 num_solutions_found += 1
00092
00093 avg_time = avg_time / NUM_COORDS
00094 print
00095 print "Found " + str(num_solutions_found) + " of 200 random coords ignoring orientation"
00096 print "Average IK call time: " + str(avg_time)
00097 print
00098
00099
00100 avg_time = 0.0
00101 num_solutions_found = 0
00102 bx = by = bz = 9999.0
00103 brx = bry = brz = 9999.0
00104 for x, y, z in rand_coords:
00105 ini_t = time.time()
00106 sol = ik_solver.get_ik(qinit,
00107 x, y, z,
00108 rx, ry, rz, rw,
00109 bx, by, bz,
00110 brx, bry, brz)
00111 fin_t = time.time()
00112 call_time = fin_t - ini_t
00113
00114 avg_time += call_time
00115 if sol:
00116
00117
00118 num_solutions_found += 1
00119
00120 avg_time = avg_time / NUM_COORDS
00121
00122 print
00123 print "Found " + str(num_solutions_found) + " of 200 random coords ignoring everything"
00124 print "Average IK call time: " + str(avg_time)
00125 print
00126
00127
00128
00129
00130
00131
00132
00133
00134