8000 add python ikine_LM · aforechi/robotics-toolbox-python@6d09752 · GitHub
[go: up one dir, main page]
Skip to content

Commit 6d09752

Browse files
jhavljhavl
committed
add python ikine_LM
1 parent f868da3 commit 6d09752

File tree

2 files changed

+71
-16
lines changed

2 files changed

+71
-16
lines changed

‎roboticstoolbox/robot/DHRobot.py

Lines changed: 28 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,8 @@
3535
from numpy import any
3636
from typing import Union, Tuple
3737

38+
ArrayLike = Union[list, np.ndarray, tuple, set]
39+
3840
iksol = namedtuple("IKsolution", "q, success, reason")
3941

4042

@@ -855,7 +857,7 @@ def twists(self, q=None):
855857

856858
return tw, T[-1]
857859

858-
def ets(self, *args, **kwargs):
860+
def ets(self, *args, **kwargs) -> ETS:
859861
"""
860862
Robot kinematics as an elemenary transform sequence
861863
@@ -2318,7 +2320,9 @@ def ik_nr(
23182320
TODO
23192321
"""
23202322

2321-
return self.ets().ik_nr(Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping)
2323+
return self.ets().ik_nr(
2324+
Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping
2325+
)
23222326

23232327
def ik_gn(
23242328
self,
@@ -2424,11 +2428,29 @@ def ik_gn(
24242428
TODO
24252429
"""
24262430

2427-
return self.ets().ik_gn(Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping)
2428-
2429-
2430-
2431+
return self.ets().ik_gn(
2432+
Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping
2433+
)
24312434

2435+
def ikine_LM(
2436+
self,
2437+
Tep: Union[np.ndarray, SE3],
2438+
q0: Union[ArrayLike, None] = None,
2439+
ilimit: int = 30,
2440+
slimit: int = 100,
2441+
tol: float = 1e-6,
2442+
joint_limits: bool = True,
2443+
mask: Union[ArrayLike, None] = None,
2444+
):
2445+
return self.ets().ikine_LM(
2446+
Tep=Tep,
2447+
q0=q0,
2448+
ilimit=ilimit,
2449+
slimit=slimit,
2450+
tol=tol,
2451+
joint_limits=joint_limits,
2452+
mask=mask,
2453+
)
24322454

24332455

24342456
class SerialLink(DHRobot):

‎roboticstoolbox/robot/ETS.py

Lines changed: 43 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
@author: Peter Corke
66
"""
77

8+
from abc import ABC, abstractmethod
89
from collections import UserList
910
from numpy import (
1011
pi,
@@ -37,6 +38,7 @@
3738
simplify,
3839
)
3940
from roboticstoolbox import rtb_get_param
41+
from roboticstoolbox.robot.IK import IK_LM, IK_NR
4042

4143
from collections import UserList
4244
from spatialmath.base import issymbol, getmatrix
@@ -47,11 +49,11 @@
4749
ETS_jacobe,
4850
ETS_hessian0,
4951
ETS_hessiane,
50-
IK_NR,
51-
IK_GN,
52-
IK_LM_Chan,
53-
IK_LM_Wampler,
54-
IK_LM_Sugihara,
52+
IK_NR_c,
53+
IK_GN_c,
54+
IK_LM_Chan_c,
55+
IK_LM_Wampler_c,
56+
IK_LM_Sugihara_c,
5557
)
5658
from copy import deepcopy
5759
from roboticstoolbox import rtb_get_param
@@ -73,6 +75,12 @@
7375
c_property = property
7476

7577

78+
# class AbstractETS(ABC):
79+
# @abstractmethod
80+
# def jacobm(self) -> ndarray:
81+
# pass
82+
83+
7684
class BaseETS(UserList):
7785
def __init__(self, *args):
7886
super().__init__(*args)
@@ -1774,7 +1782,7 @@ def ik_lm_chan(
17741782
TODO
17751783
"""
17761784

1777-
return IK_LM_Chan(self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ)
1785+
return IK_LM_Chan_c(self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ)
17781786

17791787
def ik_lm_wampler(
17801788
self,
@@ -1879,7 +1887,9 @@ def ik_lm_wampler(
18791887
TODO
18801888
"""
18811889

1882-
return IK_LM_Wampler(self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ)
1890+
return IK_LM_Wampler_c(
1891+
self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ
1892+
)
18831893

18841894
def ik_lm_sugihara(
18851895
self,
@@ -1984,7 +1994,7 @@ def ik_lm_sugihara(
19841994
TODO
19851995
"""
19861996

1987-
return IK_LM_Sugihara(
1997+
return IK_LM_Sugihara_c(
19881998
self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ
19891999
)
19902000

@@ -2092,7 +2102,7 @@ def ik_nr(
20922102
TODO
20932103
"""
20942104

2095-
return IK_NR(
2105+
return IK_NR_c(
20962106
self._fknm,
20972107
Tep,
20982108
q0,
@@ -2209,7 +2219,7 @@ def ik_gn(
22092219
TODO
22102220
"""
22112221

2212-
return IK_GN(
2222+
return IK_GN_c(
22132223
self._fknm,
22142224
Tep,
22152225
q0,
@@ -2222,6 +2232,29 @@ def ik_gn(
22222232
pinv_damping,
22232233
)
22242234

2235+
def ikine_LM(
2236+
self,
2237+
Tep: Union[ndarray, SE3],
2238+
q0: Union[ArrayLike, None] = None,
2239+
ilimit: int = 30,
2240+
slimit: int = 100,
2241+
tol: float = 1e-6,
2242+
joint_limits: bool = True,
2243+
mask: Union[ArrayLike, None] = None,
2244+
):
2245+
solver = IK_NR(
2246+
ilimit=ilimit,
2247+
slimit=slimit,
2248+
tol=tol,
2249+
joint_limits=joint_limits,
2250+
mask=mask,
2251+
)
2252+
2253+
if isinstance(Tep, SE3):
2254+
Tep = Tep.A
2255+
2256+
return solver.solve(ets=self, Tep=Tep, q0=q0)
2257+
22252258

22262259
class ETS2(BaseETS):
22272260
"""

0 commit comments

Comments
 (0)
0