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

Commit 5c071d0

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents 0efd81a + 275e757 commit 5c071d0

File tree

6 files changed

+149
-65
lines changed

6 files changed

+149
-65
lines changed

‎roboticstoolbox/robot/Dynamics.py

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -49,11 +49,12 @@ def gravity(self, gravity_new):
4949
self.dynchanged()
5050

5151
# --------------------------------------------------------------------- #
52-
def dyntable(self):
52+
def dynamics(self):
5353
"""
5454
Pretty print the dynamic parameters (Robot superclass)
5555
56-
The dynamic parameters are printed in a table, with one row per link.
56+
The dynamic parameters (inertial and friction) are printed in a table,
57+
with one row per link.
5758
5859
Example:
5960
@@ -78,9 +79,9 @@ def dyntable(self):
7879
table.row(link.name, *link._dyn2list())
7980
print(table)
8081

81-
def printdyn(self):
82+
def dynamics_list(self):
8283
"""
83-
Print dynamic parameters
84+
Print dynamic parameters (Robot superclass)
8485
8586
Display the kinematic and dynamic parameters to the console in
8687
reable format

‎roboticstoolbox/robot/ELink.py

Lines changed: 10 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -13,16 +13,24 @@
1313

1414
class BaseELink(Link):
1515

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

1818
super().__init__()
1919

2020
self._name = name
21+
22+
if parent is not None:
23+
if isinstance(parent, (str, BaseELink)):
24+
self._parent = parent
25+
else:
26+
raise ValueError('parent must be BaseELink subclass or str')
27+
else:
28+
self._parent = None
29+
2130
self._joint_name = joint_name
2231

2332
self._jindex = None
2433
self._children = []
25-
self._parent = None
2634

2735
def __repr__(self):
2836
name = self.__class__.__name__
@@ -327,7 +335,6 @@ def __init__(
327335
ets=ETS(),
328336
v=None,
329337
jindex=None,
330-
parent=None,
331338
**kwargs):
332339

333340
# process common options
@@ -347,14 +354,6 @@ def __init__(
347354
elif jindex is None and v.jindex is not None:
348355
jindex = v.jindex
349356

350-
# TODO simplify this logic, can be ELink class or None
351-
# if isinstance(parent, list):
352-
# raise TypeError(
353-
# 'Only one parent link can be present')
354-
if not isinstance(parent, (ELink, str)) and parent is not None:
355-
raise TypeError(
356-
'Parent must be of type ELink, str or None')
357-
358357
# Initialise the static transform representing the constant
359358
# component of the ETS
360359
self._init_Ts()
@@ -624,7 +623,6 @@ def __init__(
624623
ets=ETS2(),
625624
v=None,
626625
jindex=None,
627-
parent=None,
628626
**kwargs):
629627

630628
# process common options
@@ -644,14 +642,6 @@ def __init__(
644642
elif jindex is None and v.jindex is not None:
645643
jindex = v.jindex
646644

647-
# TODO simplify this logic, can be ELink class or None
648-
# if isinstance(parent, list):
649-
# raise TypeError(
650-
# 'Only one parent link can be present')
651-
if not isinstance(parent, (ELink2, str)) and parent is not None:
652-
raise TypeError(
653-
'Parent must be of type ELink, str or None')
654-
655645
# Initialise the static transform representing the constant
656646
# component of the ETS
657647
self._init_Ts()

‎roboticstoolbox/robot/ERobot.py

Lines changed: 27 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -311,8 +311,11 @@ def __str__(self):
311311
parent_name,
312312
f"{{{link.name}}} = {{{parent_name}}}{s}"
313313
)
314-
315-
s = f"{self.__class__.__name__}: {self.name}"
314+
if isinstance(self, ERobot):
315+
classname = 'ERobot'
316+
elif isinstance(self, ERobot2):
317+
classname = 'ERobot2'
318+
s = f"{classname}: {self.name}"
316319
if self.manufacturer is not None and len(self.manufacturer) > 0:
317320
s += f" (by {self.manufacturer})"
318321
s += f", {self.n} joints ({self.structure})"
@@ -872,22 +875,13 @@ def __init__(self, arg, **kwargs):
872875
# we're passed an ETS string
873876
ets = arg
874877
links = []
875-
876878
# chop it up into segments, a link frame after every joint
877-
start = 0
878-
for j, k in enumerate(ets.joints()):
879-
ets_j = ets[start:k + 1]
880-
start = k + 1
881-
if j == 0:
882-
parent = None
883-
else:
884-
parent = links[-1]
879+
parent = None
880+
for j, ets_j in enumerate(arg.split()):
885881
elink = ELink(ets_j, parent=parent, name=f"link{j:d}")
882+
parent = elink
886883
links.append(elink)
887-
tail = arg[start:]
888-
if len(tail) > 0:
889-
elink = ELink(tail, parent=links[-1], name=f"link{j+1:d}")
890-
links.append(elink)
884+
891885
elif islistof(arg, ELink):
892886
links = arg
893887
else:
@@ -1063,7 +1057,7 @@ def URDF_read(file_path, tld=None):
10631057
# --------------------------------------------------------------------- #
10641058

10651059
def fkine(
1066-
self, q, end=None, start=None, tool=None,
1060+
self, q, unit='rad', end=None, start=None, tool=None,
10671061
include_base=True, fast=False):
10681062
'''
10691063
Forward kinematics
@@ -1132,7 +1126,7 @@ def fkine(
11321126
T = SE3.Empty()
11331127

11341128
for k, qk in enumerate(q):
1135-
1129+
qk = self.toradians(qk)
11361130
link = end # start with last link
11371131

11381132
# add tool if provided
@@ -1980,22 +1974,13 @@ def __init__(self, arg, **kwargs):
19801974
# we're passed an ETS string
19811975
ets = arg
19821976
links = []
1983-
19841977
# chop it up into segments, a link frame after every joint
1985-
start = 0
1986-
for j, k in enumerate(ets.joints()):
1987-
ets_j = ets[start:k + 1]
1988-
start = k + 1
1989-
if j == 0:
1990-
parent = None
1991-
else:
1992-
parent = links[-1]
1978+
parent = None
1979+
for j, ets_j in enumerate(arg.split()):
19931980
elink = ELink2(ets_j, parent=parent, name=f"link{j:d}")
1981+
parent = elink
19941982
links.append(elink)
1995-
tail = arg[start:]
1996-
if len(tail) > 0:
1997-
elink = ELink2(tail, parent=links[-1], name=f"link{j+1:d}")
1998-
links.append(elink)
1983+
19991984
elif islistof(arg, ELink2):
20001985
links = arg
20011986
else:
@@ -2013,8 +1998,9 @@ def jacob0(self, q):
20131998
def jacobe(self, q):
20141999
return self.ets().jacobe(q)
20152000

2016-
def fkine(self, q):
2017-
return self.ets().eval(q)
2001+
def fkine(self, q, unit='rad'):
2002+
2003+
return self.ets().eval(q, unit=unit)
20182004
# --------------------------------------------------------------------- #
20192005

20202006
def plot(
@@ -2108,6 +2094,7 @@ def teach(
21082094
fellipse=False,
21092095
eeframe=True,
21102096
name=False,
2097+
unit='rad',
21112098
backend='pyplot2'):
21122099
"""
21132100
2D Graphical teach pendant
@@ -2131,6 +2118,9 @@ def teach(
21312118
:type eeframe: bool
21322119
:param name: (Plot Option) Plot the name of the robot near its base
21332120
:type name: bool
2121+
:param unit: angular units: 'rad' [default], or 'deg'
2122+
:type unit: str
2123+
21342124
:return: A reference to the PyPlot object which controls the
21352125
matplotlib figure
21362126
:rtype: PyPlot
@@ -2158,6 +2148,11 @@ def teach(
21582148

21592149
if q is None:
21602150
q = np.zeros((self.n,))
2151+
else:
2152+
q = getvector(q, self.n)
2153+
2154+
if unit == 'deg':
2155+
q = self.toradians(q)
21612156

21622157
# Make an empty 3D figure
21632158
env = self._get_graphical_backend(backend)

‎roboticstoolbox/robot/ETS.py

Lines changed: 44 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,15 +13,14 @@
1313
issymbol, tr2jac, transl2, trot2, removesmall, trinv, \
1414
verifymatrix, iseye, tr2jac2
1515

16-
1716
class BaseETS(UserList, ABC):
1817

1918
# T is a NumPy array (4,4) or None
2019
# ets_tuple = namedtuple('ETS3', 'eta axis_func axis joint T jindex flip')
2120

2221
def __init__(
2322
self, axis=None, eta=None, axis_func=None,
24-
F438 unit='rad', j=None, flip=False):
23+
unit='rad', j=None, flip=False, qlim=None):
2524
"""
2625
Elementary transform sequence (superclass)
2726
@@ -148,7 +147,7 @@ def __init__(
148147
# Save all the params in a named tuple
149148
e = SimpleNamespace(
150149
eta=eta, axis_func=axis_func,
151-
axis=axis, joint=joint, T=T, jindex=j, flip=flip)
150+
axis=axis, joint=joint, T=T, jindex=j, flip=flip, qlim=qlim)
152151

153152
# And make it the only value of this instance
154153
self.data = [e]
@@ -397,6 +396,10 @@ def structure(self):
397396
return ''.join(
398397
['R' if self.isrevolute else 'P' for i in self.joints()])
399398

399+
@property
400+
def qlim(self):
401+
return self.data[0].qlim
402+
400403
def joints(self):
401404
"""
402405
Get index of joint transforms
@@ -527,6 +530,25 @@ def eval(self, q=None, unit='rad'):
527530

528531
return T
529532

533+
def split(self):
534+
"""
535+
Split ETS into link segments
536+
537+
Returns a list of ETS, each one, apart from the last,
538+
ends with a variable ET.
539+
"""
540+
segments = []
541+
start = 0
542+
for j, k in enumerate(self.joints()):
543+
ets_j = self[start:k + 1]
544+
start = k + 1
545+
segments.append(ets_j)
546+
tail = self[start:]
547+
if len(tail) > 0:
548+
segments.append(tail)
549+
550+
return segments
551+
530552
def compile(self):
531553
"""
532554
Compile an ETS
@@ -907,6 +929,25 @@ def inv(self):
907929
inv *= et
908930
return inv
909931

932+
def plot(self, *args, **kwargs):
933+
from roboticstoolbox.robot.ERobot import ERobot, ERobot2
934+
935+
if isinstance(self, ETS):
936+
robot = ERobot(self)
937+
else:
938+
robot = ERobot2(self)
939+
940+
robot.plot(*args, **kwargs)
941+
942+
def teach(self, *args, **kwargs):
943+
from roboticstoolbox.robot.ERobot import ERobot, ERobot2
944+
945+
if isinstance(self, ETS):
946+
robot = ERobot(self)
947+
else:
948+
robot = ERobot2(self)
949+
950+
robot.teach(*args, **kwargs)
910951

911952
class ETS(BaseETS):
912953
"""

‎roboticstoolbox/robot/Link.py

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -130,15 +130,16 @@ def dynpar(self, name, value, default):
130130
return 1
131131

132132
dynchange = 0
133+
# link inertial parameters
133134
dynchange += dynpar(self, 'm', m, 0.0)
134135
dynchange += dynpar(self, 'r', r, np.zeros((3,)))
135136
dynchange += dynpar(self, 'I', I, np.zeros((3, 3)))
136137

137-
# Motor dynamic parameters
138+
# Motor inertial and frictional parameters
138139
dynchange += dynpar(self, 'Jm', Jm, 0.0)
139140
dynchange += dynpar(self, 'B', B, 0.0)
140141
dynchange += dynpar(self, 'Tc', Tc, np.zeros((2,)))
141-
dynchange += dynpar(self, 'G', G, 1.0)
142+
dynchange += dynpar(self, 'G', G, 0.0)
142143

143144
self.actuator = None # reference to more advanced actuator model
144145

0 commit comments

Comments
 (0)
0