8000 added homogenous option for collisions · A905275/robotics-toolbox-python@bc3f2cc · GitHub
[go: up one dir, main page]
Skip to content

Commit bc3f2cc

Browse files
jhavljhavl
committed
added homogenous option for collisions
1 parent 0c365cc commit bc3f2cc

File tree

3 files changed

+30
-16
lines changed

3 files changed

+30
-16
lines changed

‎roboticstoolbox/examples/swift_benchmark.py

Lines changed: 26 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -12,23 +12,37 @@
1212
env.launch()
1313

1414

15-
panda = rtb.models.Panda()
16-
panda.q = panda.qr
17-
env.add(panda, show_robot=True)
15+
# panda = rtb.models.Panda()
16+
# panda.q = panda.qr
17+
# env.add(panda, show_robot=True)
1818

1919

20-
ev = [0.01, 0, 0, 0, 0, 0]
21-
panda.qd = np.linalg.pinv(panda.jacobe(panda.q, fast=True)) @ ev
22-
env.step(0.001)
20+
# ev = [0.01, 0, 0, 0, 0, 0]
21+
# panda.qd = np.linalg.pinv(panda.jacobe(panda.q, fast=True)) @ ev
22+
# env.step(0.001)
2323

2424

25-
def stepper():
26-
for i in range(10000):
27-
panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
28-
# panda.jacob0(panda.q, fast=True)
25+
# def stepper():
26+
# for i in range(10000):
27+
# panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
28+
# # panda.jacob0(panda.q, fast=True)
29+
30+
# # panda.fkine_all_fast(panda.q)
31+
# env.step(0.001)
32+
2933

30-
# panda.fkine_all_fast(panda.q)
31-
env.step(0.001)
34+
import spatialgeometry as sg
35+
import spatialmath as sm
3236

37+
b1 = sg.Box(scale=[0.2, 0.1, 0.2], base=sm.SE3(1.0, 1, 1))
38+
b2 = sg.Cylinder(radius=0.2, length=0.8, base=sm.SE3(0.0, 1, 1))
39+
40+
env.add(b1)
41+
env.add(b2)
42+
43+
def stepper():
44+
for i in range(100000):
45+
a, b, c = b1.closest_point(b2, inf_dist=4.0, homogenous=False)
3346

47+
a, b, c = b1.closest_point(b2, inf_dist=4.0, homogenous=False)
3448
cProfile.run("stepper()")

‎roboticstoolbox/robot/ERobot.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2029,10 +2029,10 @@ def link_collision_damper(
20292029
bin = None
20302030

20312031
def indiv_calculation(link, link_col, q):
2032-
d, wTlp, wTcp = link_col.closest_point(shape, di)
2032+
d, wTlp, wTcp = link_col.closest_point(shape, di, homogenous=False)
20332033

20342034
if d is not None:
2035-
lpTcp = -wTlp[:3] + wTcp[:3]
2035+
lpTcp = -wTlp + wTcp
20362036

20372037
norm = lpTcp / d
20382038
norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)

‎roboticstoolbox/robot/Link.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -752,7 +752,7 @@ def G(self, G_new):
752752

753753
# -------------------------------------------------------------------------- #
754754

755-
def closest_point(self, shape, inf_dist=1.0):
755+
def closest_point(self, shape, inf_dist=1.0, homogenous=True):
756756
'''
757757
closest_point(shape, inf_dist) returns the minimum euclidean
758758
distance between this link and shape, provided it is less than
@@ -777,7 +777,7 @@ def closest_point(self, shape, inf_dist=1.0):
777777
p2 = None
778778

779779
for col in self.collision:
780-
td, tp1, tp2 = col.closest_point(shape, inf_dist)
780+
td, tp1, tp2 = col.closest_point(shape, inf_dist, homogenous)
781781

782782
if td is not None and td < d:
783783
d = td

0 commit comments

Comments
 (0)
0