Progress

jhavl · jhavl · commit 844347c71b66 · 2020-09-16T18:04:33.000+10:00
diff --git a/examples/Swift.py b/examples/Swift.py
@@ -17,7 +17,7 @@
 Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
 
 arrived = False
-env.add(panda, show_collision=True)
+env.add(panda, show_collision=True, show_robot=False)
 time.sleep(1)
 
 dt = 0.05
diff --git a/examples/collision.py b/examples/collision.py
@@ -6,8 +6,41 @@
 import roboticstoolbox as rp
 import spatialmath as sm
 import numpy as np
+import fcl
 
-r = rp.models.Panda()
-r.q = r.qr
+# r = rp.models.Panda()
+# r.q = r.qr
 
-r.scollision()
+# r.scollision()
+
+
+
+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()
+t2 = sm.SE3(10, 2.7, 5.8) * sm.SE3.Rx(1.2) * sm.SE3.Ry(0.2) * sm.SE3.Rz(2.2)
+# 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)
+
+request = fcl.DistanceRequest()
+result = fcl.DistanceResult()
+ret = fcl.distance(co1, co2, request, result)
+print(ret)
+print(result.nearest_points)
+np1 = result.nearest_points[0]
+np2 = result.nearest_points[1]
+
+aTp1 = (t1 * sm.SE3(np1)).t
+bTp2 = (t2 * sm.SE3(np2)).t
+
+p1Tp2 = bTp2 - aTp1
+
+d = np.linalg.norm(p1Tp2)
+print(d)
diff --git a/roboticstoolbox/models/xacro/franka_description/robots/panda_arm.xacro b/roboticstoolbox/models/xacro/franka_description/robots/panda_arm.xacro
@@ -267,7 +267,7 @@
       <collision>
         <origin xyz="0 0 0.08" rpy="0 0 0"/>
         <geometry>
-          <sphere radius="${0.04+safety_distance}"  />
+          <sphere radius="${0.025+safety_distance}"  />
         </geometry>
       </collision>
       <collision>
diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py
@@ -927,25 +927,70 @@ def jacob0v(self, q=None):
 
     def scollision(self):
         i = 0
-        for l1 in self.ets:
-            for ob1 in l1.collision:
-                for l2 in self.ets:
-                    # print(l2.name)
-                    if l1.name != l2.name and \
-                       any([l2.name != li.name for li in l1.parent]) and \
-                       any([l2.name != li.name for li in l1.child]):
-                        for ob2 in l2.collision:
-                            # i += 1
-                            # print(i)
-                            request = fcl.CollisionRequest()
-                            result = fcl.CollisionResult()
-                            ret = fcl.collide(ob1.co, ob2.co, request, result)
-                            if ret:
-                                print(l1.name + ' -> ' + l2.name)
-                                # import code
-                                # code.interact(local=dict(globals(), **locals()))
-                                # print(ob1)
-                                # print(ob2)
+
+        l1 = self.ets[1]
+        ob1 = l1.collision[0]
+
+        for l2 in self.ets:
+            print(l2.name)
+            if l1.name != l2.name and \
+                    any([l2.name != li.name for li in l1.parent]) and \
+                    any([l2.name != li.name for li in l1.child]):
+                ob2 = l2.collision[0]
+                request = fcl.DistanceRequest()
+                result = fcl.DistanceResult()
+                ret = fcl.distance(ob1.co, ob2.co, request, result)
+                print(ret)
+                # print(result.nearest_points)
+                np1 = result.nearest_points[0]
+                np2 = result.nearest_points[1]
+
+                aTb = (ob1.wT.inv() * ob2.wT).t
+                bTa = (ob1.wT.inv() * ob2.wT).inv().t
+
+                bTp = -(aTb - np1)
+                aTp = -(bTa - np2)
+
+
+
+                # t1 = ob1.wT.t + np1
+                # t2 = ob2.wT.t + np2
+                # d = np.linalg.norm(bTp - aTp)
+                d = np.linalg.norm(np1 - np2)
+                print(d)
+
+
+        # for l1 in self.ets:
+        #     print(l1.name)
+        #     for ob1 in l1.collision:
+                
+        #         print(ob1.stype)
+        #         print(ob1.wT)
+
+
+                # for l2 in self.ets:
+                #     # print(l2.name)
+                #     if l1.name != l2.name and \
+                #        any([l2.name != li.name for li in l1.parent]) and \
+                #        any([l2.name != li.name for li in l1.child]):
+                #         for ob2 in l2.collision:
+                #             # i += 1
+                #             # print(i)
+                #             request = fcl.CollisionRequest()
+                #             result = fcl.CollisionResult()
+                #             ret = fcl.collide(ob1.co, ob2.co, request, result)
+                #             if ret:
+                #                 request = fcl.DistanceRequest()
+                #                 result = fcl.DistanceResult()
+                #                 ret = fcl.distance(ob1.co, ob2.co, request, result)
+                #                 print(l1.name + ' -> ' + l2.name)
+                #                 print(ret)
+                #                 print(result.nearest_points)
+
+                #                 # import code
+                #                 # code.interact(local=dict(globals(), **locals()))
+                #                 # print(ob1)
+                #                 # print(ob2)
                             # print(ret)
 
                 
