8000 Add traj option tom Python Ik solvers · pbdahzou/robotics-toolbox-python@66cf821 · GitHub
[go: up one dir, main page]
Skip to content

Commit 66cf821

Browse files
jhavljhavl
committed
Add traj option tom Python Ik solvers
1 parent 779080b commit 66cf821

File tree

2 files changed

+60
-10
lines changed

2 files changed

+60
-10
lines changed

‎roboticstoolbox/robot/ETS.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -2578,8 +2578,8 @@ def ikine_LM(
25782578
**kwargs,
25792579
)
25802580

2581-
if isinstance(Tep, SE3):
2582-
Tep = Tep.A
2581+
# if isinstance(Tep, SE3):
2582+
# Tep = Tep.A
25832583

25842584
return solver.solve(ets=self, Tep=Tep, q0=q0)
25852585

@@ -2717,8 +2717,8 @@ def ikine_NR(
27172717
**kwargs,
27182718
)
27192719

2720-
if isinstance(Tep, SE3):
2721-
Tep = Tep.A
2720+
# if isinstance(Tep, SE3):
2721+
# Tep = Tep.A
27222722

27232723
return solver.solve(ets=self, Tep=Tep, q0=q0)
27242724

@@ -2871,8 +2871,8 @@ def ikine_GN(
28712871
**kwargs,
28722872
)
28732873

2874-
if isinstance(Tep, SE3):
2875-
Tep = Tep.A
2874+
# if isinstance(Tep, SE3):
2875+
# Tep = Tep.A
28762876

28772877
return solver.solve(ets=self, Tep=Tep, q0=q0)
28782878

@@ -3068,8 +3068,8 @@ def ikine_QP(
30683068
**kwargs,
30693069
)
30703070

3071-
if isinstance(Tep, SE3):
3072-
Tep = Tep.A
3071+
# if isinstance(Tep, SE3):
3072+
# Tep = Tep.A
30733073

30743074
return solver.solve(ets=self, Tep=Tep, q0=q0)
30753075

‎roboticstoolbox/robot/IK.py

Lines changed: 52 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -238,12 +238,62 @@ def solve(
238238

239239
q0 = q0_method
240240

241+
traj = False
242+
243+
methTep: np.ndarray
244+
241245
if isinstance(Tep, SE3):
242-
Tep: np.ndarray = Tep.A
246+
if len(Tep) > 1:
247+
traj = True
248+
methTep = np.empty((len(Tep), 4, 4))
243249

244-
if Tep.shape != (4, 4):
250+
for i, T in enumerate(Tep):
251+
methTep[i] = T.A
252+
else:
253+
methTep = Tep.A
254+
elif Tep.ndim == 3:
255+
traj = True
256+
methTep = Tep
257+
elif Tep.shape != (4, 4):
245258
raise ValueError("Tep must be a 4x4 SE3 matrix")
259+
else:
260+
methTep = Tep
261+
262+
if traj:
263+
q = np.empty((methTep.shape[0], ets.n))
264+
success = True
265+
interations = 0
266+
searches = 0
267+
residual = np.inf
268+
reason = ""
269+
270+
for i, T in enumerate(methTep):
271+
sol = self._solve(ets, T, q0)
272+
q[i] = sol.q
273+
if not sol.success:
274+
success = False
275+
reason = sol.reason
276+
interations += sol.iterations
277+
searches += sol.searches
278+
279+
if sol.residual < residual:
280+
residual = sol.residual
281+
282+
return IKSolution(
283+
q=q,
284+
success=success,
285+
iterations=interations,
286+
searches=searches,
287+
residual=residual,
288+
reason=reason,
289+
)
290+
291+
else:
292+
sol = self._solve(ets, methTep, q0)
293+
294+
return sol
246295

296+
def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution:
247297
# Iteration count
248298
i = 0
249299
total_i = 0

0 commit comments

Comments
 (0)
0