8000 collisions working · ctc-eng/robotics-toolbox-python@43eb84d · GitHub
[go: up one dir, main page]
Skip to content
10000

Commit 43eb84d

Browse files
jhavljhavl
committed
collisions working
1 parent 3c65a4a commit 43eb84d

File tree

4 files changed

+140
-30
lines changed

4 files changed

+140
-30
lines changed

‎examples/swift.py

Lines changed: 85 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -10,35 +10,57 @@
1010
import qpsolvers as qp
1111
import fcl
1212

13+
ta = 0
14+
tb = 0
15+
count = 0
16+
1317

1418
def link_calc(link, col, ob):
15-
di = 0.25
19+
di = 0.35
1620
ds = 0.05
1721

22+
t1 = time.time()
23+
1824
request = fcl.DistanceRequest()
1925
result = fcl.DistanceResult()
20-
ret = fcl.distance(col.co, sphere.co, request, result)
26+
ret = fcl.distance(col.co, ob.co, request, result)
27+
2128

2229
# wTlp = link._fk * sm.SE3((col.base * sm.SE3(result.nearest_points[0])).t)
2330
wTlp = col.base * sm.SE3(result.nearest_points[0])
2431
wTcp = ob.base * sm.SE3(result.nearest_points[1])
2532
lpTcp = wTlp.inv() * wTcp
2633

34+
t2 = time.time()
35+
2736
d = ret
2837

38+
global ta, tb, count
39+
2940
if d < di:
41+
count += 1
3042

3143
n = lpTcp.t / d
3244
nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
3345

3446
Je = panda.jacobe(to_link=link, offset=col.base)
3547
n = Je.shape[1]
36-
l_Ain = np.c_[nh @ Je, np.zeros((1, 13 - n))]
37-
l_bin = 0.1 * (d - ds) / (di - ds)
48+
dp = nh @ ob.v
49+
l_Ain = np.zeros((1, 13))
50+
l_Ain[0, :n] = nh @ Je
51+
# = np.c_[nh @ Je, np.zeros((1, 13 - n))]
52+
l_bin = (100.1 * (d - ds) / (di - ds)) + dp
3853
else:
3954
l_Ain = None
4055
l_bin = None
4156

57+
t3 = time.time()
58+
59+
60+
61+
ta += t2 - t1
62+
tb += t3 - t2
63+
4264
return l_Ain, l_bin
4365

4466

@@ -79,7 +101,6 @@ def link_closest_point(links, ob):
79101
return c_ret, c_res, lpTcp
80102

81103

82-
83104
env = rp.backend.Swift()
84105
env.launch()
85106

@@ -89,21 +110,27 @@ def link_closest_point(links, ob):
89110
# Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
90111
Tep = panda.fkine() * sm.SE3.Tz(0.6) * sm.SE3.Tx(-0.1) # * sm.SE3.Ty(-0.1)
91112

92-
sphere = rp.Shape.Sphere(0.05, sm.SE3(0.5, 0, 0.2))
93-
sphere.wT = sm.SE3()
94-
sphere.v = [-0.01, 0, 0.01, 0, 0, 0]
113+
114+
s1 = rp.Shape.Sphere(0.05, sm.SE3(0.5, 0, 0.2))
115+
s2 = rp.Shape.Sphere(0.05, sm.SE3(0.2, 0.35, 0.5))
116+
s3 = rp.Shape.Sphere(0.05, sm.SE3(0.45, -0.25, 0.1))
117+
s1.v = [-0.08, 0.2, 0.08, 0, 0, 0]
118+
s2.v = [0, -0.25, 0, 0, 0, 0]
119+
s3.v = [0, 0.1, 0, 0, 0, 0]
95120

96121
arrived = False
97-
env.add(panda, show_collision=True, show_robot=False)
98-
env.add(sphere)
122+
# env.add(panda, show_collision=True, show_robot=False)
123+
env.add(panda)
124+
env.add(s1)
125+
env.add(s2)
126+
env.add(s3)
99127
time.sleep(1)
100128

101129
dt = 0.05
102130
ps = 0.05
103131
pi = 0.6
104132

105133
# env.record_start('file.webm')
106-
107134
while not arrived:
108135

109136
v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
@@ -125,7 +152,7 @@ def link_closest_point(links, ob):
125152
n = 7
126153
Q = np.eye(n + 6)
127154
Q[:n, :n] *= Y
128-
Q[n:, n:] = (1 / e) * np.eye(6)
155+
Q[n:, n:] = 100 * (1 / e) * np.eye(6)
129156
Aeq = np.c_[panda.jacobe(), np.eye(6)]
130157
beq = v.reshape((6,))
131158
Jm = panda.jacobm().reshape((panda.n,))
@@ -142,26 +169,41 @@ def link_closest_point(links, ob):
142169
if j >= 0:
143170
l_groups[j].append(link)
144171

145-
146-
147172
# Distance Jacobian
148173
Ain = None
149174
bin = None
150175

151-
152176
# Get closest link
153177
linkA = panda._fkpath[-1]
154178
linkB = panda._fkpath[-2]
155179
linkC = panda._fkpath[-3]
156180
panda.fkine_all()
181+
links = panda._fkpath[1:]
157182

183+
t0 = time.time()
184+
185+
count = 0
186+
ta = 0
187+
tb = 0
158188

159-
links = panda._fkpath[2:]
189+
for link in links:
190+
for col in link.collision:
191+
l_Ain, l_bin = link_calc(link, col, s1)
192+
193+
if l_Ain is not None and l_bin is not None:
194+
if Ain is None:
195+
Ain = l_Ain
196+
else:
197+
Ain = np.r_[Ain, l_Ain]
198+
199+
if bin is None:
200+
bin = np.array(l_bin)
201+
else:
202+
bin = np.r_[bin, l_bin]
160203

161-
t0 = time.time()
162204
for link in links:
163205
for col in link.collision:
164-
l_Ain, l_bin = link_calc(link, col, sphere)
206+
l_Ain, l_bin = link_calc(link, col, s2)
165207

166208
if l_Ain is not None and l_bin is not None:
167209
if Ain is None:
@@ -174,6 +216,27 @@ def link_closest_point(links, ob):
174216
else:
175217
bin = np.r_[bin, l_bin]
176218

219+
for link in links:
220+
for col in link.collision:
221+
l_Ain, l_bin = link_calc(link, col, s3)
222+
223+
if l_Ain is not None and l_bin is not None:
224+
if Ain is None:
225+
Ain = l_Ain
226+
else:
227+
Ain = np.r_[Ain, l_Ain]
228+
229+
if bin is None:
230+
bin = np.array(l_bin)
231+
else:
232+
bin = np.r_[bin, l_bin]
233+
234+
235+
print(count)
236+
print(ta)
237+
print(tb)
238+
239+
177240
# retA, resA, lpTcp = link_closest_point([linkA], sphere)
178241
# # cpTc = wTcp.inv() * (sphere.base * sm.SE3(resA.nearest_points[1]))
179242

@@ -207,4 +270,7 @@ def link_closest_point(links, ob):
207270
else:
208271
panda.qd = qd[:panda.n]
209272

210-
env.step(25)
273+
env.step(20)
274+
275+
print(ta/count)
276+
print(tb/count)

‎examples/test2.py

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
import roboticstoolbox as rp
2+
import spatialmath as sm
3+
import numpy as np
4+
import time
5+
6+
import cProfile
7+
panda = rp.models.Panda()
8+
panda.q = panda.qr
9+
cProfile.run('panda.jacobe()')
10+
11+
12+
# env = rp.backend.Swift()
13+
# env.launch()
14+
15+
# panda = rp.models.Panda()
16+
# panda.q = panda.qr
17+
18+
# Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
19+
20+
# arrived = False
21+
# env.add(panda)
22+
23+
# dt = 0.05
24+
25+
# while not arrived:
26+
27+
28+
# start = time.time()
29+
# panda.fkine_all()
30+
# v, arrived = rp.p_servo(panda.fkine(), Tep, 1)
31+
# panda.qd = np.r_[np.linalg.pinv(panda.jacobe()) @ v, [0, 0]]
32+
# env.step(50)
33+
# stop = time.time()
34+
35+
# print(panda.manipulability())
36+
37+
# if stop - start < dt:
38+
# time.sleep(dt - (stop - start))

‎roboticstoolbox/backend/Swift/Swift.py

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -56,11 +56,10 @@ def step(self, dt=50):
5656
super().step
5757

5858
self._step_robots(dt)
59+
self._step_shapes(dt)
5960

6061
# self._draw_ellipses()
61-
self._draw_robots()
62-
63-
# self._update_robots()
62+
self._draw_all()
6463

6564
def reset(self):
6665
'''
@@ -162,11 +161,14 @@ def _step_shapes(self, dt):
162161

163162
shape.base = sm.SE3(t) * sm.SE3.RPY(r)
164163

165-
def _draw_robots(self):
164+
def _draw_all(self):
166165

167166
for i in range(len(self.robots)):
168167
self.robots[i].fkine_all()
169-
self.swift.poses([i, self.robots[i].fk_dict()])
168+
self.swift.robot_poses([i, self.robots[i].fk_dict()])
169+
170+
for i in range(len(self.shapes)):
171+
self.swift.shape_poses([i, self.shapes[i].fk_dict()])
170172

171173
def record_start(self, file):
172174
self.swift.record_start(file)

‎roboticstoolbox/robot/Shape.py

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ def __init__(
2828
co=None,
2929
stype=None):
3030

31+
self._wT = SE3()
3132
self.co = co
3233
self.base = base
3334
self.wT = None
@@ -76,6 +77,11 @@ def fk_dict(self):
7677
def __repr__(self):
7778
return f'{self.stype},\n{self.base}'
7879

80+
def _update_fcl(self):
81+
if _fcl and self.co is not None:
82+
tf = fcl.Transform(self.wT.R, self.wT.t)
83+
self.co.setTransform(tf)
84+
7985
@property
8086
def v(self):
8187
return self._v
@@ -86,17 +92,14 @@ def v(self, value):
8692

8793
@property
8894
def wT(self):
89-
return self._wT
95+
return self._wT * self.base
9096

9197
@wT.setter
9298
def wT(self, T):
9399
if not isinstance(T, SE3):
94100
T = SE3(T)
95-
self._wT = T * self.base
96-
97-
if _fcl and self.co is not None:
98-
tf = fcl.Transform(self._wT.R, self._wT.t)
99-
self.co.setTransform(tf)
101+
self._wT = T
102+
self._update_fcl()
100103

101104
@property
102105
def base(self):
@@ -151,6 +154,7 @@ def base(self, T):
151154
if not isinstance(T, SE3):
152155
T = SE3(T)
153156
self._base = T
157+
self._update_fcl()
154158

155159
@classmethod
156160
def Box(cls, scale, base=None):

0 commit comments

Comments
 (0)
0