8000 collision progress · ctc-eng/robotics-toolbox-python@e32a52c · GitHub
[go: up one dir, main page]
Skip to content
10000

Commit e32a52c

Browse files
jhavljhavl
committed
collision progress
1 parent 77eb32f commit e32a52c

File tree

5 files changed

+116
-18
lines changed

5 files changed

+116
-18
lines changed

‎examples/collision.py

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -15,23 +15,27 @@
1515

1616

1717

18-
obj1 = fcl.Box(1, 1, 1)
19-
co1 = fcl.CollisionObject(obj1, fcl.Transform())
20-
obj2 = fcl.Box(1, 1, 1)
21-
co2 = fcl.CollisionObject(obj2, fcl.Transform())
18+
# obj1 = fcl.Box(1, 1, 1)
19+
# co1 = fcl.CollisionObject(obj1, fcl.Transform())
20+
# obj2 = fcl.Box(1, 1, 1)
21+
# co2 = fcl.CollisionObject(obj2, fcl.Transform())
2222

23-
t1 = sm.SE3()
23+
t1 = sm.SE3() * sm.SE3.Rx(-1.2) * sm.SE3.Ry(0.8)
24+
# t2 = sm.SE3(10, 10, 10)
2425
t2 = sm.SE3(10, 2.7, 5.8) * sm.SE3.Rx(1.2) * sm.SE3.Ry(0.2) * sm.SE3.Rz(2.2)
26+
27+
obj1 = rp.Shape.Cylinder(radius=0.5, length=1, base=t1)
28+
obj2 = rp.Shape.Cylinder(radius=0.5, length=1, base=t2)
2529
# t2 = sm.SE3(0, 10, 0) * sm.SE3.Rx(1.5)
2630

27-
tf1 = fcl.Transform(t1.R, t1.t)
28-
co1.setTransform(tf1)
29-
tf2 = fcl.Transform(t2.R, t2.t)
30-
co2.setTransform(tf2)
31+
# tf1 = fcl.Transform(t1.R, t1.t)
32+
# co1.setTransform(tf1)
33+
# tf2 = fcl.Transform(t2.R, t2.t)
34+
# co2.setTransform(tf2)
3135

3236
request = fcl.DistanceRequest()
3337
result = fcl.DistanceResult()
34-
ret = fcl.distance(co1, co2, request, result)
38+
ret = fcl.distance(obj1.co, obj2.co, request, result)
3539
print(ret)
3640
print(result.nearest_points)
3741
np1 = result.nearest_points[0]

‎examples/swift.py

Lines changed: 87 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,25 +8,96 @@
88
import numpy as np
99
import time
1010
import qpsolvers as qp
11+
import fcl
12+
13+
14+
def link_closest_point(links, ob):
15+
16+
c_ret = np.inf
17+
c_res = None
18+
c_base = None
19+
20+
for link in links:
21+
for col in link.collision:
22+
# col = link.collision[0]
23+
request = fcl.DistanceRequest()
24+
result = fcl.DistanceResult()
25+
ret = fcl.distance(col.co, sphere.co, request, result)
26+
if ret < c_ret:
27+
c_ret = ret
28+
c_res = result
29+
c_base = col.base
30+
31+
# Take the link transform represented in the world frame
32+
# Multiply it by the translation of the link frame to the nearest point
33+
# The result is the closest point pose represented in the world frame
34+
# where the closest point is represented with the links rotational frame
35+
# rather than the links collision objects rotational frame
36+
c_wTcp = link._fk * sm.SE3((c_base * sm.SE3(result.nearest_points[0])).t)
37+
38+
return c_ret, c_res, c_wTcp
39+
40+
41+
# def link_closest_point(link, ob):
42+
43+
# c_ret = np.inf
44+
# c_res = None
45+
# c_base = None
46+
# c_type = None
47+
48+
# for col in link.collision:
49+
# # col = link.collision[0]
50+
# request = fcl.DistanceRequest()
51+
# result = fcl.DistanceResult()
52+
# ret = fcl.distance(col.co, sphere.co, request, result)
53+
# if ret < c_ret:
54+
# c_ret = ret
55+
# c_res = result
56+
# c_base = col.base
57+
# c_type = col.stype
58+
59+
# # if c_type == 'cylinder':
60+
# # print(result.nearest_points[0])
61+
62+
# # result.nearest_points[0][2] = 0
63+
64+
# # Take the link transform represented in the world frame
65+
# # Multiply it by the translation of the link frame to the nearest point
66+
# # The result is the closest point pose represented in the world frame
67+
# # where the closest point is represented with the links rotational frame
68+
# # rather than the links collision obj B41A ects rotational frame
69+
# c_wTcp = link._fk * sm.SE3((c_base * sm.SE3(result.nearest_points[0])).t)
70+
71+
# return c_ret, c_res, c_wTcp
72+
73+
74+
def closer(ret1, res1, ret2, res2):
75+
if ret1 < ret2:
76+
return ret1, res1
77+
else:
78+
return ret2, res2
79+
1180

1281
env = rp.backend.Swift()
1382
env.launch()
1483

1584
panda = rp.models.Panda()
1685
panda.q = panda.qr
86+
# panda.q[4] += 0.1
1787
# Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
18-
Tep = panda.fkine() * sm.SE3.Tz(0.6) * sm.SE3.Tx(-0.1) #* sm.SE3.Ty(-0.1)
88+
Tep = panda.fkine() * sm.SE3.Tz(0.6) * sm.SE3.Tx(-0.1) # * sm.SE3.Ty(-0.1)
1989

2090
sphere = rp.Shape.Sphere(0.05, sm.SE3(0.5, 0, 0.2))
91+
sphere.wT = sm.SE3()
2192

2293
arrived = False
23-
env.add(panda, show_collision=False, show_robot=True)
94+
env.add(panda, show_collision=True, show_robot=False)
2495
env.add(sphere)
2596
time.sleep(1)
2697

2798
dt = 0.05
2899
ps = 0.05
29-
pi = 0.9
100+
pi = 0.6
30101

31102
# env.record_start('file.webm')
32103

@@ -60,10 +131,20 @@
60131
beq = v.reshape((6,))
61132
c = np.r_[-panda.jacobm().reshape((panda.n,)), np.zeros(6)]
62133

134+
135+
# Get closest link
136+
linkA = panda._fkpath[-1]
137+
linkB = panda._fkpath[-2]
138+
linkC = panda._fkpath[-3]
139+
panda.fkine_all()
140+
retA, resA, wTcp = link_closest_point([linkA, linkB], sphere)
141+
cpTc = wTcp.inv() * (sphere.base * sm.SE3(resA.nearest_points[1]))
142+
143+
144+
d0 = np.linalg.norm(cpTc.t)
145+
n0 = cpTc.t / d0
146+
63147
# Distance Jacobian
64-
eTc0 = panda.fkine().inv() * sphere.base
65-
d0 = np.linalg.norm(eTc0.t)
66-
n0 = eTc0.t / d0
67148
ds = 0.05
68149
di = 0.6
69150

‎examples/test.py

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
import roboticstoolbox as rp
2+
import numpy as np
3+
4+
r = rp.models.Panda()
5+
6+
for link in r._fkpath:
7+
print(link.name)
8+
print(link.ets)
9+
print(link.jtype)
10+
11+
print(np.round(r.jacob0(), 2))

‎roboticstoolbox/robot/ELink.py

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -389,7 +389,6 @@ def A(self, q=None):
389389

390390
tr = tr * T
391391

392-
393392
return tr
394393

395394
def islimit(self, q):

‎roboticstoolbox/robot/Shape.py

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ def to_dict(self):
5656
}
5757

5858
return shape
59-
59+
6060
def fk_dict(self):
6161

6262
if self.stype == 'cylinder':
@@ -71,6 +71,9 @@ def fk_dict(self):
7171

7272
return shape
7373

74+
def __repr__(self):
75+
return f'{self.stype},\n{self.base}'
76+
7477
@property
7578
def wT(self):
7679
return self._wT

0 commit comments

Comments
 (0)
0