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

Commit 4210843

Browse files
Samuel-DrewSamuel-Drew
committed
2 parents fab4625 + 9a208d8 commit 4210843

File tree

11 files changed

+912
-354
lines changed

11 files changed

+912
-354
lines changed

‎docs/source/arm_ets.rst

Lines changed: 72 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -3,44 +3,90 @@ Elementary transform sequence (ETS) models
33

44
.. codeauthor:: Jesse Haviland
55

6-
A number of models are defined in terms of elementary transform sequences.
7-
They can be listed by:
6+
Elementary transforms are canonic rotations or translations about, or along,
7+
the x-, y- or z-axes. The amount of rotation or translation can be a constant
8+
or a variable. A variable amount corresponds to a joint.
9+
10+
Consider the example:
811

912
.. runblock:: pycon
13+
:linenos:
1014

11-
>>> import roboticstoolbox as rtb
12-
>>> rtb.models.list(mtype="ETS")
15+
>>> from roboticstoolbox import ETS
16+< 57AE div class="diff-text-inner"> >>> ETS.rx(45, 'deg')
17+
>>> ETS.tz(0.75)
18+
>>> e = ETS.rx(0.3) * ETS.tz(0.75)
19+
>>> print(e)
20+
>>> e.eval()
21+
22+
In lines 2-5 we defined two elementary transforms. Line 2 defines a rotation
23+
of 45° about the x-axis, and line 4 defines a translation of 0.75m along the z-axis.
24+
Compounding them in line 6, the result is the two elementary transforms in a
25+
sequence - an elementary transform sequence (ETS). An ETS can be arbitrarily
26+
long.
27+
28+
Line 8 *evaluates* the sequence, substituting in values, and the result is an
29+
SE(3) matrix encapsulated in an ``SE3`` object.
30+
31+
The real power comes from having variable arguments to the elementary transforms
32+
as shown in this example where we define a simple two link planar manipulator.
33+
34+
35+
.. runblock:: pycon
36+
:linenos:
37+
38+
>>> from roboticstoolbox import ETS
39+
>>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
40+
>>> print(e)
41+
>>> len(e)
42+
>>> e[1:3]
43+
>>> e.eval([0, 0])
44+
>>> e.eval([90, -90], 'deg')
45+
46+
Line 2 succinctly describes the kinematics in terms of elementary transforms: a rotation around the z-axis by the
47+
first joint angle, then a translation in the x-direction, then a rotation around
48+
the z-axis by the second joint angle, and finally a translation in the
49+
x-direction.
50+
51+
Line 3 creates the elementary transform sequence, with variable transforms.
52+
``e`` is a single object that encapsulates a list of elementary transforms, and list like
53+
methods such as ``len`` as well as indexing and slicing as shown in lines 5-8.
54+
55+
Lines 9-18 *evaluate* the sequence, and substitutes elements from the passed
56+
arrays as the joint variables.
1357

14-
:references:
58+
This approach is general enough to be able to describe any serial-link robot
59+
manipulator. For a branched manipulator we can use ETS to describe the
60+
connections between every parent and child link pair.
1561

16-
- https://petercorke.com/robotics/a-simple-and-systematic-approach-to-assigning-denavit-hartenberg-parameters/
62+
**Reference:**
63+
64+
- `A simple and systematic approach to assigning Denavit-Hartenberg parameters <https://petercorke.com/robotics/a-simple-and-systematic-approach-to-assigning-denavit-hartenberg-parameters>`_.
65+
Peter I. Corke, IEEE Transactions on Robotics, 23(3), pp 590-594, June 2007.
1766

1867
ETS - 3D
1968
--------
20-
.. automodule:: roboticstoolbox.robot.ETS
21-
:members:
69+
70+
.. autoclass:: roboticstoolbox.robot.ETS.ETS
71+
:members: rx, ry, rz, tx, ty, tz, eta, eval, T, joints, axis, jtype, config, __getitem__, __mul__, __repr__, __str__
2272
:undoc-members:
2373
:show-inheritance:
24-
:inherited-members:
25-
:special-members:
26-
:exclude-members: count, index, sort, remove, __dict__, __weakref__, __add__, __init__, __repr__, __str__, __module__
2774

2875
ETS - 2D
2976
--------
30-
.. automodule:: roboticstoolbox.robot.ETS2
31-
:members:
32-
:undoc-members:
33-
:show-inheritance:
34-
:inherited-members:
35-
:special-members:
36-
:exclude-members: count, index, sort, remove, __dict__, __weakref__, __add__, __init__, __repr__, __str__, __module__
37-
38-
ET
39-
------------
40-
.. automodule:: roboticstoolbox.robot.ET
41-
:members:
77+
78+
.. autoclass:: roboticstoolbox.robot.ETS2.ETS
79+
:members: rx, ry, rz, tx, ty, tz, eta, eval, T, joints, axis, jtype, config, __getitem__, __mul__, __repr__, __str__
4280
:undoc-members:
4381
:show-inheritance:
44-
:inherited-members:
45-
:special-members:
46-
:exclude-members: count, index, sort, remove, __dict__, __weakref__, __add__, __init__, __repr__, __str__, __module__
82+
83+
ETS Robot models
84+
----------------
85+
86+
A number of models are defined in terms of elementary transform sequences.
87+
They can be listed by:
88+
89+
.. runblock:: pycon
90+
91+
>>> import roboticstoolbox as rtb
92+
>>> rtb.models.list(mtype="ETS")

‎roboticstoolbox/models/DH/Puma560.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,7 @@ def __init__(self, symbolic=False):
169169
L,
170170
name="Puma 560",
171171
manufacturer="Unimation",
172-
keywords=('dynamics', 'symbolic'),
172+
keywords=('dynamics', 'symbolic', 'mesh'),
173173
symbolic=symbolic,
174174
meshdir="meshes/UNIMATE/puma560"
175175
)

‎roboticstoolbox/models/ETS/Panda.py

Lines changed: 36 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -29,50 +29,43 @@ def __init__(self):
2929
tool_offset = (103)*mm
3030

3131
l0 = ELink(
32-
ETS.tz(0.333),
33-
ETS.rz(),
32+
ETS.tz(0.333) * ETS.rz(),
3433
name='link0',
3534
parent=None
3635
)
3736

3837
l1 = ELink(
39-
ETS.rx(-90*deg),
40-
ETS.rz(),
38+
ETS.rx(-90*deg) * ETS.rz(),
4139
name='link1',
4240
parent=l0
4341
)
4442

4543
l2 = ELink(
46-
ETS.rx(90*deg) * ETS.tz(0.316),
47-
ETS.rz(),
44+
ETS.rx(90*deg) * ETS.tz(0.316) * ETS.rz(),
4845
name='link2',
4946
parent=l1
5047
)
5148

5249
l3 = ELink(
53-
ETS.tx(0.0825) * ETS.rx(90*deg),
54-
ETS.rz(),
50+
ETS.tx(0.0825) * ETS.rx(90, 'deg') * ETS.rz(),
5551
name='link3',
5652
parent=l2
5753
)
5854

5955
l4 = ELink(
60-
ETS.tx(-0.0825) * ETS.rx(-90*deg) * ETS.tz(0.384),
61-
ETS.rz(),
56+
ETS.tx(-0.0825) * ETS.rx(-90, 'deg') * ETS.tz(0.384) * ETS.rz(),
6257
name='link4',
6358
parent=l3
6459
)
6560

6661
l5 = ELink(
67-
ETS.rx(90*deg),
68-
ETS.rz(),
62+
ETS.rx(90, 'deg') * ETS.rz(),
6963
name='link5',
7064
parent=l4
7165
)
7266

7367
l6 = ELink(
74-
ETS.tx(0.088) * ETS.rx(90*deg) * ETS.tz(0.107),
75-
ETS.rz(),
68+
ETS.tx(0.088) * ETS.rx(90, 'deg') * ETS.tz(0.107) * ETS.rz(),
7669
name='link6',
7770
parent=l5
7871
)
@@ -100,3 +93,32 @@ def __init__(self):
10093

10194
robot = Panda()
10295
2851 print(robot)
96+
97+
print(robot.n, robot.M)
98+
print(robot.elinks)
99+
print(robot.q_idx)
100+
for link in robot:
101+
print(link.name, link)
102+
print(link.ets, link.v, link.isjoint)
103+
print(link.parent, link.child)
104+
print(link.Ts)
105+
print(link.geometry, link.collision)
106+
107+
print()
108+
109+
from ansitable import ANSITable, Column
110+
111+
table = ANSITable(
112+
Column("link"),
113+
Column("parent"),
114+
Column("ETS", headalign="^", colalign="<"),
115+
border="thin")
116+
for link in robot:
117+
if link.isjoint:
118+
color = ""
119+
else:
120+
color = "<<blue>>"
121+
table.row(color + link.name,
122+
link.parent.name if link.parent is not None else "-",
123+
link.ets * link.v if link.v is not None else link.ets)
124+
table.print()

‎roboticstoolbox/models/ETS/Puma560.py

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
from roboticstoolbox import ETS as ET
2+
from roboticstoolbox import ERobot
3+
l1 = 0.672
4+
l2 = 0.2337
5+
l3 = 0.4318
6+
l4 = -0.0837
7+
l5 = 0.4318
8+
l6 = 0.0203
9+
10+
e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() * ET.tz(l3) * ET.tx(l6) * \
11+
ET.ty(l4) * ET.ry() * ET.tz(l5) * ET.rz() * ET.ry() * ET.rz() * ET.tx(0.2)
12+
13+
robot = ERobot(e, name="my first ERobot")
14+
print(robot)

‎roboticstoolbox/robot/DHDynamics.py

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -381,6 +381,14 @@ def accel(self, q, qd, torque):
381381
382382
:math:`\ddot{q} = \mathbf{I}^{-1} \left(\tau - \mathbf{C}(q)\dot{q} - \mathbf{g}(q)\right)`
383383
384+
Example:
385+
386+
.. runblock:: pycon
387+
388+
>>> import roboticstoolbox as rtb
389+
>>> puma = rtb.models.DH.Puma560()
390+
>>> puma.accel(puma.qz, 0.5 * np.ones(6), np.zeros(6))
391+
384392
**Trajectory operation**
385393
386394
If `q`, `qd`, torque are matrices (m,n) then ``qdd`` is a matrix (m,n)

‎roboticstoolbox/robot/DHRobot.py

Lines changed: 71 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -119,63 +119,102 @@ def qs(j, link):
119119

120120
def angle(theta):
121121
if sym.issymbol(theta):
122-
return "<<red>>" + str(L.alpha)
122+
return "<<red>>" + str(theta)
123123
else:
124-
str(L.alpha * deg) + "\u00b0"
124+
return str(theta * deg) + "\u00b0"
125125

126+
has_qlim = any([link._qlim is not None for link in self])
127+
if has_qlim:
128+
qlim_columns = [
129+
Column("q⁻", headalign="^"), Column("q⁺", headalign="^"),
130+
]
131+
qlim = self.qlim
132+
133+
else:
134+
qlim_columns = []
126135
if self.mdh:
136+
# MDH format
127137
table = ANSITable(
128138
Column("aⱼ₋₁", headalign="^"),
129139
Column("⍺ⱼ₋₁", headalign="^"),
130140
Column("θⱼ", headalign="^"),
131141
Column("dⱼ", headalign="^"),
142+
*qlim_columns,
132143
border="thick"
133144
)
134145
for j, L in enumerate(self):
146+
if has_qlim:
147+
if L.isprismatic():
148+
ql = [qlim[0, j], qlim[1, j]]
149+
else:
150+
ql = [angle(qlim[k, j]) for k in [0, 1]]
151+
else:
152+
ql = []
135153
if L.isprismatic():
136-
table.row(L.a, angle(L.alpha), angle(L.theta), qs(j, L))
154+
table.row(L.a, angle(L.alpha), angle(L.theta), qs(j, L), *ql)
137155
else:
138-
table.row(L.a, angle(L.alpha), qs(j, L), L.d)
156+
table.row(L.a, angle(L.alpha), qs(j, L), L.d, *ql)
139157
else:
140158
# DH format
141159
table = ANSITable(
142160
Column("θⱼ", headalign="^", colalign="<"),
143161
Column("dⱼ", headalign="^"),
144162
Column("aⱼ", headalign="^"),
145163
Column("⍺ⱼ", headalign="^"),
164+
*qlim_columns,
146165
border="thick"
147166
)
148167
for j, L in enumerate(self):
168+
if has_qlim:
169+
if L.isprismatic():
170+
ql = [qlim[0, j], qlim[1, j]]
171+
else:
172+
ql = [angle(qlim[k, j]) for k in [0, 1]]
173+
else:
174+
ql = []
149175
if L.isprismatic():
150176
table.row(
151-
angle(L.theta), qs(j, L), L.a, angle(L.alpha * deg))
177+
angle(L.theta), qs(j, L), L.a, angle(L.alpha), *ql)
152178
else:
153-
table.row(qs(j, L), L.d, L.a, angle(L.alpha))
179+
table.row(qs(j, L), L.d, L.a, angle(L.alpha), *ql)
154180

155181
s = str(table)
156182

157-
table = ANSITable(
158-
Column("", colalign=">"),
159-
Column("", colalign="<"), border="thin", header=False)
160-
if self._base is not None:
161-
table.row(
162-
"base", self._base.printline(
163-
orient="rpy/xyz", fmt="{:.2g}", file=None))
164-
if self._tool is not None:
165-
table.row(
166-
"tool", self._tool.printline(
167-
orient="rpy/xyz", fmt="{:.2g}", file=None))
168-
169-
for name, q in self._configdict.items():
170-
qlist = []
171-
for i, L in enumerate(self):
172-
if L.isprismatic():
173-
qlist.append(f"{q[i]:.3g}")
174-
else:
175-
qlist.append(f"{q[i] * deg:.3g}\u00b0")
176-
table.row(name, ', '.join(qlist))
183+
# show tool and base
184+
if self._tool is not None or self._tool is not None:
185+
table = ANSITable(
186+
Column("", colalign=">"),
187+
Column("", colalign="<"),
188+
border="thin", header=False)
189+
if self._base is not None:
190+
table.row(
191+
"base", self._base.printline(
192+
orient="rpy/xyz", fmt="{:.2g}", file=None))
193+
if self._tool is not None:
194+
table.row(
195+
"tool", self._tool.printline(
196+
orient="rpy/xyz", fmt="{:.2g}", file=None))
197+
s += "\n" + str(table)
177198

178-
return s + "\n" + str(table)
199+
# show named configurations
200+
if len(self._configdict) > 0:
201+
table = ANSITable(
202+
Column("name", colalign=">"),
203+
*[Column(f"q{j:d}", colalign="<", headalign="<") for j in range(self.n)],
204+
border="thin")
205+
206+
for name, q in self._configdict.items():
207+
qlist = []
208+
for i, L in enumerate(self):
209+
if L.isprismatic():
210+
qlist.append(f"{q[i]:.3g}")
211+
else:
212+
qlist.append(angle(q[i]))
213+
table.row(name, *qlist)
214+
215+
s += "\n" + str(table)
216+
217+
return s
179218

180219

181220
def __add__(self, L):
@@ -2515,12 +2554,13 @@ def __init__(self, *args, **kwargs):
25152554
import roboticstoolbox as rtb
25162555
# import spatialmath.base.symbolic as sym
25172556

2518-
puma = rtb.models.DH.Planar2()
2557+
planar = rtb.models.DH.Planar2()
25192558
# J = puma.jacob0(puma.qn)
25202559
# print(J)
25212560
# print(puma.manipulability(puma.qn))
25222561
# print(puma.manipulability(puma.qn, 'asada'))
25232562
#tw, T0 = puma.twists(puma.qz)
2524-
print(puma.qlim)
2525-
print(puma[0].qlim)
2526-
print(puma.islimit([0, -5]))
2563+
print(planar)
2564+
2565+
puma = rtb.models.DH.Puma560()
2566+
print(puma)

0 commit comments

Comments
 (0)
0