8000 added to reqs · oridong/robotics-toolbox-python@e2d8777 · GitHub
[go: up one dir, main page]
Skip to content

Commit e2d8777

Browse files
jhavljhavl
committed
added to reqs
1 parent d8d5130 commit e2d8777

File tree

4 files changed

+29
-243
lines changed

4 files changed

+29
-243
lines changed

‎examples/swift2.py

Lines changed: 19 additions & 238 deletions
Original file line numberDiff line numberDiff line change
@@ -4,256 +4,37 @@
44
"""
55

66
from roboticstoolbox.backends import swift
7-
from math import pi
87
import roboticstoolbox as rtb
9-
from spatialmath import SO3, SE3
108
import spatialmath as sm
119
import numpy as np
12-
import pathlib
13-
import os
14-
import time
15-
import cProfile
1610

17-
# r = rtb.models.DH.Panda()
18-
r = rtb.models.Panda()
19-
r.q = r.qr
11+
env = swift.Swift()
12+
env.launch()
2013

21-
T = r.fkine([1, 2, 3, 4, 5, 6, 7])
2214

23-
r.ikine_LM(T, q0=r.q)
15+
# ur = rtb.models.UR5()
16+
# ur.base = sm.SE3(0.3, 1, 0) * sm.SE3.Rz(np.pi / 2)
17+
# ur.q = [-0.4, -np.pi / 2 - 0.3, np.pi / 2 + 0.3, -np.pi / 2, -np.pi / 2, 0]
18+
# env.add(ur)
2419

20+
# lbr = rtb.models.LBR()
21+
# lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi / 2)
22+
# lbr.q = lbr.qr
23+
# env.add(lbr)
2524

26-
# # import fknm
27-
# # import numpy as np
28-
# # import spatialmath as sm
29-
# # import cProfile
30-
# # import roboticstoolbox as rtb
31-
32-
# # l = rtb.ELink(ets=rtb.ETS.rx(1.0), v=rtb.ETS.rz(flip=True))
33-
34-
# # # print(l.isjoint)
35-
# # # print(l._v.isflip)
36-
# # # print(l._v.axis)
37-
# # # print(l._Ts.A)
38-
39-
# # c = fknm.link_init(l.isjoint, l._v.isflip, 2, l._Ts.A)
40-
41-
# # arr = np.empty((4, 4))
42-
# # fknm.link_A(1.0, c, arr)
43-
# # print(arr)
44-
# # print(l.A(1))
45-
46-
# # # l._Ts.A[1, 1] = 99
47-
# # # l._isjoint = 2
48-
# # # fknm.link_update(c, l.isjoint, l._v.isflip, 2, l._Ts.A)
49-
50-
# # # fknm.link_A(c, arr, 1.0)
51-
# # # print(arr)
52-
53-
# # # print(l.A(1, True))
54-
55-
56-
# # # a = np.empty((4, 4))
57-
58-
# # # fknm.rz(2, np.array([1.0, 2.0]))
59-
# # # fknm.rz(np.pi, a)
60-
# # # print(sm.base.trotz(np.pi))
61-
62-
# # # print(a)
63-
# # # sm.base.trotz(1)
64-
65-
66-
# # def Rz(eta):
67-
# # arr = np.empty((4, 4))
68-
# # fknm.link_A(eta, c, arr)
69-
# # return arr
70-
71-
72-
# # def cc(it):
73-
# # for i in range(it):
74-
# # # arr = np.empty((4, 4))
75-
# # # fknm.rz(i, arr)
76-
# # a = Rz(i)
77-
78-
79-
# # def slow(it):
80-
# # for i in range(it):
81-
# # a = l.A(i)
82-
83-
84-
# # it = 10000
85-
# # cProfile.run('cc(it)')
86-
# # cProfile.run('slow(it)')
87-
88-
89-
# from spatialmath.base import r2q
90-
91-
92-
# # pm = rtb.models.DH.Panda()
93-
# # p = rtb.models.ETS.Panda()
94-
# # p2 = rtb.models.Panda()
95-
# # q = np.array([1.0, 2, 3, 4, 5, 6, 7])
96-
# # # q = np.ones(7)
97-
# # p.q = q
98-
# # pm.q = q
99-
100-
# # p.fkine_all(q)
101-
# # p2.fkine_all(q)
102-
# # r2 = pm.fkine_all(q)
103-
104-
# # for i in range(7):
105-
# # print(np.round(p.links[i]._fk, 2))
106-
# # # print(np.round(p2.links[i]._fk, 2))
107-
# # print(np.round(r2[i].A, 2))
108-
109-
# # print()
110-
# # print()
111-
112-
113-
# # print(np.round(p.fkine(p.qz, end=p.links[1], fast=True), 2))
114-
115-
# # import swift
116-
117-
118-
# # num = 500000
119-
# # b = np.random.randn(num)
120-
# # sm.base.trotz(1.0)
121-
122-
# # def stepper():
123-
# # for i in range(num):
124-
# # sm.base.trotz(b[i])
125-
126-
127-
# # cProfile.run('stepper()')
128-
129-
# # ur = rtb.models.UR5()
130-
# # ur.base = sm.SE3(0.3, 1, 0) * sm.SE3.Rz(np.pi/2)
131-
# # ur.q = [-0.4, -np.pi/2 - 0.3, np.pi/2 + 0.3, -np.pi/2, -np.pi/2, 0]
132-
# # env.add(ur)
133-
134-
# # lbr = rtb.models.LBR()
135-
# # lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi/2)
136-
# # lbr.q = lbr.qr
137-
# # env.add(lbr)
138-
139-
# # k = rtb.models.KinovaGen3()
140-
# # k.q = k.qr
141-
# # k.q[0] = np.pi + 0.15
142-
# # k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi/2)
143-
# # env.add(k)
144-
145-
# env = swift.Swift(_dev=False)
146-
# env.launch()
147-
148-
149-
# def slidercb(e):
150-
# print(e)
151-
152-
153-
# def selectcb(e):
154-
# print(e)
155-
156-
157-
# def checkcb(e):
158-
# print(e)
159-
# # select.value = e
160-
161-
162-
# label = swift.Label('Demo')
163-
# slider = swift.Slider(slidercb, 10, 95, 5, 15, 'slider this is', ' d')
164-
# select = swift.Select(selectcb, 'selec', [
165-
# 'on', 'tw', 'three'], 2)
166-
167-
# check = swift.Checkbox(checkcb, 'checkbox', [
168-
# 'on', 'tw', 'three'], [False, True, True])
169-
170-
171-
# def buttoncb(e):
172-
# print('BUTTON')
173-
# # check.checked = [True, True, False]
174-
# # check.desc = 'new desc'
175-
# slider.value = 60
176-
177-
178-
# button = swift.Button(buttoncb, 'button')
179-
180-
181-
# def radiocb(e):
182-
# print(e)
183-
# select.value = e
184-
185-
186-
# radio = swift.Radio(radiocb, 'radio', [
187-
# 'on', 'tw', 'three'], 2)
188-
189-
# # env.add(label)
190-
# # env.add(slider)
191-
# # env.add(button)
192-
# # env.add(select)
193-
# # env.add(radio)
194-
# # env.add(check)
195-
196-
# # while True:
197-
# # env.step(0.05)
198-
# # time.sleep(0.001)
199-
200-
# # env.hold()
201-
25+
# k = rtb.models.KinovaGen3()
26+
# k.q = k.qr
27+
# k.q[0] = np.pi + 0.15
28+
# k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi / 2)
29+
# env.add(k)
20230

20331
# panda = rtb.models.Panda()
20432
# panda.q = panda.qr
205-
# # panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi/2)
33+
# panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi / 2)
20634
# env.add(panda, show_robot=True)
20735

36+
r = rtb.models.YuMi()
37+
env.add(r)
20838

209-
# ev = [0.01, 0, 0, 0, 0, 0]
210-
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
211-
# env.step(0.001)
212-
213-
214-
# def stepper():
215-
# for i in range(10000):
216-
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
217-
# # panda.fkine_all_fast(panda.q)
218-
# env.step(0.001)
219-
220-
221-
# # box = rtb.Box([0.8, 0.1, 0.1])
222-
# # env.add(box)
223-
224-
# # stepper()
225-
# # env.remove(panda)
226-
227-
228-
# # r = rtb.models.LBR()
229-
# # r.q = r.qr
230-
# # r.qd = [0.01, 0.01, 0.01, 0, 0, 0, 0]
231-
# # env.add(r)
232-
# # for i in range(10000):
233-
# # panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
234-
# # env.step(0.001)
235-
236-
# # # env.remove(box)
237-
# cProfile.run('stepper()')
238-
239-
# # env._run_thread = False
240-
# # env.restart()
241-
242-
# # r = rtb.models.LBR()
243-
# # r.q = r.qr
244-
# # r.qd = [0.01, 0.01, 0.01, 0, 0, 0, 0]
245-
# # env.add(r)
246-
# # for i in range(1000000):
247-
# # panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
248-
# # env.step(0.001)
249-
250-
# # env.reset()
251-
# # env.add(r)
252-
# # env.close()
253-
254-
# # cProfile.run('stepper()')
255-
256-
# # print(panda.fkine(panda.q))
257-
# # print(panda.fkine_fast(panda.q))
25839

259-
# env.hold()
40+
env.hold()

‎roboticstoolbox/models/URDF/KinovaGen3.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ class KinovaGen3(ERobot):
3131

3232
def __init__(self):
3333

34-
elinks, name = super().urdf_to_ets_args(
34+
elinks, name = self.URDF_read(
3535
"kortex_description/robots/gen3.xacro")
3636

3737
super().__init__(
@@ -42,13 +42,13 @@ def __init__(self):
4242
)
4343

4444
# self.qdlim = np.array([
45-
# 2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 3.0, 3.0])
45+
# 2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 3.0, 3.0])
4646

4747
self.addconfiguration("qz", np.array(
4848
[0, 0, 0, 0, 0, 0, 0]))
4949

5050
self.addconfiguration("qr", np.array(
51-
[np.pi, -0.3, 0, -1.6, 0, -1.0, np.pi/2]))
51+
[np.pi, -0.3, 0, -1.6, 0, -1.0, np.pi / 2]))
5252

5353

5454
if __name__ == '__main__': # pragma nocover

‎roboticstoolbox/robot/ERobot.py

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2198,9 +2198,13 @@ def teach(
21982198

21992199
robot = ERobot([
22002200
ELink(ETS.rz(), name='link1'),
2201-
ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(), name='link2', parent='link1'),
2201+
ELink(
2202+
ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(),
2203+
name='link2', parent='link1'),
22022204
ELink(ETS.tx(1), name='ee_1', parent='link2'),
2203-
ELink(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(), name='link3', parent='link1'),
2205+
ELink(
2206+
ETS.tx(1) * ETS.ty(0.5) * ETS.rz(),
2207+
name='link3', parent='link1'),
22042208
ELink(ETS.tx(1), name='ee_2', parent='link3')
22052209
])
22062210
print(robot)

‎setup.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
req = [
88
'numpy>=1.18.0',
99
'spatialmath-python>=0.9.2',
10+
'spatialgeometry>=0.1.0',
1011
'pgraph-python',
1112
'scipy',
1213
'matplotlib',

0 commit comments

Comments
 (0)
0