Skip to content

Commit fbef42f

Browse files
committed
This reverts commit 4210843, reversing changes made to fab4625.
1 parent 4210843 commit fbef42f

File tree

11 files changed

+354
-912
lines changed

11 files changed

+354
-912
lines changed

docs/source/arm_ets.rst

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

44
.. codeauthor:: Jesse Haviland
55

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:
11-
12-
.. runblock:: pycon
13-
:linenos:
14-
15-
>>> from roboticstoolbox import ETS
16-
>>> 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-
6+
A number of models are defined in terms of elementary transform sequences.
7+
They can be listed by:
348

359
.. 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.
5710

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.
11+
>>> import roboticstoolbox as rtb
12+
>>> rtb.models.list(mtype="ETS")
6113

62-
**Reference:**
14+
:references:
6315

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.
16+
- https://petercorke.com/robotics/a-simple-and-systematic-approach-to-assigning-denavit-hartenberg-parameters/
6617

6718
ETS - 3D
6819
--------
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__
20+
.. automodule:: roboticstoolbox.robot.ETS
21+
:members:
7222
:undoc-members:
7323
:show-inheritance:
24+
:inherited-members:
25+
:special-members:
26+
:exclude-members: count, index, sort, remove, __dict__, __weakref__, __add__, __init__, __repr__, __str__, __module__
7427

7528
ETS - 2D
7629
--------
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__
30+
.. automodule:: roboticstoolbox.robot.ETS2
31+
:members:
8032
:undoc-members:
8133
:show-inheritance:
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")
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:
42+
:undoc-members:
43+
:show-inheritance:
44+
:inherited-members:
45+
:special-members:
46+
:exclude-members: count, index, sort, remove, __dict__, __weakref__, __add__, __init__, __repr__, __str__, __module__

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', 'mesh'),
172+
keywords=('dynamics', 'symbolic'),
173173
symbolic=symbolic,
174174
meshdir="meshes/UNIMATE/puma560"
175175
)

roboticstoolbox/models/ETS/Panda.py

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

3131
l0 = ELink(
32-
ETS.tz(0.333) * ETS.rz(),
32+
ETS.tz(0.333),
33+
ETS.rz(),
3334
name='link0',
3435
parent=None
3536
)
3637

3738
l1 = ELink(
38-
ETS.rx(-90*deg) * ETS.rz(),
39+
ETS.rx(-90*deg),
40+
ETS.rz(),
3941
name='link1',
4042
parent=l0
4143
)
4244

4345
l2 = ELink(
44-
ETS.rx(90*deg) * ETS.tz(0.316) * ETS.rz(),
46+
ETS.rx(90*deg) * ETS.tz(0.316),
47+
ETS.rz(),
4548
name='link2',
4649
parent=l1
4750
)
4851

4952
l3 = ELink(
50-
ETS.tx(0.0825) * ETS.rx(90, 'deg') * ETS.rz(),
53+
ETS.tx(0.0825) * ETS.rx(90*deg),
54+
ETS.rz(),
5155
name='link3',
5256
parent=l2
5357
)
5458

5559
l4 = ELink(
56-
ETS.tx(-0.0825) * ETS.rx(-90, 'deg') * ETS.tz(0.384) * ETS.rz(),
60+
ETS.tx(-0.0825) * ETS.rx(-90*deg) * ETS.tz(0.384),
61+
ETS.rz(),
5762
name='link4',
5863
parent=l3
5964
)
6065

6166
l5 = ELink(
62-
ETS.rx(90, 'deg') * ETS.rz(),
67+
ETS.rx(90*deg),
68+
ETS.rz(),
6369
name='link5',
6470
parent=l4
6571
)
6672

6773
l6 = ELink(
68-
ETS.tx(0.088) * ETS.rx(90, 'deg') * ETS.tz(0.107) * ETS.rz(),
74+
ETS.tx(0.088) * ETS.rx(90*deg) * ETS.tz(0.107),
75+
ETS.rz(),
6976
name='link6',
7077
parent=l5
7178
)
@@ -93,32 +100,3 @@ def __init__(self):
93100

94101
robot = Panda()
95102
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: 0 additions & 14 deletions
This file was deleted.

roboticstoolbox/robot/DHDynamics.py

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -381,14 +381,6 @@ 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-
392384
**Trajectory operation**
393385
394386
If `q`, `qd`, torque are matrices (m,n) then ``qdd`` is a matrix (m,n)

roboticstoolbox/robot/DHRobot.py

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

120120
def angle(theta):
121121
if sym.issymbol(theta):
122-
return "<<red>>" + str(theta)
122+
return "<<red>>" + str(L.alpha)
123123
else:
124-
return str(theta * deg) + "\u00b0"
124+
str(L.alpha * 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 = []
135126
if self.mdh:
136-
# MDH format
137127
table = ANSITable(
138128
Column("aⱼ₋₁", headalign="^"),
139129
Column("⍺ⱼ₋₁", headalign="^"),
140130
Column("θⱼ", headalign="^"),
141131
Column("dⱼ", headalign="^"),
142-
*qlim_columns,
143132
border="thick"
144133
)
145134
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 = []
153135
if L.isprismatic():
154-
table.row(L.a, angle(L.alpha), angle(L.theta), qs(j, L), *ql)
136+
table.row(L.a, angle(L.alpha), angle(L.theta), qs(j, L))
155137
else:
156-
table.row(L.a, angle(L.alpha), qs(j, L), L.d, *ql)
138+
table.row(L.a, angle(L.alpha), qs(j, L), L.d)
157139
else:
158140
# DH format
159141
table = ANSITable(
160142
Column("θⱼ", headalign="^", colalign="<"),
161143
Column("dⱼ", headalign="^"),
162144
Column("aⱼ", headalign="^"),
163145
Column("⍺ⱼ", headalign="^"),
164-
*qlim_columns,
165146
border="thick"
166147
)
167148
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 = []
175149
if L.isprismatic():
176150
table.row(
177-
angle(L.theta), qs(j, L), L.a, angle(L.alpha), *ql)
151+
angle(L.theta), qs(j, L), L.a, angle(L.alpha * deg))
178152
else:
179-
table.row(qs(j, L), L.d, L.a, angle(L.alpha), *ql)
153+
table.row(qs(j, L), L.d, L.a, angle(L.alpha))
180154

181155
s = str(table)
182156

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)
198-
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)
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))
216177

217-
return s
178+
return s + "\n" + str(table)
218179

219180

220181
def __add__(self, L):
@@ -2554,13 +2515,12 @@ def __init__(self, *args, **kwargs):
25542515
import roboticstoolbox as rtb
25552516
# import spatialmath.base.symbolic as sym
25562517

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

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy