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