9
9
import time
10
10
import qpsolvers as qp
11
11
import fcl
12
+ import pybullet as p
12
13
13
14
problems = np .array ([
14
15
[0 , 1 ],
103
104
radius = 0.05
104
105
)
105
106
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
+
106
122
env = rp .backend .Swift ()
107
123
env .launch ()
108
124
119
135
120
136
env .add (r )
121
137
# env.add(r, show_robot=False, show_collision=True)
138
+ env .add (se )
139
+ env .add (s00 )
122
140
env .add (s0 )
123
141
env .add (s1 )
124
142
env .add (s2 )
@@ -151,95 +169,64 @@ def plane_int(t0, t1, ob):
151
169
152
170
res = line .intersect_plane (plane )
153
171
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
164
173
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
171
177
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
176
181
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
177
185
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 ]
181
188
182
- ret = True
189
+ if above0 and above1 or (not above0 and not above1 ):
190
+ return False
183
191
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
187
196
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
191
200
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
195
204
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 ]
204
207
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
207
210
208
- if not T .t [0 ] < ob .base .t [0 ]:
211
+ return True
212
+ else :
209
213
return False
210
214
211
215
216
+ def link_calc (link , col , ob , q ):
217
+ di = 0.3
218
+ ds = 0.02
212
219
213
- return ret
214
- # l = np.argmin(ob.scale)
215
-
216
-
217
-
218
-
220
+ ret = p .getClosestPoints (col .co , ob .co , di )
219
221
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 ])
235
226
lpTcp = wTlp .inv () * wTcp
236
227
237
- d = ret
228
+ d = ret [ 8 ]
238
229
239
- if d < di :
240
- # print()
241
- # print(d)
242
- # print(np.linalg.norm(lpTcp.t))
243
230
n = lpTcp .t / d
244
231
nh = np .expand_dims (np .r_ [n , 0 , 0 , 0 ], axis = 0 )
245
232
@@ -252,8 +239,10 @@ def link_calc(link, col, ob, q, norm):
252
239
else :
253
240
l_Ain = None
254
241
l_bin = None
242
+ d = 1000
243
+ wTcp = None
255
244
256
- return l_Ain , l_bin , ret
245
+ return l_Ain , l_bin , d , wTcp
257
246
258
247
259
248
def servo (q0 , q1 , it ):
@@ -262,6 +251,7 @@ def servo(q0, q1, it):
262
251
tep = l2 ._fk .t
263
252
264
253
r .q [i0 :i1 ] = q0
254
+ r .fkine_all ()
265
255
r .qd = np .zeros (r .n )
266
256
env .step (1 )
267
257
@@ -271,117 +261,84 @@ def servo(q0, q1, it):
271
261
i = 0
272
262
Q = 0.1 * np .eye (n + 6 )
273
263
274
- # s0.base = sm.SE3(l1._fk.t) * sm.SE3.Tx(0.25) #r.fkine_graph(r.q[:i1], to_link=l1)
275
264
s0 .base = sm .SE3 (tep )
276
- # s0.v = [-0.1, 0, 0, 0, 0, 0]
277
-
278
265
279
266
while not arrived and i < it :
280
267
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
284
270
285
271
eTep = r .fkine_graph (q , l0 , l1 ).inv () * Tep
286
272
e = np .sum (np .abs (np .r_ [eTep .t , eTep .rpy () * np .pi / 180 ]))
287
273
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 )
290
275
Aeq = np .c_ [r .jacobe (q , l0 , l1 ), np .eye (6 )]
291
276
beq = v .reshape ((6 ,))
292
277
Jm = r .jacobm (q , from_link = l0 , to_link = l1 ).reshape (7 ,)
293
278
c = np .r_ [- Jm , np .zeros (6 )]
294
- # c = np.zeros(13)
295
279
296
280
Ain = None
297
281
bin = None
298
282
299
283
closest = 1000000
284
+ closest_obj = None
285
+ closest_p = None
300
286
j = 0
301
287
for link in links :
302
288
if link .jtype == link .VARIABLE :
303
289
j += 1
304
290
305
291
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
345
331
beq = v .reshape ((6 ,))
346
332
Aeq = np .c_ [r .jacob0 (q , l0 , l1 ), np .eye (6 )]
347
333
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
359
339
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
340
r .qd [i0 :i1 ] = qd [:n ]
380
341
381
- # print()
382
- # print(np.round(v, 2))
383
- # print(np.round(qd[7:], 2))
384
-
385
342
i += 1
386
343
env .step (50 )
387
344
@@ -395,11 +352,12 @@ def servo(q0, q1, it):
395
352
396
353
for i in range (probs ):
397
354
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 )
399
356
400
357
print (ret )
401
358
if ret :
402
359
j += 1
360
+ print (j )
403
361
404
362
print (j )
405
363
0 commit comments