Skip to content

Commit 610a7b5

Browse files
committed
SCript to evaluate IK performance
1 parent 1914859 commit 610a7b5

File tree

1 file changed

+75
-0
lines changed

1 file changed

+75
-0
lines changed
Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
import numpy as np
2+
from spatialmath import SE3
3+
import roboticstoolbox as rtb
4+
import timeit
5+
from ansitable import ANSITable, Column
6+
7+
# change for the robot IK under test, must set:
8+
# * robot, the DHRobot object
9+
# * T, the end-effector pose
10+
# * q0, the initial joint angles for solution
11+
12+
# Puma robot case
13+
# robot = rtb.models.DH.Puma560()
14+
# q = robot.qn
15+
# q0 = robot.qz
16+
# T = robot.fkine(q)
17+
18+
# Panda robot case
19+
robot = rtb.models.DH.Panda()
20+
q0 = robot.qr
21+
T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
22+
# this pose does quite badly, it's from the RTB README.md
23+
24+
# build the list of IK methods to test
25+
ikfuncs = [
26+
robot.ikine_LM, # Levenberg-Marquadt
27+
robot.ikine_LMS, # Levenberg-Marquadt (Sugihara)
28+
robot.ikine_unc, #numerical solution with no constraints
29+
robot.ikine_con, # numerical solution with constraints
30+
robot.ikine_min # numerical solution 2 with constraints
31+
]
32+
if "ikine_a" in robot:
33+
ikfuncs.append(robot.ikine_a) # analytic solution
34+
35+
# setup to run timeit
36+
setup = '''
37+
from __main__ import robot, T, q0
38+
'''
39+
N = 10
40+
41+
# setup results table
42+
table = ANSITable(
43+
Column("Operation", headalign="^"),
44+
Column("Time (μs)", headalign="^", fmt="{:.2f}"),
45+
Column("Error", headalign="^", fmt="{:.3g}"),
46+
border="thick")
47+
48+
# test the IK methods
49+
for ik in ikfuncs:
50+
print('Testing:', ik.__name__)
51+
52+
# test the method, don't pass q0 to the analytic function
53+
if ik.__name__ == "ikine_a":
54+
sol = ik(T)
55+
statement = f"sol = robot.{ik.__name__}(T)"
56+
else:
57+
sol = ik(T, q0=q0)
58+
statement = f"sol = robot.{ik.__name__}(T, q0=q0)"
59+
60+
# print error message if there is one
61+
if not sol.success:
62+
print(' failed:', sol.reason)
63+
64+
# evalute the error
65+
err = np.linalg.norm(T - robot.fkine(sol.q))
66+
print(' error', err)
67+
68+
# evaluate the execution time
69+
t = timeit.timeit(stmt=statement, setup=setup, number=N)
70+
71+
# add it to the output table
72+
table.row(ik.__name__, t/N*1e6, err)
73+
74+
# pretty print the results
75+
table.print()

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy