Skip to content

Commit e6ccf32

Browse files
jhavljhavl
committed
pre-pybullet
1 parent 0146cf4 commit e6ccf32

File tree

15 files changed

+2776
-121
lines changed

15 files changed

+2776
-121
lines changed

‎examples/bookshelf.py

Lines changed: 364 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,71 @@
1010
import qpsolvers as qp
1111
import fcl
1212

13+
problems = np.array([
14+
[0, 1],
15+
[0, 2],
16+
[0, 3],
17+
[0, 4],
18+
[0, 5],
19+
[0, 6],
20+
[0, 7],
21+
[0, 8],
22+
[0, 9],
23+
[1, 2],
24+
[1, 3],
25+
[1, 4],
26+
[1, 5],
27+
[1, 6],
28+
[1, 7],
29+
[1, 8],
30+
[1, 9],
31+
[2, 3],
32+
[2, 4],
33+
[2, 5],
34+
[2, 6],
35+
[2, 7],
36+
[2, 8],
37+
[2, 9],
38+
[3, 4],
39+
[3, 5],
40+
[3, 6],
41+
[3, 7],
42+
[3, 8],
43+
[3, 9],
44+
[4, 5],
45+
[4, 6],
46+
[4, 7],
47+
[4, 8],
48+
[4, 9],
49+
[5, 6],
50+
[5, 7],
51+
[5, 8],
52+
[5, 9],
53+
[6, 7],
54+
[6, 8],
55+
[6, 9],
56+
[7, 8],
57+
[7, 9],
58+
[8, 9]])
59+
60+
qdmax = np.array([
61+
2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100,
62+
5000000, 5000000, 5000000, 5000000, 5000000, 5000000])
63+
lb = -qdmax
64+
ub = qdmax
65+
66+
q0 = [-0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173]
67+
q1 = [-0.1361, -0.1915, -1.2602, -0.8652, -2.8852, -0.7962, -2.039]
68+
q2 = [0.2341, -0.2138, -1.2602, -0.4709, -3.0149, -0.7505, -2.0164]
69+
q3 = [0.1584, 0.3429, -1.2382, -0.9829, -2.0892, -1.6126, -0.5582]
70+
q4 = [0.3927, 0.1763, -1.2382, -0.1849, -1.96, -1.4092, -1.0492]
71+
q5 = [-0.632, 0.5012, -1.2382, -0.8353, 2.2571, -0.1041, 0.3066]
72+
q6 = [0.1683, 0.7154, -0.4195, -1.0496, 2.4832, -0.6028, -0.6401]
73+
q7 = [-0.1198, 0.5299, -0.6291, -0.4348, 2.1715, -1.6403, 1.8299]
74+
q8 = [0.2743, 0.4088, -0.5291, -0.4304, 2.119, -1.9994, 1.7162]
75+
q9 = [0.2743, 0.4088, -0.5291, -0.4304, -0.9985, -1.0032, -1.7278]
76+
qs = [q0, q1, q2, q3, q4, q5, q6, q7, q8, q9]
77+
1378
s1 = rp.Shape.Box(
1479
scale=[0.60, 1.1, 0.02],
1580
base=sm.SE3(0.95, 0, 0.20))
@@ -34,15 +99,27 @@
3499
scale=[0.60, 0.02, 1.40],
35100
base=sm.SE3(0.95, -0.55, 0.7))
36101

102+
s0 = rp.Shape.Sphere(
103+
radius=0.05
104+
)
105+
37106
env = rp.backend.Swift()
38107
env.launch()
39108

40-
# panda = rp.models.Panda()
41-
# panda.q = panda.qr
42-
43109
r = rp.models.PR2()
110+
r.q = [
111+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
112+
0.0, 0.16825, 0.0, 0.0, 0.0, -0.5652894131595758, -0.1940789551546196,
113+
-1.260201738335192, -0.7895653603354864, -2.322747882942366,
114+
-0.3918504494615993, -2.5173485998351066, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
115+
0.0, 2.1347678577910827, 0.05595277251194286, 0.48032314980402596,
116+
-2.0802263633096487, 1.2294916701952125, -0.8773017824611689,
117+
2.932954218704465, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
118+
# r.base = sm.SE3(0.11382412910461426, 0.0, 0.0)
44119

45120
env.add(r)
121+
# env.add(r, show_robot=False, show_collision=True)
122+
env.add(s0)
46123
env.add(s1)
47124
env.add(s2)
48125
env.add(s3)
@@ -51,5 +128,289 @@
51128
env.add(s6)
52129
time.sleep(1)
53130

131+
# ETS number 43
132+
l0 = r.elinks['r_shoulder_pan_joint']
133+
134+
# ETS number 51
135+
l1 = r.elinks['r_wrist_roll_joint']
136+
l2 = r.elinks['r_gripper_joint']
137+
138+
i0 = 16
139+
i1 = i0 + 7
140+
links, n = r.get_path(l0, l1)
141+
142+
143+
def plane_int(t0, t1, ob):
144+
145+
point = ob.base.t
146+
normal = np.zeros(3)
147+
normal[np.argmin(ob.scale)] = 1
148+
plane = sm.geom3d.Plane.PN(point, normal)
149+
150+
line = sm.Plucker.PQ(t0, t1)
151+
152+
res = line.intersect_plane(plane)
153+
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
164+
165+
if t0[2] < ob.base.t[2]:
166+
return False
167+
168+
# print()
169+
# print(res.p)
170+
# print(t1)
171+
172+
return True
173+
# return True
174+
# else:
175+
# return False
176+
177+
178+
def shape(T, ob):
179+
# if not isinstance(ob, rp.Shape.Box):
180+
# return False
181+
182+
ret = True
183+
184+
# request = fcl.DistanceRequest()
185+
# result = fcl.DistanceResult()
186+
# ret = fcl.distance(link.collision[0].co, ob.co, request, result)
187+
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
191+
192+
# if ret != np.linalg.norm(lpTcp.t):
193+
# wTcp = sm.SE3(wTlp.t) * sm.SE3(0, 0, -ret)
194+
# lpTcp = wTlp.inv() * wTcp
195+
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
204+
205+
if not T.t[2] > ob.base.t[2]:
206+
return False
207+
208+
if not T.t[0] < ob.base.t[0]:
209+
return False
210+
211+
212+
213+
return ret
214+
# l = np.argmin(ob.scale)
215+
216+
217+
218+
219+
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)
235+
lpTcp = wTlp.inv() * wTcp
236+
237+
d = ret
238+
239+
if d < di:
240+
# print()
241+
# print(d)
242+
# print(np.linalg.norm(lpTcp.t))
243+
n = lpTcp.t / d
244+
nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
245+
246+
Je = r.jacobe(q, from_link=l0, to_link=link, offset=col.base)
247+
n = Je.shape[1]
248+
dp = nh @ ob.v
249+
l_Ain = np.zeros((1, 13))
250+
l_Ain[0, :n] = nh @ Je
251+
l_bin = (1 * (d - ds) / (di - ds)) + dp
252+
else:
253+
l_Ain = None
254+
l_bin = None
255+
256+
return l_Ain, l_bin, ret
257+
258+
259+
def servo(q0, q1, it):
260+
r.q[i0:i1] = q1
261+
r.fkine_all()
262+
tep = l2._fk.t
263+
264+
r.q[i0:i1] = q0
265+
r.qd = np.zeros(r.n)
266+
env.step(1)
267+
268+
Tep = r.fkine_graph(q1, l0, l1)
269+
270+
arrived = False
271+
i = 0
272+
Q = 0.1 * np.eye(n + 6)
273+
274+
# s0.base = sm.SE3(l1._fk.t) * sm.SE3.Tx(0.25) #r.fkine_graph(r.q[:i1], to_link=l1)
275+
s0.base = sm.SE3(tep)
276+
# s0.v = [-0.1, 0, 0, 0, 0, 0]
277+
278+
279+
while not arrived and i < it:
280+
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
284+
285+
eTep = r.fkine_graph(q, l0, l1).inv() * Tep
286+
e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi/180]))
287+
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))
290+
Aeq = np.c_[r.jacobe(q, l0, l1), np.eye(6)]
291+
beq = v.reshape((6,))
292+
Jm = r.jacobm(q, from_link=l0, to_link=l1).reshape(7,)
293+
c = np.r_[-Jm, np.zeros(6)]
294+
# c = np.zeros(13)
295+
296+
Ain = None
297+
bin = None
298+
299+
closest = 1000000
300+
j = 0
301+
for link in links:
302+
if link.jtype == link.VARIABLE:
303+
j += 1
304+
305+
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])
345+
beq = v.reshape((6,))
346+
Aeq = np.c_[r.jacob0(q, l0, l1), np.eye(6)]
347+
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
359+
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)
379+
r.qd[i0:i1] = qd[:n]
380+
381+
# print()
382+
# print(np.round(v, 2))
383+
# print(np.round(qd[7:], 2))
384+
385+
i += 1
386+
env.step(50)
387+
388+
return arrived
389+
390+
391+
it_max = 20000
392+
393+
probs = 45
394+
j = 0
395+
396+
for i in range(probs):
397+
print(problems[i, 0], problems[i, 1])
398+
ret = servo(qs[problems[i, 0]], qs[problems[i, 1]], 300)
399+
400+
print(ret)
401+
if ret:
402+
j += 1
403+
404+
print(j)
405+
54406

407+
# for i in range(10):
408+
# # for j in range(i+1, 10):
409+
# # print(i, j)
410+
# # print(servo(qs[i], qs[j], 100))
411+
# print(servo(qs[i], qs[9], it_max))
55412

413+
# 1 -> 9
414+
# 2 -> 9
415+
# 5 -> 9
416+
# 6 -> 9

0 commit comments

Comments
 (0)