Skip to content

Commit 1e7b498

Browse files
jhavljhavl
committed
started on ik examples
1 parent fe9e95d commit 1e7b498

File tree

2 files changed

+412
-0
lines changed

2 files changed

+412
-0
lines changed

‎roboticstoolbox/examples/ik_exp.py

Lines changed: 276 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,276 @@
1+
import numpy as np
2+
import roboticstoolbox as rtb
3+
import spatialmath as sm
4+
import fknm
5+
import time
6+
import swift
7+
import spatialgeometry as sg
8+
import sys
9+
from ansitable import ANSITable
10+
11+
from numpy import ndarray
12+
from spatialmath import SE3
13+
from typing import Union, overload, List, Set
14+
15+
# Our robot and ETS
16+
robot = rtb.models.Panda()
17+
ets = robot.ets()
18+
19+
### Experiment parameters
20+
# Number of problems to solve
21+
problems = 10000
22+
23+
# Cartesion DoF priority matrix
24+
we = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
25+
26+
# random valid q values which will define Tep
27+
q_rand = ets.random_q(problems)
28+
29+
# Our desired end-effector poses
30+
Tep = np.zeros((problems, 4, 4))
31+
32+
for i in range(problems):
33+
Tep[i] = ets.eval(q_rand[i])
34+
35+
# Maximum iterations allowed in a search
36+
ilimit = 30
37+
38+
# Maximum searches allowed per problem
39+
slimit = 100
40+
41+
# Solution tolerance
42+
tol = 1e-6
43+
44+
# Reject solutions with invalid joint limits
45+
reject_jl = True
46+
47+
48+
class IK:
49+
def __init__(self, name, solve, problems=problems):
50+
51+
# Solver attributes
52+
self.name = name
53+
54+
self.solve = solve
55+
56+
# Solver results
57+
self.iterations = np.zeros(problems)
58+
self.searches = np.zeros(problems)
59+
self.residual = np.zeros(problems)
60+
self.success = np.zeros(problems)
61+
62+
self.total_iterations = 0
63+
self.total_searches = 0
64+
65+
66+
solvers = [
67+
# IK(
68+
# "Newton Raphson",
69+
# lambda Tep: ets.ik_nr(
70+
# Tep,
71+
# q0=None,
72+
# ilimit=ilimit,
73+
# slimit=slimit,
74+
# tol=tol,
75+
# reject_jl=reject_jl,
76+
# we=we,
77+
# use_pinv=False,
78+
# pinv_damping=0.0,
79+
# ),
80+
# ),
81+
# IK(
82+
# "Gauss Newton",
83+
# lambda Tep: ets.ik_gn(
84+
# Tep,
85+
# q0=None,
86+
# ilimit=ilimit,
87+
# slimit=slimit,
88+
# tol=tol,
89+
# reject_jl=reject_jl,
90+
# we=we,
91+
# use_pinv=False,
92+
# pinv_damping=0.0,
93+
# ),
94+
# ),
95+
IK(
96+
"Newton Raphson Pinv",
97+
lambda Tep: ets.ik_nr(
98+
Tep,
99+
q0=None,
100+
ilimit=ilimit,
101+
slimit=slimit,
102+
tol=tol,
103+
reject_jl=reject_jl,
104+
we=we,
105+
use_pinv=True,
106+
pinv_damping=0.0,
107+
),
108+
),
109+
IK(
110+
"Gauss Newton Pinv",
111+
lambda Tep: ets.ik_gn(
112+
Tep,
113+
q0=None,
114+
ilimit=ilimit,
115+
slimit=slimit,
116+
tol=tol,
117+
reject_jl=reject_jl,
118+
we=we,
119+
use_pinv=True,
120+
pinv_damping=0.0,
121+
),
122+
),
123+
IK(
124+
"LM Chan 0.1",
125+
lambda Tep: ets.ik_lm_chan(
126+
Tep,
127+
q0=None,
128+
ilimit=ilimit,
129+
slimit=slimit,
130+
tol=tol,
131+
reject_jl=reject_jl,
132+
we=we,
133+
λ=0.1,
134+
),
135+
),
136+
# IK(
137+
# "LM Chan 1.0",
138+
# lambda Tep: ets.ik_lm_chan(
139+
# Tep,
140+
# q0=None,
141+
# ilimit=ilimit,
142+
# slimit=slimit,
143+
# tol=tol,
144+
# reject_jl=reject_jl,
145+
# we=we,
146+
# λ=1.0,
147+
# ),
148+
# ),
149+
# IK(
150+
# "LM Wampler",
151+
# lambda Tep: ets.ik_lm_wampler(
152+
# Tep,
153+
# q0=None,
154+
# ilimit=ilimit,
155+
# slimit=slimit,
156+
# tol=tol,
157+
# reject_jl=reject_jl,
158+
# we=we,
159+
# λ=1e-2,
160+
# ),
161+
# ),
162+
IK(
163+
"LM Wampler 1e-4",
164+
lambda Tep: ets.ik_lm_wampler(
165+
Tep,
166+
q0=None,
167+
ilimit=ilimit,
168+
slimit=slimit,
169+
tol=tol,
170+
reject_jl=reject_jl,
171+
we=we,
172+
λ=1e-4,
173+
),
174+
),
175+
# IK(
176+
# "LM Wampler 1e-6",
177+
# lambda Tep: ets.ik_lm_wampler(
178+
# Tep,
179+
# q0=None,
180+
# ilimit=ilimit,
181+
# slimit=slimit,
182+
# tol=tol,
183+
# reject_jl=reject_jl,
184+
# we=we,
185+
# λ=1e-6,
186+
# ),
187+
# ),
188+
# IK(
189+
# "LM Sugihara 0.001",
190+
# lambda Tep: ets.ik_lm_sugihara(
191+
# Tep,
192+
# q0=None,
193+
# ilimit=ilimit,
194+
# slimit=slimit,
195+
# tol=tol,
196+
# reject_jl=reject_jl,
197+
# we=we,
198+
# λ=0.001,
199+
# ),
200+
# ),
201+
# IK(
202+
# "LM Sugihara 0.01",
203+
# lambda Tep: ets.ik_lm_sugihara(
204+
# Tep,
205+
# q0=None,
206+
# ilimit=ilimit,
207+
# slimit=slimit,
208+
# tol=tol,
209+
# reject_jl=reject_jl,
210+
# we=we,
211+
# λ=0.01,
212+
# ),
213+
# ),
214+
IK(
215+
"LM Sugihara 0.1",
216+
lambda Tep: ets.ik_lm_sugihara(
217+
Tep,
218+
q0=None,
219+
ilimit=ilimit,
220+
slimit=slimit,
221+
tol=tol,
222+
reject_jl=reject_jl,
223+
we=we,
224+
λ=0.1,
225+
),
226+
),
227+
]
228+
229+
for i in range(problems):
230+
print(i + 1)
231+
232+
for solver in solvers:
233+
_, success, iterations, searches, residual = solver.solve(Tep[i])
234+
235+
if success:
236+
solver.success[i] = success
237+
solver.iterations[i] = iterations
238+
solver.searches[i] = searches
239+
solver.residual[i] = residual
240+
solver.total_iterations += solver.iterations[i]
241+
solver.total_searches += solver.searches[i]
242+
else:
243+
solver.success[i] = np.nan
244+
solver.iterations[i] = np.nan
245+
solver.searches[i] = np.nan
246+
solver.residual[i] = np.nan
247+
248+
249+
print(f"\nNumerical Inverse Kinematics Methods Compared over {problems} problems\n")
250+
251+
table = ANSITable(
252+
"Method",
253+
"sLimit/iLimit",
254+
"Mean Steps",
255+
"Median Steps",
256+
"Infeasible",
257+
"Infeasible %",
258+
"Mean Attempts",
259+
# "Median Attempts",
260+
"Max Attempts",
261+
border="thin",
262+
)
263+
264+
for solver in solvers:
265+
table.row(
266+
solver.name,
267+
f"{slimit}, {ilimit}",
268+
np.round(np.nanmean(solver.iterations), 2),
269+
np.nanmedian(solver.iterations),
270+
np.sum(np.isnan(solver.success)),
271+
np.round(np.sum(np.isnan(solver.success)) / problems * 100.0, 2),
272+
np.round(np.nanmean(solver.searches), 2),
273+
np.nanmax(solver.searches),
274+
)
275+
276+
table.print()

0 commit comments

Comments
 (0)