8000 end added #243 · olamarre/robotics-toolbox-python@fe6fc53 · GitHub
[go: up one dir, main page]
Skip to content

Commit fe6fc53

Browse files
jhavljhavl
committed
end added petercorke#243 
1 parent 0a6e425 commit fe6fc53

File tree

1 file changed

+25
-9
lines changed
  • roboticstoolbox/robot

1 file changed

+25
-9
lines changed

‎roboticstoolbox/robot/IK.py

Lines changed: 25 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -118,6 +118,7 @@ def ikine_LM(
118118
search=False,
119119
slimit=100,
120120
transpose=None,
121+
end=None,
121122
):
122123
"""
123124
Numerical inverse kinematics by Levenberg-Marquadt optimization
@@ -248,6 +249,9 @@ def ikine_LM(
248249
if not isinstance(T, SE3):
249250
raise TypeError("argument must be SE3")
250251

252+
if isinstance(self, rtb.DHRobot):
253+
end = None
254+
251255
solutions = []
252256

253257
if search:
@@ -323,14 +327,14 @@ def ikine_LM(
323327
failure = f"iteration limit {ilimit} exceeded"
324328
break
325329

326-
e = base.tr2delta(self.fkine(q).A, Tk.A)
330+
e = base.tr2delta(self.fkine(q, end=end).A, Tk.A)
327331

328332
# Are we there yet?
329333
if base.norm(W @ e) < tol:
330334
break
331335

332336
# Compute the Jacobian
333-
J = self.jacobe(q)
337+
J = self.jacobe(q, end=end)
334338

335339
JtJ = J.T @ W @ J
336340

@@ -356,7 +360,7 @@ def ikine_LM(
356360
qnew = q + dq
357361

358362
# And figure out the new error
359-
enew = base.tr2delta(self.fkine(qnew).A, Tk.A)
363+
enew = base.tr2delta(self.fkine(qnew, end=end).A, Tk.A)
360364

361365
# Was it a good update?
362366
if np.linalg.norm(W @ enew) < np.linalg.norm(W @ e):
@@ -404,7 +408,9 @@ def ikine_LM(
404408

405409
# --------------------------------------------------------------------- #
406410

407-
def ikine_LMS(self, T, q0=None, mask=None, ilimit=500, tol=1e-10, wN=1e-3, Lmin=0):
411+
def ikine_LMS(
412+
self, T, q0=None, mask=None, ilimit=500, tol=1e-10, wN=1e-3, Lmin=0, end=None
413+
):
408414
"""
409415
Numerical inverse kinematics by Levenberg-Marquadt optimization
410416
(Robot superclass)
@@ -512,6 +518,9 @@ def ikine_LMS(self, T, q0=None, mask=None, ilimit=500, tol=1e-10, wN=1e-3, Lmin=
512518
if not isinstance(T, SE3):
513519
raise TypeError("argument must be SE3")
514520

521+
if isinstance(self, rtb.DHRobot):
522+
end = None
523+
515524
solutions = []
516525

517526
if q0 is None:
@@ -545,15 +554,15 @@ def ikine_LMS(self, T, q0=None, mask=None, ilimit=500, tol=1e-10, wN=1e-3, Lmin=
545554
failure = f"iteration limit {ilimit} exceeded"
546555
break
547556

548-
e = _angle_axis(self.fkine(q).A, Tk.A)
557+
e = _angle_axis(self.fkine(q, end=end).A, Tk.A)
549558

550559
# Are we there yet?
551560
E = 0.5 * e.T @ W @ e
552561
if E < tol:
553562
break
554563

555564
# Compute the Jacobian and projection matrices
556-
J = self.jacob0(q)
565+
J = self.jacob0(q, end=end)
557566
WN = E * np.eye(self.n) + wN * np.eye(self.n)
558567
H = J.T @ W @ J + WN # n x n
559568
g = J.T @ W @ e # n x 1
@@ -607,6 +616,7 @@ def ikine_min(
607616
stiffness=0,
608617
costfun=None,
609618
options={},
619+
end=None,
610620
):
611621
r"""
612622
Inverse kinematics by optimization with joint limits (Robot superclass)
@@ -715,6 +725,9 @@ def ikine_min(
715725
if not isinstance(T, SE3):
716726
raise TypeError("argument must be SE3")
717727

728+
if isinstance(self, rtb.DHRobot):
729+
end = None
730+
718731
if q0 is None:
719732
q0 = np.zeros((self.n))
720733
else:
@@ -745,7 +758,7 @@ def ikine_min(
745758

746759
def cost(q, T, weight, costfun, stiffness):
747760
# T, weight, costfun, stiffness = args
748-
e = _angle_axis(self.fkine(q).A, T) * weight
761+
e = _angle_axis(self.fkine(q, end=end).A, T) * weight
749762
E = (e ** 2).sum()
750763

751764
if stiffness > 0:
@@ -785,7 +798,7 @@ def cost(q, T, weight, costfun, stiffness):
785798
# --------------------------------------------------------------------- #
786799

787800
def ikine_global(
788-
self, T, qlim=False, ilimit=1000, tol=1e-16, method=None, options={}
801+
self, T, qlim=False, ilimit=1000, tol=1e-16, method=None, options={}, end=None
789802
):
790803
r"""
791804
.. warning:: Experimental code for using SciPy global optimizers.
@@ -804,6 +817,9 @@ def ikine_global(
804817
if not isinstance(T, SE3):
805818
raise TypeError("argument must be SE3")
806819

820+
if isinstance(self, rtb.DHRobot):
821+
end = None
822+
807823
solutions = []
808824

809825
# wr = 1 / self.reach
@@ -833,7 +849,7 @@ def ikine_global(
833849

834850
def cost(q, T, weight):
835851
# T, weight, costfun, stiffness = args
836-
e = _angle_axis(self.fkine(q).A, T) * weight
852+
e = _angle_axis(self.fkine(q, end=end).A, T) * weight
837853
return (e ** 2).sum()
838854

839855
for _ in T:

0 commit comments

Comments
 (0)
0