test_pkg.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from trac_ik_python.trac_ik import IK
4 from numpy.random import random
5 import time
6 
7 
8 if __name__ == '__main__':
9  # roslaunch pr2_description upload_pr2.launch
10  # Needed beforehand
11 
12  ik_solver = IK("torso_lift_link",
13  "r_wrist_roll_link")
14 
15  print("IK solver uses link chain:")
16  print(ik_solver.link_names)
17 
18  print("IK solver base frame:")
19  print(ik_solver.base_link)
20 
21  print("IK solver tip link:")
22  print(ik_solver.tip_link)
23 
24  print("IK solver for joints:")
25  print(ik_solver.joint_names)
26 
27  print("IK solver using joint limits:")
28  lb, up = ik_solver.get_joint_limits()
29  print("Lower bound: " + str(lb))
30  print("Upper bound: " + str(up))
31 
32  qinit = [0.] * ik_solver.number_of_joints
33  x = y = z = 0.0
34  rx = ry = rz = 0.0
35  rw = 1.0
36  bx = by = bz = 0.001
37  brx = bry = brz = 0.1
38 
39  # Generate a set of random coords in the arm workarea approx
40  NUM_COORDS = 200
41  rand_coords = []
42  for _ in range(NUM_COORDS):
43  x = random() * 0.5
44  y = random() * 0.6 + -0.3
45  z = random() * 0.7 + -0.35
46  rand_coords.append((x, y, z))
47 
48  # Check some random coords with fixed orientation
49  avg_time = 0.0
50  num_solutions_found = 0
51  for x, y, z in rand_coords:
52  ini_t = time.time()
53  sol = ik_solver.get_ik(qinit,
54  x, y, z,
55  rx, ry, rz, rw,
56  bx, by, bz,
57  brx, bry, brz)
58  fin_t = time.time()
59  call_time = fin_t - ini_t
60  # print "IK call took: " + str(call_time)
61  avg_time += call_time
62  if sol:
63  # print "X, Y, Z: " + str( (x, y, z) )
64  # print "SOL: " + str(sol)
65  num_solutions_found += 1
66  avg_time = avg_time / NUM_COORDS
67 
68  print
69  print "Found " + str(num_solutions_found) + " of 200 random coords"
70  print "Average IK call time: " + str(avg_time)
71  print
72 
73  # Check if orientation bounds work
74  avg_time = 0.0
75  num_solutions_found = 0
76  brx = bry = brz = 9999.0 # We don't care about orientation
77  for x, y, z in rand_coords:
78  ini_t = time.time()
79  sol = ik_solver.get_ik(qinit,
80  x, y, z,
81  rx, ry, rz, rw,
82  bx, by, bz,
83  brx, bry, brz)
84  fin_t = time.time()
85  call_time = fin_t - ini_t
86  # print "IK call took: " + str(call_time)
87  avg_time += call_time
88  if sol:
89  # print "X, Y, Z: " + str( (x, y, z) )
90  # print "SOL: " + str(sol)
91  num_solutions_found += 1
92 
93  avg_time = avg_time / NUM_COORDS
94  print
95  print "Found " + str(num_solutions_found) + " of 200 random coords ignoring orientation"
96  print "Average IK call time: " + str(avg_time)
97  print
98 
99  # Check if big position and orientation bounds work
100  avg_time = 0.0
101  num_solutions_found = 0
102  bx = by = bz = 9999.0
103  brx = bry = brz = 9999.0
104  for x, y, z in rand_coords:
105  ini_t = time.time()
106  sol = ik_solver.get_ik(qinit,
107  x, y, z,
108  rx, ry, rz, rw,
109  bx, by, bz,
110  brx, bry, brz)
111  fin_t = time.time()
112  call_time = fin_t - ini_t
113  # print "IK call took: " + str(call_time)
114  avg_time += call_time
115  if sol:
116  # print "X, Y, Z: " + str( (x, y, z) )
117  # print "SOL: " + str(sol)
118  num_solutions_found += 1
119 
120  avg_time = avg_time / NUM_COORDS
121 
122  print
123  print "Found " + str(num_solutions_found) + " of 200 random coords ignoring everything"
124  print "Average IK call time: " + str(avg_time)
125  print
126 
127 # std::vector<double> CartToJnt(const std::vector<double> q_init,
128 # const double x, const double y, const double z,
129 # const double rx, const double ry, const double rz, const double rw,
130 # // bounds x y z
131 # const double boundx=0.0, const double boundy=0.0, const double boundz=0.0,
132 # // bounds on rotation x y z
133 # const double boundrx=0.0, const double boundry=0.0, const double
134 # boundrz=0.0){


trac_ik_python
Author(s): Sam Pfeiffer
autogenerated on Tue Apr 23 2019 02:39:19