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


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