Skip to content

Commit ae2e830

Browse files
jhavljhavl
committed
pybullet implemented
1 parent e6ccf32 commit ae2e830

File tree

7 files changed

+595
-816
lines changed

7 files changed

+595
-816
lines changed

‎examples/bookshelf.py

Lines changed: 112 additions & 154 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import time
1010
import qpsolvers as qp
1111
import fcl
12+
import pybullet as p
1213

1314
problems = np.array([
1415
[0, 1],
@@ -103,6 +104,21 @@
103104
radius=0.05
104105
)
105106

107+
s = [s1, s2, s3, s4, s5, s6]
108+
109+
s0 = rp.Shape.Sphere(
110+
radius=0.05
111+
)
112+
113+
s00 = rp.Shape.Sphere(
114+
radius=0.05
115+
)
116+
117+
se = rp.Shape.Sphere(
118+
radius=0.02,
119+
base=sm.SE3(0.18, 0.01, 0)
120+
)
121+
106122
env = rp.backend.Swift()
107123
env.launch()
108124

@@ -119,6 +135,8 @@
119135

120136
env.add(r)
121137
# env.add(r, show_robot=False, show_collision=True)
138+
env.add(se)
139+
env.add(s00)
122140
env.add(s0)
123141
env.add(s1)
124142
env.add(s2)
@@ -151,95 +169,64 @@ def plane_int(t0, t1, ob):
151169

152170
res = line.intersect_plane(plane)
153171

154-
if res is None:
155-
return False
156-
157-
if res.p[0] < (ob.base.t[0] - (ob.scale[0] / 2.0)) or \
158-
res.p[0] > (ob.base.t[0] + (ob.scale[0] / 2.0)):
159-
return False
160-
161-
# if res.p[1] < (ob.base.t[1] - (ob.scale[1] / 2.0)) or \
162-
# res.p[1] > (ob.base.t[1] + (ob.scale[1] / 2.0)):
163-
# return False
172+
off = 0.1
164173

165-
if t0[2] < ob.base.t[2]:
166-
return False
167-
168-
# print()
169-
# print(res.p)
170-
# print(t1)
174+
if normal[2]:
175+
if res is None:
176+
return False
171177

172-
return True
173-
# return True
174-
# else:
175-
# return False
178+
if res.p[0] < (ob.base.t[0] - (ob.scale[0] / 2.0) - off) or \
179+
res.p[0] > (ob.base.t[0] + (ob.scale[0] / 2.0) + off):
180+
return False
176181

182+
if res.p[1] < (ob.base.t[1] - (ob.scale[1] / 2.0) - off) or \
183+
res.p[1] > (ob.base.t[1] + (ob.scale[1] / 2.0) + off):
184+
return False
177185

178-
def shape(T, ob):
179-
# if not isinstance(ob, rp.Shape.Box):
180-
# return False
186+
above0 = t0[2] > ob.base.t[2]
187+
above1 = t1[2] > ob.base.t[2]
181188

182-
ret = True
189+
if above0 and above1 or (not above0 and not above1):
190+
return False
183191

184-
# request = fcl.DistanceRequest()
185-
# result = fcl.DistanceResult()
186-
# ret = fcl.distance(link.collision[0].co, ob.co, request, result)
192+
return True
193+
elif normal[1]:
194+
if res is None:
195+
return False
187196

188-
# wTlp = link.collision[0].base * sm.SE3(result.nearest_points[0])
189-
# wTcp = ob.base * sm.SE3(result.nearest_points[1])
190-
# lpTcp = wTlp.inv() * wTcp
197+
if res.p[0] < (ob.base.t[0] - (ob.scale[0] / 2.0) - off) or \
198+
res.p[0] > (ob.base.t[0] + (ob.scale[0] / 2.0) + off):
199+
return False
191200

192-
# if ret != np.linalg.norm(lpTcp.t):
193-
# wTcp = sm.SE3(wTlp.t) * sm.SE3(0, 0, -ret)
194-
# lpTcp = wTlp.inv() * wTcp
201+
if res.p[2] < (ob.base.t[2] - (ob.scale[2] / 2.0) - off) or \
202+
res.p[2] > (ob.base.t[2] + (ob.scale[2] / 2.0) + off):
203+
return False
195204

196-
# if not wTlp.t[2] > wTcp.t[2]:
197-
# return False
198-
199-
# print()
200-
# print(wTlp.t)
201-
# print(wTcp.t)
202-
# if not wTlp.t[0] > wTcp.t[0]:
203-
# return False
205+
above0 = t0[1] > ob.base.t[1]
206+
above1 = t1[1] > ob.base.t[1]
204207

205-
if not T.t[2] > ob.base.t[2]:
206-
return False
208+
if above0 and above1 or (not above0 and not above1):
209+
return False
207210

208-
if not T.t[0] < ob.base.t[0]:
211+
return True
212+
else:
209213
return False
210214

211215

216+
def link_calc(link, col, ob, q):
217+
di = 0.3
218+
ds = 0.02
212219

213-
return ret
214-
# l = np.argmin(ob.scale)
215-
216-
217-
218-
220+
ret = p.getClosestPoints(col.co, ob.co, di)
219221

220-
def link_calc(link, col, ob, q, norm):
221-
di = 0.16
222-
ds = 0.05
223-
224-
request = fcl.DistanceRequest()
225-
result = fcl.DistanceResult()
226-
ret = fcl.distance(col.co, ob.co, request, result)
227-
228-
wTlp = col.base * sm.SE3(result.nearest_points[0])
229-
wTcp = ob.base * sm.SE3(result.nearest_points[1])
230-
lpTcp = wTlp.inv() * wTcp
231-
232-
if ret != np.linalg.norm(lpTcp.t):
233-
wTcp = sm.SE3(wTlp.t) * sm.SE3(ret * norm)
234-
# wTcp = sm.SE3(wTlp.t) * sm.SE3(0, 0, -ret)
222+
if len(ret) > 0:
223+
ret = ret[0]
224+
wTlp = sm.SE3(ret[5])
225+
wTcp = sm.SE3(ret[6])
235226
lpTcp = wTlp.inv() * wTcp
236227

237-
d = ret
228+
d = ret[8]
238229

239-
if d < di:
240-
# print()
241-
# print(d)
242-
# print(np.linalg.norm(lpTcp.t))
243230
n = lpTcp.t / d
244231
nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
245232

@@ -252,8 +239,10 @@ def link_calc(link, col, ob, q, norm):
252239
else:
253240
l_Ain = None
254241
l_bin = None
242+
d = 1000
243+
wTcp = None
255244

256-
return l_Ain, l_bin, ret
245+
return l_Ain, l_bin, d, wTcp
257246

258247

259248
def servo(q0, q1, it):
@@ -262,6 +251,7 @@ def servo(q0, q1, it):
262251
tep = l2._fk.t
263252

264253
r.q[i0:i1] = q0
254+
r.fkine_all()
265255
r.qd = np.zeros(r.n)
266256
env.step(1)
267257

@@ -271,117 +261,84 @@ def servo(q0, q1, it):
271261
i = 0
272262
Q = 0.1 * np.eye(n + 6)
273263

274-
# s0.base = sm.SE3(l1._fk.t) * sm.SE3.Tx(0.25) #r.fkine_graph(r.q[:i1], to_link=l1)
275264
s0.base = sm.SE3(tep)
276-
# s0.v = [-0.1, 0, 0, 0, 0, 0]
277-
278265

279266
while not arrived and i < it:
280267
q = r.q[i0:i1]
281-
v, arrived = rp.p_servo(r.fkine_graph(q, l0, l1), Tep, 0.5)
282-
# v = np.array([-0.1, 0, 0, 0, 0, 0])
283-
# v[2] = 0
268+
v, arrived = rp.p_servo(r.fkine_graph(q, l0, l1), Tep, 1, 0.25)
269+
se._wT = l1._fk
284270

285271
eTep = r.fkine_graph(q, l0, l1).inv() * Tep
286272
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi/180]))
287273

288-
Q[n:, n:] = 200 * (1 / e) * np.eye(6)
289-
# [1/100, 1/100, 1/100, 1/100, 1/100, 1/100] * (100 * np.eye(6))
274+
Q[n:, n:] = (1 / e) * np.eye(6)
290275
Aeq = np.c_[r.jacobe(q, l0, l1), np.eye(6)]
291276
beq = v.reshape((6,))
292277
Jm = r.jacobm(q, from_link=l0, to_link=l1).reshape(7,)
293278
c = np.r_[-Jm, np.zeros(6)]
294-
# c = np.zeros(13)
295279

296280
Ain = None
297281
bin = None
298282

299283
closest = 1000000
284+
closest_obj = None
285+
closest_p = None
300286
j = 0
301287
for link in links:
302288
if link.jtype == link.VARIABLE:
303289
j += 1
304290

305291
for col in link.collision:
306-
l_Ain, l_bin, ret = link_calc(link, col, s3, q[:j], np.array([0, 0, -1]))
307-
if ret < closest:
308-
closest = ret
309-
310-
if l_Ain is not None and l_bin is not None:
311-
if Ain is None:
312-
Ain = l_Ain
313-
else:
314-
Ain = np.r_[Ain, l_Ain]
315-
316-
if bin is None:
317-
bin = np.array(l_bin)
318-
else:
319-
bin = np.r_[bin, l_bin]
320-
321-
j = 0
322-
for link in links:
323-
if link.jtype == link.VARIABLE:
324-
j += 1
325-
326-
for col in link.collision:
327-
l_Ain, l_bin, ret = link_calc(link, col, s6, q[:j], np.array([0, -1, 0]))
328-
if ret < closest:
329-
closest = ret
330-
331-
if l_Ain is not None and l_bin is not None:
332-
if Ain is None:
333-
Ain = l_Ain
334-
else:
335-
Ain = np.r_[Ain, l_Ain]
336-
337-
if bin is None:
338-
bin = np.array(l_bin)
339-
else:
340-
bin = np.r_[bin, l_bin]
341-
342-
# if closest < 0.2 and shape(l2._fk, s3):
343-
if plane_int(l2._fk.t, tep, s3):
344-
v = np.array([-0.1, 0, 0, 0, 0, 0])
292+
for obj in s:
293+
l_Ain, l_bin, ret, _ = link_calc(link, col, obj, q[:j])
294+
295+
if l_Ain is not None and l_bin is not None:
296+
if Ain is None:
297+
Ain = l_Ain
298+
else:
299+
Ain = np.r_[Ain, l_Ain]
300+
301+
if bin is None:
302+
bin = np.array(l_bin)
303+
else:
304+
bin = np.r_[bin, l_bin]
305+
306+
for obj in s:
307+
l_Ain, l_bin, ret, wTcp = link_calc(l1, se, obj, r.q[i0:i1])
308+
if ret < closest:
309+
closest = ret
310+
closest_obj = obj
311+
closest_p = wTcp
312+
313+
if l_Ain is not None and l_bin is not None:
314+
if Ain is None:
315+
Ain = l_Ain
316+
else:
317+
Ain = np.r_[Ain, l_Ain]
318+
319+
if bin is None:
320+
bin = np.array(l_bin)
321+
else:
322+
bin = np.r_[bin, l_bin]
323+
324+
s00.base = closest_p
325+
326+
if plane_int(se.wT.t, tep, closest_obj):
327+
# v = np.array([-0.1, 0, 0, 0, 0, 0])
328+
v[0] = -0.4
329+
v[3:] /= 10
330+
# v[2] /= 10
345331
beq = v.reshape((6,))
346332
Aeq = np.c_[r.jacob0(q, l0, l1), np.eye(6)]
347333

348-
# plane_int(l2._fk.t, tep, s3)
349-
350-
# j = 0
351-
# for link in links:
352-
# if link.jtype == link.VARIABLE:
353-
# j += 1
354-
355-
# for col in link.collision:
356-
# l_Ain, l_bin, ret = link_calc(link, col, s0, q[:j], np.array([0, -1, 0]))
357-
# if ret < closest:
358-
# closest = ret
334+
try:
335+
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
336+
except (ValueError, TypeError):
337+
print("Value Error")
338+
break
359339

360-
# if l_Ain is not None and l_bin is not None:
361-
# if Ain is None:
362-
# Ain = l_Ain
363-
# else:
364-
# Ain = np.r_[Ain, l_Ain]
365-
366-
# if bin is None:
367-
# bin = np.array(l_bin)
368-
# else:
369-
# bin = np.r_[bin, l_bin]
370-
371-
# print(ret)
372-
# if ret < 0.15:
373-
# beq[0] += -0.1
374-
# print(np.round(beq, 2))
375-
# print(closest)
376-
377-
378-
qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
379340
r.qd[i0:i1] = qd[:n]
380341

381-
# print()
382-
# print(np.round(v, 2))
383-
# print(np.round(qd[7:], 2))
384-
385342
i += 1
386343
env.step(50)
387344

@@ -395,11 +352,12 @@ def servo(q0, q1, it):
395352

396353
for i in range(probs):
397354
print(problems[i, 0], problems[i, 1])
398-
ret = servo(qs[problems[i, 0]], qs[problems[i, 1]], 300)
355+
ret = servo(qs[problems[i, 0]], qs[problems[i, 1]], 500)
399356

400357
print(ret)
401358
if ret:
402359
j += 1
360+
print(j)
403361

404362
print(j)
405363

‎examples/collision.py

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -79,18 +79,20 @@
7979

8080

8181
c1 = p.createCollisionShape(shapeType=p.GEOM_SPHERE, radius=0.1)
82-
c2 = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=[0.1, 0.1, 0.1])
82+
c2 = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=[1, 0.5, 0.1])
8383

8484
b1 = p.createMultiBody(baseMass=1,
8585
baseInertialFramePosition=[0, 0, 0],
8686
baseCollisionShapeIndex=c1,
87-
basePosition=[0, 0, 1],
87+
basePosition=[0, 0, 0],
8888
useMaximalCoordinates=True)
8989

90+
# p.resetBasePositionAndOrientation(b1, [0, 0, 1], [1, 0, 0, 0])
91+
9092
b2 = p.createMultiBody(baseMass=1,
9193
baseInertialFramePosition=[0, 0, 0],
9294
baseCollisionShapeIndex=c2,
93-
basePosition=[1, 0, 1],
95+
basePosition=[0, 0, 1],
9496
useMaximalCoordinates=True)
9597

9698
print(p.getClosestPoints(b1, b2, 2))

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