added homogenous option for collisions

Author: jhavl

roboticstoolbox/examples/swift_benchmark.py

@@ -12,23 +12,37 @@
 env.launch()
 
 
-panda = rtb.models.Panda()
-panda.q = panda.qr
-env.add(panda, show_robot=True)
+# panda = rtb.models.Panda()
+# panda.q = panda.qr
+# env.add(panda, show_robot=True)
 
 
-ev = [0.01, 0, 0, 0, 0, 0]
-panda.qd = np.linalg.pinv(panda.jacobe(panda.q, fast=True)) @ ev
-env.step(0.001)
+# ev = [0.01, 0, 0, 0, 0, 0]
+# panda.qd = np.linalg.pinv(panda.jacobe(panda.q, fast=True)) @ ev
+# env.step(0.001)
 
 
-def stepper():
-    for i in range(10000):
-        panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
-        # panda.jacob0(panda.q, fast=True)
+# def stepper():
+#     for i in range(10000):
+#         panda.qd = np.linalg.pinv(panda.jacob0(panda.q, fast=True)) @ ev
+#         # panda.jacob0(panda.q, fast=True)
+
+#         # panda.fkine_all_fast(panda.q)
+#         env.step(0.001)
+
 
-        # panda.fkine_all_fast(panda.q)
-        env.step(0.001)
+import spatialgeometry as sg
+import spatialmath as sm
 
+b1 = sg.Box(scale=[0.2, 0.1, 0.2], base=sm.SE3(1.0, 1, 1))
+b2 = sg.Cylinder(radius=0.2, length=0.8, base=sm.SE3(0.0, 1, 1))
+
+env.add(b1)
+env.add(b2)
+
+def stepper():
+    for i in range(100000):
+        a, b, c = b1.closest_point(b2, inf_dist=4.0, homogenous=False)
 
+a, b, c = b1.closest_point(b2, inf_dist=4.0, homogenous=False)
 cProfile.run("stepper()")

roboticstoolbox/robot/ERobot.py

@@ -2029,10 +2029,10 @@ def link_collision_damper(
         bin = None
 
         def indiv_calculation(link, link_col, q):
-            d, wTlp, wTcp = link_col.closest_point(shape, di)
+            d, wTlp, wTcp = link_col.closest_point(shape, di, homogenous=False)
 
             if d is not None:
-                lpTcp = -wTlp[:3] + wTcp[:3]
+                lpTcp = -wTlp + wTcp
 
                 norm = lpTcp / d
                 norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)

roboticstoolbox/robot/Link.py

@@ -752,7 +752,7 @@ def G(self, G_new):
 
 # -------------------------------------------------------------------------- #
 
-    def closest_point(self, shape, inf_dist=1.0):
+    def closest_point(self, shape, inf_dist=1.0, homogenous=True):
         '''
         closest_point(shape, inf_dist) returns the minimum euclidean
         distance between this link and shape, provided it is less than
@@ -777,7 +777,7 @@ def closest_point(self, shape, inf_dist=1.0):
         p2 = None
 
         for col in self.collision:
-            td, tp1, tp2 = col.closest_point(shape, inf_dist)
+            td, tp1, tp2 = col.closest_point(shape, inf_dist, homogenous)
 
             if td is not None and td < d:
                 d = td