8000 Fix bug in ETS for tip to tip case · oridong/robotics-toolbox-python@c1e023f · GitHub
[go: up one dir, main page]
Skip to content

Commit c1e023f

Browse files
petercorkepetercorke
committed
Fix bug in ETS for tip to tip case
1 parent 275e757 commit c1e023f

File tree

3 files changed

+37
-26
lines changed

3 files changed

+37
-26
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: 21 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -505,7 +505,7 @@ def ets(self, start=None, end=None, explored=None, path=None):
505505
>>> panda = rtb.models.ETS.Panda()
506506
>>> panda.ets()
507507
"""
508-
v = self._getlink(start, self.base_link)
508+
link = self._getlink(start, self.base_link)
509509
if end is None and len(self.ee_links) > 1:
510510
raise ValueError(
511511
'ambiguous, specify which end-effector is required')
@@ -515,34 +515,38 @@ def ets(self, start=None, end=None, explored=None, path=None):
515515
explored = set()
516516
toplevel = path is None
517517

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

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

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

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

549+
546550
# --------------------------------------------------------------------- #
547551

548552
def showgraph(self, **kwargs):

‎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