8000 Merge pull request #367 from hucik14/poe2ets · tomralex/robotics-toolbox-python@3fcc4d8 · GitHub
[go: up one dir, main page]
Skip to content

Commit 3fcc4d8

Browse files
jhavljhavl
authored
Merge pull request petercorke#367 from hucik14/poe2ets
poe2ets conversion feature
2 parents 9ff184c + d417d55 commit 3fcc4d8

File tree

2 files changed

+234
-15
lines changed

2 files changed

+234
-15
lines changed

‎roboticstoolbox/robot/PoERobot.py

Lines changed: 156 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,8 @@
33
from spatialmath import Twist3, SE3
44
from spatialmath.base import skew
55
from roboticstoolbox.robot import Link, Robot
6+
from roboticstoolbox.robot.ET import ET
7+
from roboticstoolbox.robot.ETS import ETS
68

79

810
class PoELink(Link):
@@ -15,7 +17,11 @@ class PoELink(Link):
1517
"""
1618

1719
def __init__(self, twist, name=None):
18-
super().__init__()
20+
# get ETS of the link in the world frame, given by its twist
21+
ets = self._ets_world(twist)
22+
23+
# initialize the link with its world frame ETS
24+
super().__init__(ets)
1925
self.S = Twist3(twist)
2026
self.name = name
2127

@@ -33,6 +39,87 @@ def __str__(self):
3339
s += "]"
3440
return s
3541

42+
def _ets_world(self, twist: Twist3) -> ETS:
43+
"""
44+
Convert twist to its corresponding ETS
45+
46+
This method obtains an SE3 object that corresponds to a given twist and
47+
returns its ETS in the world frame.
48+
49+
:param twist: given twist as Twist3 object
50+
51+
References
52+
----------
53+
- D. Huczala, T. Kot, J. Mlotek, J. Suder and M. Pfurner, *An Automated
54+
Conversion Between Selected Robot Kinematic Representations*, ICCMA 2022,
55+
Luxembourg, doi: 10.1109/ICCMA56665.2022.10011595
56+
57+
"""
58+
59+
# set base of the robot to the origin
60+
base = SE3()
61+
# get screw axis components
62+
w = twist.w
63+
v = twist.v
64+
65+
if isinstance(self, PoEPrismatic):
66+
# get the prismatic axis's directional vector component
67+
a_vec = v
68+
# vector of the x-axis (default to origin's n vector)
69+
n_vec = base.n
70+
# set point on screw axis to origin (prismatic joint has no moment)
71+
t_vec = base.t
72+
73+
elif isinstance(self, PoERevolute):
74+
# get the nearest point of the screw axis closest to the origin
75+
principal_point = np.cross(w, v)
76+
77+
# get vector of the x-axis
78+
if np.isclose(np.linalg.norm(principal_point), 0.0): # the points are
79+
# coincident
80+
n_vec = base.n
81+
else: # along the direction to principal point
82+
n_vec = principal_point / np.linalg.norm(principal_point)
83+
84+
# get the revolute axis directional vector component
85+
a_vec = w
86+
# point on screw axis
87+
t_vec = principal_point
88+
89+
else: # not a joint
90+
n_vec = base.n
91+
a_vec = base.a
92+
t_vec = v
93+
94+
o_vec = np.cross(a_vec, n_vec)
95+
96+
# construct transform from obtained vectors
97+
twist_as_SE3 = SE3.OA(o_vec, a_vec)
98+
twist_as_SE3.t = t_vec
99+
# get RPY parameters
100+
rpy = twist_as_SE3.rpy()
101+
102+
# prepare list of ETs, due to RPY convention the RPY order is reversed
103+
et_list = [
104+
ET.tx(twist_as_SE3.t[0]),
105+
ET.ty(twist_as_SE3.t[1]),
106+
ET.tz(twist_as_SE3.t[2]),
107+
ET.Rz(rpy[2]),
108+
ET.Ry(rpy[1]),
109+
ET.Rx(rpy[0]),
110+
]
111+
# remove ETs with empty transform
112+
et_list = [et for et in et_list if not np.isclose(et.eta, 0.0)]
113+
114+
# assign joint variable at the end of list (if the frame is not base or tool
115+
# frame)
116+
if isinstance(self, PoEPrismatic):
117+
et_list.append(ET.tz())
118+
elif isinstance(self, PoERevolute):
119+
et_list.append(ET.Rz())
120+
121+
return ETS(et_list)
122+
36123

37124
class PoERevolute(PoELink):
38125
def __init__(self, axis, point, **kwargs):
@@ -83,11 +170,17 @@ def __init__(self, links, T0, **kwargs):
83170
:seealso: :class:`PoEPrismatic` :class:`PoERevolute`
84171
"""
85172

86-
self._n = len(links)
173+
# add base link and end-effector link
174+
links.insert(0, PoELink(Twist3()))
175+
links.append(PoELink(T0.twist()))
87176

88177
super().__init__(links, **kwargs)
89178
self.T0 = T0
90179

180+
# update ETS according to the given links order (in PoELink their ETS is
181+
# given WITH relation to base frame, NOT to previous joint's ETS)
182+
self._update_ets()
183+
91184
def __str__(self):
92185
"""
93186
Pretty prints the PoE Model of the robot.
@@ -110,7 +203,6 @@ def __repr__(self):
110203
s += ")"
111204
return s
112205

113-
114206
def nbranches(self):
115207
return 0
116208

@@ -124,11 +216,11 @@ def fkine(self, q):
124216
:rtype: SE3
125217
"""
126218
T = None
127-
for link, qk in zip(self, q):
219+
for i in range(self.n):
128220
if T is None:
129-
T = link.S.exp(qk)
221+
T = self.links[i + 1].S.exp(q[i])
130222
else:
131-
T *= link.S.exp(qk)
223+
T *= self.links[i + 1].S.exp(q[i])
132224

133225
return T * self.T0
134226

@@ -146,9 +238,9 @@ def jacob0(self, q):
146238
"""
147239
columns = []
148240
T = SE3()
149-
for link, qk in zip(self, q):
150-
columns.append(T.Ad() @ link.S.S)
151-
T *= link.S.exp(qk)
241+
for i in range(self.n):
242+
columns.append(T.Ad() @ self.links[i + 1].S.S)
243+
T *= self.links[i + 1].S.exp(q[i])
152244
T *= self.T0
153245
J = np.column_stack(columns)
154246

@@ -168,21 +260,70 @@ def jacobe(self, q):
168260
"""
169261
columns = []
170262
T = SE3()
171-
for link, qk in zip(self, q):
172-
columns.append(T.Ad() @ link.S.S)
173-
T *= link.S.exp(qk)
263+
for i in range(self.n):
264+
columns.append(T.Ad() @ self.links[i + 1].S.S)
265+
T *= self.links[i + 1].S.exp(q[i])
174266
T *= self.T0
175267
J = np.column_stack(columns)
176268

177269
# convert velocity twist from world frame to EE frame
178270
return T.inv().Ad() @ J
179271

180-
def ets(self):
181-
return NotImplemented
272+
def _update_ets(self):
273+
"""
274+
Updates ETS of links when PoERobot is initialized according to joint order
275+
276+
By default, PoE representation specifies twists in relation to the base frame
277+
of a robot. Since the PoELinks are initialized prior to PoERobot, their ETS is
278+
given in the base frame, not in relation between links. This method creates
279+
partial transforms between links and obtains new ETSs that respect the links
280+
order.
281+
"""
282+
283+
# initialize transformations between joints from joint 1 to ee, related to
284+
# the world (base) frame
285+
twist_as_SE3_world = [SE3(link.Ts) for link in self.links]
286+
287+
# update the ee since its twist can provide transform with different x-, y-axes
288+
twist_as_SE3_world[-1] = self.T0
289+
290+
# initialize partial transforms
291+
twist_as_SE3_partial = [SE3()] * (self.n + 2)
292+
293+
# get partial transforms between links
294+
for i in reversed(range(1, self.n + 2)):
295+
twist_as_SE3_partial[i] = (
296+
twist_as_SE3_world[i - 1].inv() * twist_as_SE3_world[i]
297+
)
298+
299+
# prepare ET sequence
300+
for i, tf in enumerate(twist_as_SE3_partial):
301+
# get RPY parameters
302+
rpy = tf.rpy()
303+
304+
# prepare list of ETs, due to RPY convention the RPY order is reversed
305+
et_list = [
306+
ET.tx(tf.t[0]),
307+
ET.ty(tf.t[1]),
308+
ET.tz(tf.t[2]),
309+
ET.Rz(rpy[2]),
310+
ET.Ry(rpy[1]),
311+
ET.Rx(rpy[0]),
312+
]
313+
# remove ETs with empty transform
314+
et_list = [et for et in et_list if not np.isclose(et.eta, 0.0)]
315+
316+
# assign joint variable with corresponding index
317+
if self.links[i].isrevolute:
318+
et_list.append(ET.Rz(jindex=i - 1))
319+
elif self.links[i].isprismatic:
320+
et_list.append(ET.tz(jindex=i - 1))
321+
322+
# update the ETS for given link
323+
self.links[i].ets = ETS(et_list)
182324

183325

184326
if __name__ == "__main__": # pragma nocover
185-
186327
T0 = SE3.Trans(2, 0, 0)
187328

188329
# rotate about z-axis, through (0,0,0)

‎tests/test_PoERobot.py

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
"""
2+
@author: Daniel Huczala
3+
"""
4+
5+
import unittest
6+
import numpy.testing as nt
7+
import numpy as np
8+
from roboticstoolbox import Robot, PoERobot, PoERevolute, PoEPrismatic
9+
from spatialmath import SE3
10+
from spatialmath.base import trnorm
11+
12+
13+
class TestPoERobot(unittest.TestCase):
14+
def test_poe2ets_conversion(self):
15+
# 2RPR standard structure
16+
link1 = PoERevolute([0, 0, 1], [0, 0, 0])
17+
link2 = PoERevolute([0, 1, 0], [0, 0, 0.2])
18+
link3 = PoEPrismatic([0, 1, 0])
19+
link4 = PoERevolute([0, -1, 0], [0.2, 0, 0.5])
20+
tool = SE3(
21+
np.array([[1, 0, 0, 0.3], [0, 0, -1, 0], [0, 1, 0, 0.5], [0, 0, 0, 1]])
22+
)
23+
24+
r = PoERobot([link1, link2, link3, link4], tool)
25+
r_as_ets = Robot(r.ets())
26+
27+
q = [-1.3, 0, 2.5, -1.7]
28+
29+
# test fkine
30+
nt.assert_almost_equal(r.fkine(q).A, r_as_ets.fkine(q).A)
31+
32+
# test jacobians
33+
nt.assert_almost_equal(r.jacob0(q), r_as_ets.jacob0(q))
34+
nt.assert_almost_equal(r.jacobe(q), r_as_ets.jacobe(q))
35+
36+
#########################
37+
# 3RP arbitrary structure
38+
link1a = PoERevolute([0, 0, 1], [0, 0, 0])
39+
40+
w = [-0.635, 0.495, 0.592]
41+
w = w / np.linalg.norm(w)
42+
p = [-0.152, -0.023, -0.144]
43+
link2a = PoERevolute(w, p)
44+
45+
w = [-0.280, 0.790, 0.544]
46+
w = w / np.linalg.norm(w)
47+
p = [-0.300, -0.003, -0.150]
48+
link3a = PoERevolute(w, p)
49+
50+
w = [-0.280, 0.790, 0.544]
51+
w = w / np.linalg.norm(w)
52+
link4a = PoEPrismatic(w)
53+
54+
toola = np.array(
55+
[
56+
[0.2535, -0.5986, 0.7599, 0.2938],
57+
[-0.8063, 0.3032, 0.5078, -0.0005749],
58+
[-0.5344, -0.7414, -0.4058, 0.08402],
59+
[0, 0, 0, 1],
60+
]
61+
)
62+
toola = SE3(trnorm(toola))
63+
64+
ra = PoERobot([link1a, link2a, link3a, link4a], toola)
65+
ra_as_ets = Robot(ra.ets())
66+
67+
qa = [-1.3, -0.4, 2.5, -1.7]
68+
69+
# test fkine
70+
nt.assert_almost_equal(ra.fkine(qa).A, ra_as_ets.fkine(qa).A)
71+
72+
# test jacobians
73+
nt.assert_almost_equal(ra.jacob0(qa), ra_as_ets.jacob0(qa))
74+
nt.assert_almost_equal(ra.jacobe(qa), ra_as_ets.jacobe(qa))
75+
76+
77+
if __name__ == "__main__":
78+
unittest.main()

0 commit comments

Comments
 (0)
0