collision progress

jhavl · jhavl · commit e32a52cbb8b9 · 2020-09-18T11:42:20.000+10:00
diff --git a/examples/collision.py b/examples/collision.py
@@ -15,23 +15,27 @@
 
 
 
-obj1 = fcl.Box(1, 1, 1)
-co1 = fcl.CollisionObject(obj1, fcl.Transform())
-obj2 = fcl.Box(1, 1, 1)
-co2 = fcl.CollisionObject(obj2, fcl.Transform())
+# obj1 = fcl.Box(1, 1, 1)
+# co1 = fcl.CollisionObject(obj1, fcl.Transform())
+# obj2 = fcl.Box(1, 1, 1)
+# co2 = fcl.CollisionObject(obj2, fcl.Transform())
 
-t1 = sm.SE3()
+t1 = sm.SE3() * sm.SE3.Rx(-1.2) * sm.SE3.Ry(0.8)
+# t2 = sm.SE3(10, 10, 10)
 t2 = sm.SE3(10, 2.7, 5.8) * sm.SE3.Rx(1.2) * sm.SE3.Ry(0.2) * sm.SE3.Rz(2.2)
+
+obj1 = rp.Shape.Cylinder(radius=0.5, length=1, base=t1)
+obj2 = rp.Shape.Cylinder(radius=0.5, length=1, base=t2)
 # t2 = sm.SE3(0, 10, 0) * sm.SE3.Rx(1.5)
 
-tf1 = fcl.Transform(t1.R, t1.t)
-co1.setTransform(tf1)
-tf2 = fcl.Transform(t2.R, t2.t)
-co2.setTransform(tf2)
+# tf1 = fcl.Transform(t1.R, t1.t)
+# co1.setTransform(tf1)
+# tf2 = fcl.Transform(t2.R, t2.t)
+# co2.setTransform(tf2)
 
 request = fcl.DistanceRequest()
 result = fcl.DistanceResult()
-ret = fcl.distance(co1, co2, request, result)
+ret = fcl.distance(obj1.co, obj2.co, request, result)
 print(ret)
 print(result.nearest_points)
 np1 = result.nearest_points[0]
diff --git a/examples/swift.py b/examples/swift.py
@@ -8,25 +8,96 @@
 import numpy as np
 import time
 import qpsolvers as qp
+import fcl
+
+
+def link_closest_point(links, ob):
+
+    c_ret = np.inf
+    c_res = None
+    c_base = None
+
+    for link in links:
+        for col in link.collision:
+            # col = link.collision[0]
+            request = fcl.DistanceRequest()
+            result = fcl.DistanceResult()
+            ret = fcl.distance(col.co, sphere.co, request, result)
+            if ret < c_ret:
+                c_ret = ret
+                c_res = result
+                c_base = col.base
+
+    # Take the link transform represented in the world frame
+    # Multiply it by the translation of the link frame to the nearest point
+    # The result is the closest point pose represented in the world frame
+    # where the closest point is represented with the links rotational frame
+    # rather than the links collision objects rotational frame
+    c_wTcp = link._fk * sm.SE3((c_base * sm.SE3(result.nearest_points[0])).t)
+
+    return c_ret, c_res, c_wTcp
+
+
+# def link_closest_point(link, ob):
+
+#     c_ret = np.inf
+#     c_res = None
+#     c_base = None
+#     c_type = None
+
+#     for col in link.collision:
+#         # col = link.collision[0]
+#         request = fcl.DistanceRequest()
+#         result = fcl.DistanceResult()
+#         ret = fcl.distance(col.co, sphere.co, request, result)
+#         if ret < c_ret:
+#             c_ret = ret
+#             c_res = result
+#             c_base = col.base
+#             c_type = col.stype
+
+#     # if c_type == 'cylinder':
+#     # print(result.nearest_points[0])
+
+#     # result.nearest_points[0][2] = 0
+
+#     # Take the link transform represented in the world frame
+#     # Multiply it by the translation of the link frame to the nearest point
+#     # The result is the closest point pose represented in the world frame
+#     # where the closest point is represented with the links rotational frame
+#     # rather than the links collision objects rotational frame
+#     c_wTcp = link._fk * sm.SE3((c_base * sm.SE3(result.nearest_points[0])).t)
+
+#     return c_ret, c_res, c_wTcp
+
+
+def closer(ret1, res1, ret2, res2):
+    if ret1 < ret2:
+        return ret1, res1
+    else:
+        return ret2, res2
+
 
 env = rp.backend.Swift()
 env.launch()
 
 panda = rp.models.Panda()
 panda.q = panda.qr
+# panda.q[4] += 0.1
 # Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
-Tep = panda.fkine() * sm.SE3.Tz(0.6)  * sm.SE3.Tx(-0.1) #* sm.SE3.Ty(-0.1)
+Tep = panda.fkine() * sm.SE3.Tz(0.6) * sm.SE3.Tx(-0.1)  # * sm.SE3.Ty(-0.1)
 
 sphere = rp.Shape.Sphere(0.05, sm.SE3(0.5, 0, 0.2))
+sphere.wT = sm.SE3()
 
 arrived = False
-env.add(panda, show_collision=False, show_robot=True)
+env.add(panda, show_collision=True, show_robot=False)
 env.add(sphere)
 time.sleep(1)
 
 dt = 0.05
 ps = 0.05
-pi = 0.9
+pi = 0.6
 
 # env.record_start('file.webm')
 
@@ -60,10 +131,20 @@
     beq = v.reshape((6,))
     c = np.r_[-panda.jacobm().reshape((panda.n,)), np.zeros(6)]
 
+
+    # Get closest link
+    linkA = panda._fkpath[-1]
+    linkB = panda._fkpath[-2]
+    linkC = panda._fkpath[-3]
+    panda.fkine_all()
+    retA, resA, wTcp = link_closest_point([linkA, linkB], sphere)
+    cpTc = wTcp.inv() * (sphere.base * sm.SE3(resA.nearest_points[1]))
+
+
+    d0 = np.linalg.norm(cpTc.t)
+    n0 = cpTc.t / d0
+
     # Distance Jacobian
-    eTc0 = panda.fkine().inv() * sphere.base
-    d0 = np.linalg.norm(eTc0.t)
-    n0 = eTc0.t / d0
     ds = 0.05
     di = 0.6
 
diff --git a/examples/test.py b/examples/test.py
@@ -0,0 +1,11 @@
+import roboticstoolbox as rp
+import numpy as np
+
+r = rp.models.Panda()
+
+for link in r._fkpath:
+    print(link.name)
+    print(link.ets)
+    print(link.jtype)
+
+print(np.round(r.jacob0(), 2))
diff --git a/roboticstoolbox/robot/ELink.py b/roboticstoolbox/robot/ELink.py
@@ -389,7 +389,6 @@ def A(self, q=None):
 
             tr = tr * T
 
-            
         return tr
 
     def islimit(self, q):
diff --git a/roboticstoolbox/robot/Shape.py b/roboticstoolbox/robot/Shape.py
@@ -56,7 +56,7 @@ def to_dict(self):
         }
 
         return shape
-        
+
     def fk_dict(self):
 
         if self.stype == 'cylinder':
@@ -71,6 +71,9 @@ def fk_dict(self):
 
         return shape
 
+    def __repr__(self):
+        return f'{self.stype},\n{self.base}'
+
     @property
     def wT(self):
         return self._wT
