Skip to content

Commit ae2e830

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