Skip to content

Commit c003c3f

Browse files
jhavljhavl
committed
Update rne test
1 parent a53c864 commit c003c3f

File tree

2 files changed

+338
-96
lines changed

2 files changed

+338
-96
lines changed

‎roboticstoolbox/examples/test.py

Lines changed: 222 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -2,141 +2,267 @@
22
import roboticstoolbox as rtb
33
import spatialmath as sm
44

5-
# q0 = np.array(
6-
# [
7-
# -1.66441371,
8-
# -1.20998727,
9-
# 1.04248366,
10-
# -2.10222463,
11-
# 1.05097407,
12-
# 1.41173279,
13-
# 0.0053529,
14-
# ]
15-
# )
165

17-
# tol = 1e-6
6+
def rne(robot, q, qd, qdd):
7+
n = len(robot.links)
188

19-
# panda = rtb.models.Panda().ets()
9+
# allocate intermediate variables
10+
Xup = sm.SE3.Alloc(n)
11+
Xtree = sm.SE3.Alloc(n)
2012

21-
# Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
13+
# The body velocities, accelerations and forces
14+
v = sm.SpatialVelocity.Alloc(n)
15+
a = sm.SpatialAcceleration.Alloc(n)
16+
f = sm.SpatialForce.Alloc(n)
2217

23-
# solver = rtb.IK_QP()
18+
# The joint inertia
19+
I = sm.SpatialInertia.Alloc(n) # noqa: E741
2420

25-
# sol = panda.ik_LM(Tep, tol=tol, q0=q0, method="chan")
21+
# Joint motion subspace matrix
22+
s = []
2623

27-
robot = rtb.models.KinovaGen3()
24+
q = robot.qr
25+
qd = np.zeros(n)
26+
qdd = np.zeros(n)
2827

29-
glink = rtb.Link(
30-
rtb.ET.tz(0.12),
31-
name="gripper_link",
32-
m=0.831,
33-
r=[0, 0, 0.0473],
34-
parent=robot.links[-1],
35-
)
28+
Q = np.zeros(n)
3629

37-
robot.links.append(glink)
30+
for i, link in enumerate(robot.links):
31+
32+
# Allocate the inertia
33+
I[i] = sm.SpatialInertia(link.m, link.r, link.I)
34+
35+
# Compute the link transform
36+
Xtree[i] = sm.SE3(link.Ts, check=False) # type: ignore
37+
38+
# Append the variable axis of the joint to s
39+
if link.v is not None:
40+
s.append(link.v.s)
41+
else:
42+
s.append(np.zeros(6))
43+
44+
a_grav = -sm.SpatialAcceleration([0, 0, 9.81])
45+
46+
# Forward recursion
47+
for i, link in enumerate(robot.links):
48+
49+
if link.jindex is None:
50+
qi = 0
51+
qdi = 0
52+
qddi = 0
53+
else:
54+
qi = q[link.jindex]
55+
qdi = qd[link.jindex]
56+
qddi = qdd[link.jindex]
57+
58+
vJ = sm.SpatialVelocity(s[i] * qdi)
59+
60+
# Transform from parent(j) to j
61+
if link.isjoint:
62+
Xup[i] = sm.SE3(link.A(qi)).inv()
63+
else:
64+
Xup[i] = sm.SE3(link.A()).inv()
65+
66+
if link.parent is None:
67+
v[i] = vJ
68+
a[i] = Xup[i] * a_grav + sm.SpatialAcceleration(s[i] * qddi)
69+
else:
70+
v[i] = Xup[i] * v[i - 1] + vJ
71+
a[i] = Xup[i] * a[i - 1] + sm.SpatialAcceleration(s[i] * qddi) + v[i] @ vJ
72+
73+
f[i] = I[i] * a[i] + v[i] @ (I[i] * v[i])
74+
75+
# backward recursion
76+
for i in reversed(range(n)):
77+
78+
link = robot.links[i]
79+
80+
Q[i] = sum(f[i].A * s[i])
81+
82+
if link.parent is not None:
83+
f[i - 1] = f[i - 1] + Xup[i] * f[i]
84+
85+
return Q
86+
87+
88+
def rne_eff(robot, q, qd, qdd):
89+
n = len(robot.links)
90+
91+
# allocate intermediate variables
92+
Xup = np.empty((n, 4, 4))
93+
94+
Xtree = np.empty((n, 4, 4))
95+
96+
# The body velocities, accelerations and forces
97+
v = sm.SpatialVelocity.Alloc(n)
98+
a = sm.SpatialAcceleration.Alloc(n)
99+
f = sm.SpatialForce.Alloc(n)
100+
101+
# The joint inertia
102+
I = sm.SpatialInertia.Alloc(n) # noqa: E741
38103

104+
# Joint motion subspace matrix
105+
s = []
39106

40-
n = len(robot.links)
107+
q = robot.qr
108+
qd = np.zeros(n)
109+
qdd = np.zeros(n)
41110

42-
print(n)
111+
Q = np.zeros(n)
43112

44-
# allocate intermediate variables
45-
Xup = sm.SE3.Alloc(n)
46-
Xtree = sm.SE3.Alloc(n)
113+
for i, link in enumerate(robot.links):
47114

48-
# The body velocities, accelerations and forces
49-
v = sm.SpatialVelocity.Alloc(n)
50-
a = sm.SpatialAcceleration.Alloc(n)
51-
f = sm.SpatialForce.Alloc(n)
115+
# Allocate the inertia
116+
I[i] = sm.SpatialInertia(link.m, link.r, link.I)
52117

53-
# The joint inertia
54-
I = sm.SpatialInertia.Alloc(n) # noqa: E741
118+
# Compute the link transform
119+
Xtree[i] = link.Ts
55120

56-
# Joint motion subspace matrix
57-
s = []
121+
# Append the variable axis of the joint to s
122+
if link.v is not None:
123+
s.append(link.v.s)
124+
else:
125+
s.append(np.zeros(6))
58126

59-
q = robot.qr
60-
qd = np.zeros(n)
61-
qdd = np.zeros(n)
127+
a_grav = -sm.SpatialAcceleration([0, 0, 9.81])
62128

63-
Q = np.zeros(n)
129+
# Forward recursion
130+
for i, link in enumerate(robot.links):
131+
132+
if link.jindex is None:
133+
qi = 0
134+
qdi = 0
135+
qddi = 0
136+
else:
137+
qi = q[link.jindex]
138+
qdi = qd[link.jindex]
139+
qddi = qdd[link.jindex]
140+
141+
vJ = sm.SpatialVelocity(s[i] * qdi)
142+
143+
# Transform from parent(j) to j
144+
if link.isjoint:
145+
Xup[i] = np.linalg.inv(link.A(qi))
146+
else:
147+
Xup[i] = np.linalg.inv(link.A())
148+
149+
if link.parent is None:
150+
v[i] = vJ
151+
a[i] = Xup[i] * a_grav + sm.SpatialAcceleration(s[i] * qddi)
152+
else:
153+
v[i] = Xup[i] * v[i - 1] + vJ
154+
a[i] = Xup[i] * a[i - 1] + sm.SpatialAcceleration(s[i] * qddi) + v[i] @ vJ
155+
156+
f[i] = I[i] * a[i] + v[i] @ (I[i] * v[i])
157+
158+
# backward recursion
159+
for i in reversed(range(n)):
160+
161+
link = robot.links[i]
162+
163+
Q[i] = sum(f[i].A * s[i])
164+
165+
if link.parent is not None:
166+
f[i - 1] = f[i - 1] + Xup[i] * f[i]
167+
168+
return Q
169+
170+
171+
robot = rtb.models.KinovaGen3()
172+
173+
glink = rtb.Link(
174+
rtb.ET.tz(0.12),
175+
name="gripper_link",
176+
m=0.831,
177+
r=[0, 0, 0.0473],
178+
parent=robot.links[-1],
179+
)
180+
181+
robot.links.append(glink)
64182

65183
# for link in robot.links:
66184
# print()
67185
# print(link.name)
68186
# print(link.isjoint)
69187
# print(link.m)
188+
# print(link.r)
189+
# print(link.I)
70190

71-
# for link in robot.grippers[0].links:
72-
# print()
73-
# print(link.name)
74-
# print(link.isjoint)
75-
# print(link.m)
191+
# q = rne(robot, robot.qr, np.zeros(7), np.zeros(7))
192+
# q = robot.qr
193+
# qd = np.zeros(7)
194+
# qdd = np.zeros(7)
76195

77-
for i, link in enumerate(robot.links):
196+
# for i in range(1000):
197+
# tau = robot.rne(q, qd, qdd)
198+
# # q = rne(robot, robot.qr, np.zeros(7), np.zeros(7))
199+
# # print(i)
78200

79-
# Allocate the inertia
80-
I[i] = sm.SpatialInertia(link.m, link.r, link.I)
81201

82-
# Compute the link transform
83-
Xtree[i] = sm.SE3(link.Ts, check=False) # type: ignore
202+
# print(np.round(tau, 2))
84203

85-
# Append the variable axis of the joint to s
86-
if link.v is not None:
87-
s.append(link.v.s)
88-
else:
89-
s.append(np.zeros(6))
204+
# [ 0. 0. 14.2 0.12 -9.22 0. -4.47 -0. 0. 0. 0. ]
90205

91-
a_grav = -sm.SpatialAcceleration([0, 0, 9.81])
206+
l1a = rtb.Link(ets=rtb.ETS(rtb.ET.Rx()), m=1, r=[0.5, 0, 0], name="l1")
207+
l2a = rtb.Link(ets=rtb.ETS(rtb.ET.Rz()), m=1, r=[0, 0.5, 0], parent=l1a, name="l2")
208+
l3a = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.0, 0, 0.5], parent=l2a, name="l3")
209+
robota = rtb.Robot([l1a, l2a, l3a], name="simple 3 link a")
92210

93-
# Forward recursion
94-
for i, link in enumerate(robot.links):
211+
l1b = rtb.Link(ets=rtb.ETS(rtb.ET.Rx()), m=1, r=[0.5, 0, 0], name="l1")
212+
l2b = rtb.Link(ets=rtb.ETS(rtb.ET.Rz(0.5)), m=1, r=[0, 0.5, 0], parent=l1b, name="l2")
213+
l3b = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.0, 0, 0.5], parent=l2b, name="l3")
214+
robotb = rtb.Robot([l1b, l2b, l3b], name="simple 3 link b")
95215

96-
if link.jindex is None:
97-
qi = 0
98-
qdi = 0
99-
qddi = 0
100-
else:
101-
qi = q[link.jindex]
102-
qdi = qd[link.jindex]
103-
qddi = qdd[link.jindex]
216+
l1c = rtb.Link(ets=rtb.ETS(rtb.ET.tx(0.5)), m=1, r=[0.5, 0, 0], name="l1")
217+
l2c = rtb.Link(ets=rtb.ETS(rtb.ET.Rx()), m=1, r=[0, 0.5, 0], parent=l1c, name="l2")
104218

105-
vJ = sm.SpatialVelocity(s[i] * qdi)
219+
# Branch 1
220+
l3c = rtb.Link(ets=rtb.ETS(rtb.ET.tz(0.5)), m=1, r=[0.0, 0, 0.5], parent=l2c, name="l3")
221+
l4c = rtb.Link(ets=rtb.ETS(rtb.ET.Rz()), m=1, r=[0.0, 0, 0.5], parent=l3c, name="l4")
106222

107-
# Transform from parent(j) to j
108-
if link.isjoint:
109-
Xup[i] = sm.SE3(link.A(qi)).inv()
110-
else:
111-
Xup[i] = sm.SE3(link.A()).inv()
223+
# Branch 2
224+
l5c = rtb.Link(ets=rtb.ETS(rtb.ET.tz(0.5)), m=1, r=[0.0, 0, 0.5], parent=l2c, name="l5")
225+
l6c = rtb.Link(ets=rtb.ETS(rtb.ET.Rz()), m=1, r=[0.0, 0, 0.5], parent=l5c, name="l6")
112226

113-
if link.parent is None:
114-
v[i] = vJ
115-
a[i] = Xup[i] * a_grav + sm.SpatialAcceleration(s[i] * qddi)
116-
else:
117-
jp = link.parent.jindex # type: ignore
118-
v[i] = Xup[i] * v[i - 1] + vJ
119-
a[i] = Xup[i] * a[i - 1] + sm.SpatialAcceleration(s[i] * qddi) + v[i] @ vJ
227+
robotc = rtb.Robot([l1c, l2c, l3c, l4c, l5c, l6c], name="branch 3 link c")
120228

121-
f[i] = I[i] * a[i] + v[i] @ (I[i] * v[i])
229+
za = np.array([0.5, 0.5, 0.5])
230+
zb = np.array([0.5, 0.5])
231+
zc = np.array([0.5, 0.5, 0.5])
122232

123-
# backward recursion
124-
for i in reversed(range(n)):
233+
print("\nRobot A")
234+
taua = robota.rne(za, za, za)
235+
print(taua)
125236

126-
link = robot.links[i]
237+
print("\nRobot B")
238+
taub = robotb.rne(zb, zb, zb)
239+
print(taub)
127240

128-
Q[i] = sum(f[i].A * s[i])
241+
print("\nRobot C")
242+
tauc = robotc.rne(zc, zc, zc)
243+
print(tauc)
129244

130-
if link.parent is not None:
131-
f[i - 1] = f[i - 1] + Xup[i] * f[i]
132245

133-
print(np.round(Q, 2))
246+
# l1 = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.5, 0, 0], name="l1")
247+
# l2 = rtb.Link(ets=rtb.ETS(rtb.ET.tx(1)), m=1, r=[0.5, 0, 0], parent=l1, name="l2")
248+
# l3 = rtb.Link(ets=rtb.ETS([rtb.ET.Ry()]), m=0, r=[0.0, 0, 0], parent=l2, name="l3")
249+
# robot = rtb.Robot([l1, l2, l3], name="simple 3 link")
250+
# z = np.zeros(robot.n)
134251

252+
# # check gravity load
253+
# tau = robot.rne(z, z, z) / 9.81
254+
# print(tau)
255+
256+
# # nt.assert_array_almost_equal(tau, np.r_[-2, -0.5])
257+
258+
# print("\n\nNew Robot\n")
259+
# l1 = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.5, 0, 0], name="l1")
260+
# l2 = rtb.Link(
261+
# ets=rtb.ETS([rtb.ET.tx(1), rtb.ET.Ry()]), m=1, r=[0.5, 0, 0], parent=l1, name="l2"
262+
# )
263+
# robot = rtb.Robot([l1, l2], name="simple 2 link")
264+
# z = np.zeros(robot.n)
135265

136-
for link in robot.links:
137-
print()
138-
print(link.name)
139-
print(link.isjoint)
140-
print(link.m)
141-
print(link.r)
142-
print(link.I)
266+
# # check gravity load
267+
# tau = robot.rne(z, z, z) / 9.81
268+
# print(tau)

0 commit comments

Comments
 (0)