Skip to content

Commit aeb85de

Browse files
jhavljhavl
committed
fknm tests
1 parent fd4a29d commit aeb85de

File tree

6 files changed

+193
-111
lines changed

6 files changed

+193
-111
lines changed

‎examples/swift2.py

Lines changed: 103 additions & 99 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,8 @@
8585
pm = rtb.models.DH.Panda()
8686
p = rtb.models.ETS.Panda()
8787
p2 = rtb.models.Panda()
88-
q = np.array([1, 2, 3, 4, 5, 6, 7])
88+
q = np.array([1.0, 2, 3, 4, 5, 6, 7])
89+
# q = np.ones(7)
8990
p.q = q
9091
pm.q = q
9192

@@ -101,148 +102,151 @@
101102
print()
102103
print()
103104

104-
# import swift
105105

106+
print(np.round(p.fkine(p.qz, end=p.links[1], fast=True), 2))
106107

107-
# num = 500000
108-
# b = np.random.randn(num)
109-
# sm.base.trotz(1.0)
108+
# # import swift
110109

111-
# def stepper():
112-
# for i in range(num):
113-
# sm.base.trotz(b[i])
114110

111+
# # num = 500000
112+
# # b = np.random.randn(num)
113+
# # sm.base.trotz(1.0)
115114

116-
# cProfile.run('stepper()')
115+
# # def stepper():
116+
# # for i in range(num):
117+
# # sm.base.trotz(b[i])
117118

118-
# ur = rtb.models.UR5()
119-
# ur.base = sm.SE3(0.3, 1, 0) * sm.SE3.Rz(np.pi/2)
120-
# ur.q = [-0.4, -np.pi/2 - 0.3, np.pi/2 + 0.3, -np.pi/2, -np.pi/2, 0]
121-
# env.add(ur)
122119

123-
# lbr = rtb.models.LBR()
124-
# lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi/2)
125-
# lbr.q = lbr.qr
126-
# env.add(lbr)
120+
# # cProfile.run('stepper()')
127121

128-
# k = rtb.models.KinovaGen3()
129-
# k.q = k.qr
130-
# k.q[0] = np.pi + 0.15
131-
# k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi/2)
132-
# env.add(k)
122+
# # ur = rtb.models.UR5()
123+
# # ur.base = sm.SE3(0.3, 1, 0) * sm.SE3.Rz(np.pi/2)
124+
# # ur.q = [-0.4, -np.pi/2 - 0.3, np.pi/2 + 0.3, -np.pi/2, -np.pi/2, 0]
125+
# # env.add(ur)
133126

134-
env = swift.Swift(_dev=True)
135-
env.launch()
127+
# # lbr = rtb.models.LBR()
128+
# # lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi/2)
129+
# # lbr.q = lbr.qr
130+
# # env.add(lbr)
136131

132+
# # k = rtb.models.KinovaGen3()
133+
# # k.q = k.qr
134+
# # k.q[0] = np.pi + 0.15
135+
# # k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi/2)
136+
# # env.add(k)
137137

138-
def slidercb(e):
139-
print(e)
138+
# env = swift.Swift(_dev=True)
139+
# env.launch()
140140

141141

142-
def selectcb(e):
143-
print(e)
142+
# def slidercb(e):
143+
# print(e)
144144

145145

146-
def checkcb(e):
147-
print(e)
148-
# select.value = e
146+
# def selectcb(e):
147+
# print(e)
149148

150149

151-
label = swift.Label('Demo')
152-
slider = swift.Slider(slidercb, 10, 95, 5, 15, 'slider this is', ' d')
153-
select = swift.Select(selectcb, 'selec', [
154-
'on', 'tw', 'three'], 2)
150+
# def checkcb(e):
151+
# print(e)
152+
# # select.value = e
155153

156-
check = swift.Checkbox(checkcb, 'checkbox', [
157-
'on', 'tw', 'three'], [False, True, True])
158154

155+
# label = swift.Label('Demo')
156+
# slider = swift.Slider(slidercb, 10, 95, 5, 15, 'slider this is', ' d')
157+
# select = swift.Select(selectcb, 'selec', [
158+
# 'on', 'tw', 'three'], 2)
159159

160-
def buttoncb(e):
161-
print('BUTTON')
162-
# check.checked = [True, True, False]
163-
# check.desc = 'new desc'
164-
slider.value = 60
160+
# check = swift.Checkbox(checkcb, 'checkbox', [
161+
# 'on', 'tw', 'three'], [False, True, True])
165162

166163

167-
button = swift.Button(buttoncb, 'button')
164+
# def buttoncb(e):
165+
# print('BUTTON')
166+
# # check.checked = [True, True, False]
167+
# # check.desc = 'new desc'
168+
# slider.value = 60
168169

169170

170-
def radiocb(e):
171-
print(e)
172-
select.value = e
171+
# button = swift.Button(buttoncb, 'button')
173172

174173

175-
radio = swift.Radio(radiocb, 'radio', [
176-
'on', 'tw', 'three'], 2)
174+
# def radiocb(e):
175+
# print(e)
176+
# select.value = e
177177

178-
# env.add(label)
179-
# env.add(slider)
180-
# env.add(button)
181-
# env.add(select)
182-
# env.add(radio)
183-
# env.add(check)
184178

185-
# while True:
186-
# env.step(0.05)
187-
# time.sleep(0.001)
179+
# radio = swift.Radio(radiocb, 'radio', [
180+
# 'on', 'tw', 'three'], 2)
188181

189-
# env.hold()
182+
# # env.add(label)
183+
# # env.add(slider)
184+
# # env.add(button)
185+
# # env.add(select)
186+
# # env.add(radio)
187+
# # env.add(check)
190188

189+
# # while True:
190+
# # env.step(0.05)
191+
# # time.sleep(0.001)
191192

192-
panda = rtb.models.Panda()
193-
panda.q = panda.qr
194-
# panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi/2)
195-
env.add(panda, show_robot=True)
193+
# # env.hold()
196194

197195

198-
ev = [0.01, 0, 0, 0, 0, 0]
199-
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
200-
env.step(0.001)
196+
# panda = rtb.models.Panda()
197+
# panda.q = panda.qr
198+
# # panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi/2)
199+
# env.add(panda, show_robot=True)
201200

202201

203-
def stepper():
204-
for i in range(10000):
205-
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
206-
# panda.fkine_all_fast(panda.q)
207-
env.step(0.001)
202+
# ev = [0.01, 0, 0, 0, 0, 0]
203+
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
204+
# env.step(0.001)
208205

209206

210-
# box = rtb.Box([0.8, 0.1, 0.1])
211-
# env.add(box)
207+
# def stepper():
208+
# for i in range(10000):
209+
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
210+
# # panda.fkine_all_fast(panda.q)
211+
# env.step(0.001)
212212

213-
# stepper()
214-
# env.remove(panda)
215213

214+
# # box = rtb.Box([0.8, 0.1, 0.1])
215+
# # env.add(box)
216216

217-
# r = rtb.models.LBR()
218-
# r.q = r.qr
219-
# r.qd = [0.01, 0.01, 0.01, 0, 0, 0, 0]
220-
# env.add(r)
221-
# for i in range(10000):
222-
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
223-
# env.step(0.001)
217+
# # stepper()
218+
# # env.remove(panda)
224219

225-
# # env.remove(box)
226-
cProfile.run('stepper()')
227220

228-
# env._run_thread = False
229-
# env.restart()
221+
# # r = rtb.models.LBR()
222+
# # r.q = r.qr
223+
# # r.qd = [0.01, 0.01, 0.01, 0, 0, 0, 0]
224+
# # env.add(r)
225+
# # for i in range(10000):
226+
# # panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
227+
# # env.step(0.001)
230228

231-
# r = rtb.models.LBR()
232-
# r.q = r.qr
233-
# r.qd = [0.01, 0.01, 0.01, 0, 0, 0, 0]
234-
# env.add(r)
235-
# for i in range(1000000):
236-
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
237-
# env.step(0.001)
229+
# # # env.remove(box)
230+
# cProfile.run('stepper()')
238231

239-
# env.reset()
240-
# env.add(r)
241-
# env.close()
232+
# # env._run_thread = False
233+
# # env.restart()
242234

243-
# cProfile.run('stepper()')
235+
# # r = rtb.models.LBR()
236+
# # r.q = r.qr
237+
# # r.qd = [0.01, 0.01, 0.01, 0, 0, 0, 0]
238+
# # env.add(r)
239+
# # for i in range(1000000):
240+
# # panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
241+
# # env.step(0.001)
242+
243+
# # env.reset()
244+
# # env.add(r)
245+
# # env.close()
244246

245-
# print(panda.fkine(panda.q))
246-
# print(panda.fkine_fast(panda.q))
247+
# # cProfile.run('stepper()')
247248

248-
env.hold()
249+
# # print(panda.fkine(panda.q))
250+
# # print(panda.fkine_fast(panda.q))
251+
252+
# env.hold()

‎roboticstoolbox/core/fknm.c

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,6 @@
1313
#include "fknm.h"
1414
#include <stdio.h>
1515

16-
17-
1816
// forward defines
1917
static PyObject *fkine_all(PyObject *self, PyObject *args);
2018
static PyObject *jacob0(PyObject *self, PyObject *args);
@@ -115,11 +113,13 @@ static PyObject *fkine_all(PyObject *self, PyObject *args)
115113
A(link, ret, q[link->jindex]);
116114
fk = (npy_float64 *)PyArray_DATA(link->fk);
117115

118-
if (link->parent) {
116+
if (link->parent)
117+
{
119118
pfk = (npy_float64 *)PyArray_DATA(link->parent->fk);
120119
mult(pfk, ret, fk);
121-
}
122-
else {
120+
}
121+
else
122+
{
123123
mult(base, ret, fk);
124124
}
125125
}
@@ -163,7 +163,7 @@ static PyObject *fkine(PyObject *self, PyObject *args)
163163
int n;
164164

165165
if (!PyArg_ParseTuple(
166-
args, "iOO!O!O!",
166+
args, "iOO!O!O!O!",
167167
&n,
168168
&links,
169169
&PyArray_Type, &py_q,

‎roboticstoolbox/robot/ELink.py

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -255,6 +255,16 @@ def __str__(self):
255255
parent = f" [{self.parent.name}]"
256256
return f"{name}[{self.name}({parent}): {self.ets()}] "
257257

258+
@property
259+
def fk(self):
260+
"""
261+
The forward kinemtics up to and including this link
262+
This value can be accessed after calling fkine_all(q)
263+
from the robot object.
264+
"""
265+
266+
return SE3(self._fk, check=False)
267+
258268
@property
259269
def v(self):
260270
"""

‎roboticstoolbox/robot/ERobot.py

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1020,11 +1020,6 @@ def fkine(
10201020
T = np.empty((4, 4))
10211021
fknm.fkine(m, path, q, etool, tool, T)
10221022

1023-
if tool is not None:
1024-
T2 = np.empty((4, 4))
1025-
fknm.compose(T, tool, T2)
1026-
T = T2
1027-
10281023
if not include_base:
10291024
return T
10301025
else:

‎tests/test_ERobot.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -168,7 +168,7 @@ def test_fkine_traj(self):
168168
def test_fkine_all(self):
169169
pm = rtb.models.DH.Panda()
170170
p = rtb.models.ETS.Panda()
171-
q = np.array([1, 2, 3, 4, 5, 6, 7])
171+
q = np.array([1.0, 2, 3, 4, 5, 6, 7])
172172
p.q = q
173173
pm.q = q
174174

0 commit comments

Comments
 (0)