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