8000 Merge branch 'master' of github.com:petercorke/robotics-toolbox-python · ctc-eng/robotics-toolbox-python@3a95b35 · GitHub
[go: up one dir, main page]
Skip to content

Commit 3a95b35

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents 6575d48 + bc91f4e commit 3a95b35

File tree

3 files changed

+49
-34
lines changed

3 files changed

+49
-34
lines changed

‎roboticstoolbox/robot/ELink.py

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,9 +13,9 @@
1313

1414
class BaseELink(Link):
1515

16-
def __init__(self, name=None, parent=None, joint_name=None):
16+
def __init__(self, name=None, parent=None, joint_name=None, **kwargs):
1717

18-
super().__init__()
18+
super().__init__(kwargs)
1919

2020
self._name = name
2121

@@ -256,6 +256,7 @@ def ets(self):
256256
if self.v is None:
257257
return self._ets
258258
else:
259+
self.v.jindex = self.jindex
259260
return self._ets * self.v
260261

261262
@collision.setter

‎roboticstoolbox/robot/ERobot.py

Lines changed: 33 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -506,7 +506,7 @@ def ets(self, start=None, end=None, explored=None, path=None):
506506
>>> panda = rtb.models.ETS.Panda()
507507
>>> panda.ets()
508508
"""
509-
v = self._getlink(start, self.base_link)
509+
link = self._getlink(start, self.base_link)
510510
if end is None and len(self.ee_links) > 1:
511511
raise ValueError(
512512
'ambiguous, specify which end-effector is required')
@@ -516,34 +516,38 @@ def ets(self, start=None, end=None, explored=None, path=None):
516516
explored = set()
517517
toplevel = path is None
518518

519-
explored.add(v)
520-
if v == end:
519+
explored.add(link)
520+
if link == end:
521521
return path
522522

523523
# unlike regular DFS, the neighbours of the node are its children
524524
# and its parent.
525525

526-
# visit child nodes
526+
# visit child nodes below start
527527
if toplevel:
528-
path = v.ets()
529-
for w in v.children:
530-
if w not in explored:
531-
p = self.ets(w, end, explored, path * w.ets())
532-
if p:
528+
path = link.ets()
529+
for child in link.children:
530+
if child not in explored:
531+
p = self.ets(child, end, explored, path * child.ets())
532+
if p is not None:
533533
return p
534534

535-
# visit parent node
535+
# we didn't find the node below, keep going up a level, and recursing
536+
# down again
536537
if toplevel:
537-
path = ETS()
538-
if v.parent is not None:
539-
w = v.parent
540-
if w not in explored:
541-
p = self.ets(w, end, explored, path * v.ets().inv())
542-
if p:
538+
path = None
539+
if link.parent is not None:
540+
parent = link.parent # go up one level toward the root
541+
if parent not in explored:
542+
if path is None:
543+
p = self.ets(parent, end, explored, link.ets().inv())
544+
else:
545+
p = self.ets(parent, end, explored, path * link.ets().inv())
546+
if p is not None:
543547
return p
544-
545548
return None
546549

550+
547551
# --------------------------------------------------------------------- #
548552

549553
def showgraph(self, **kwargs):
@@ -745,8 +749,8 @@ def _get_limit_links(self, end=None, start=None):
745749
"""
746750

747751
# Try cache
748-
if self._cache_end is not None:
749-
return self._cache_end, self._cache_start, self._cache_end_tool
752+
# if self._cache_end is not None:
753+
# return self._cache_end, self._cache_start, self._cache_end_tool
750754

751755
tool = None
752756
if end is None:
@@ -900,8 +904,8 @@ def URDF(cls, file_path, gripper=None):
900904
Construct an ERobot object from URDF file
901905
:param file_path: [description]
902906
:type file_path: [type]
903-
:param gripper: index or name of the gripper link
904-
:type gripper: int or str
907+
:param gripper: index or name of the gripper link(s)
908+
:type gripper: int or str or list
905909
:return: [description]
906910
:rtype: [type]
907911
If ``gripper`` is specified, links from that link outward are removed
@@ -922,7 +926,9 @@ def URDF(cls, file_path, gripper=None):
922926
else:
923927
raise TypeError('bad argument passed as gripper')
924928

925-
return cls(links, name=name, gripper=gripper)
929+
links, name = ERobot.URDF_read(file_path)
930+
931+
return cls(links, name=name, gripper_links=gripper)
926932

927933
def _reset_cache(self):
928934
self._path_cache = {}
@@ -1126,7 +1132,8 @@ def fkine(
11261132
T = SE3.Empty()
11271133

11281134
for k, qk in enumerate(q):
1129-
qk = self.toradians(qk)
1135+
if unit == 'deg':
1136+
qk = self.toradians(qk)
11301137
link = end # start with last link
11311138

11321139
# add tool if provided
@@ -1998,9 +2005,9 @@ def jacob0(self, q):
19982005
def jacobe(self, q):
19992006
return self.ets().jacobe(q)
20002007

2001-
def fkine(self, q, unit='rad'):
2008+
def fkine(self, q, unit='rad', end=None, start=None):
20022009

2003-
return self.ets().eval(q, unit=unit)
2010+
return self.ets(start, end).eval(q, unit=unit)
20042011
# --------------------------------------------------------------------- #
20052012

20062013
def plot(
@@ -2185,6 +2192,7 @@ def teach(
21852192
return env
21862193

21872194

2195+
21882196
if __name__ == "__main__": # pragma nocover
21892197

21902198
# import roboticstoolbox as rtb

‎roboticstoolbox/robot/ETS.py

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
import numpy as np
1111
from spatialmath import SE3, SE2
1212
from spatialmath.base import getvector, getunit, trotx, troty, trotz, \
13-
issymbol, tr2jac, transl2, trot2, removesmall, trinv, \
13+
issymbol, tr2jac, transl2, trot2, removesmall, trinv, trinv2, \
1414
verifymatrix, iseye, tr2jac2
1515

1616
class BaseETS(UserList, ABC):
@@ -730,7 +730,7 @@ def __mul__(self, rest):
730730
return prod
731731

732732
def __imul__(self, rest):
733-
prod = ETS()
733+
prod = self.__class__()
734734
prod.data = self.data + rest.data
735735
return prod
736736

@@ -911,7 +911,7 @@ def inv(self):
911911
the reversed order of the transforms.
912912
""" # noqa
913913

914-
inv = ETS()
914+
inv = self.__class__()
915915
for ns in reversed(self.data):
916916
# get the namespace from the list
917917

@@ -920,12 +920,12 @@ def inv(self):
920920
if nsi.joint:
921921
nsi.flip ^= True # toggle flip status
922922
elif nsi.axis[0] == 'C':
923-
nsi.T = trinv(nsi.T)
923+
nsi.T = self._inverse(nsi.T)
924924
elif nsi.eta is not None:
925-
nsi.T = trinv(nsi.T)
925+
nsi.T = self._inverse(nsi.T)
926926
nsi.eta = -nsi.eta
927-
et = ETS() # create a new ETS instance
928-
et.data = [nsi] # set it's data from the dict
927+
et = self.__class__() # create a new ETS instance
928+
et.data = [nsi] # set its data from the dict
929929
inv *= et
930930
return inv
931931

@@ -1004,6 +1004,9 @@ def __init__(self, *pos, **kwargs):
10041004
super().__init__(*pos, **kwargs)
10051005
self._ndims = 3
10061006

1007+
def _inverse(self, T):
1008+
return trinv(T)
1009+
10071010
@property
10081011
def s(self):
10091012
if self.axis[1] == 'x':
@@ -1481,6 +1484,9 @@ def __init__(self, *pos, **kwargs):
14811484
super().__init__(*pos, **kwargs)
14821485
self._ndims = 2
14831486

1487+
def _inverse(self, T):
1488+
return trinv2(T)
1489+
14841490
@classmethod
14851491
def r(cls, eta=None, unit='rad', **kwargs):
14861492
"""

0 commit comments

Comments
 (0)
0