collision updates

jhavl · jhavl · commit 9b71b62a60de · 2020-09-14T11:19:21.000+10:00
diff --git a/examples/Swift.py b/examples/Swift.py
@@ -22,20 +22,20 @@
 
 dt = 0.05
 
-env.record_start('file.webm')
+# env.record_start('file.webm')
 
-while not arrived:
+# while not arrived:
 
-    start = time.time()
-    v, arrived = rp.p_servo(panda.fkine(), Tep, 1.0)
-    panda.qd = np.linalg.pinv(panda.jacobe()) @ v
-    env.step(5)
-    stop = time.time()
+#     start = time.time()
+#     v, arrived = rp.p_servo(panda.fkine(), Tep, 1.0)
+#     panda.qd = np.linalg.pinv(panda.jacobe()) @ v
+#     env.step(5)
+#     stop = time.time()
 
-    if stop - start < dt:
-        time.sleep(dt - (stop - start))
+#     if stop - start < dt:
+#         time.sleep(dt - (stop - start))
 
-env.record_stop()
+# env.record_stop()
 
 # Uncomment to stop the plot from closing
 # env.hold()
diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py
@@ -221,17 +221,7 @@ def to_dict(self):
             #     })
 
             for gi in link.collision:
-                g_fk = link._fk * gi.base
-                if gi.scale is not None:
-                    scale = gi.scale.tolist()
-                else:
-                    scale = [1, 1, 1]
-                li['geometry'].append({
-                    'filename': gi.filename,
-                    'scale': scale,
-                    't': g_fk.t.tolist(),
-                    'q': r2q(g_fk.R).tolist()
-                })
+                li['geometry'].append(gi.to_dict(link._fk))
 
             ob['links'].append(li)
 
@@ -428,7 +418,7 @@ def gravity(self, gravity_new):
     def q(self, q_new):
         q_new = getvector(q_new, self.n)
         self._q = q_new
-        self.allfkine()
+        self.fkine_all()
 
     @qd.setter
     def qd(self, qd_new):
diff --git a/roboticstoolbox/robot/Shape.py b/roboticstoolbox/robot/Shape.py
@@ -5,6 +5,7 @@
 
 from spatialmath import SE3
 from spatialmath.base.argcheck import getvector
+from spatialmath.base import r2q
 
 try:
     import fcl
@@ -23,7 +24,8 @@ def __init__(
             length=0,
             scale=[1, 1, 1],
             filename=None,
-            co=None):
+            co=None,
+            stype=None):
 
         self.base = base
         self.primitive = primitive
@@ -32,6 +34,23 @@ def __init__(
         self.length = length
         self.filename = filename
         self.co = co
+        self.stype = stype
+
+    def to_dict(self, link_fk):
+        shape_fk = self.base * link_fk
+        # shape_fk = self.base
+
+        shape = {
+            'stype': self.stype,
+            'scale': self.scale.tolist(),
+            'filename': self.filename,
+            'radius': self.radius,
+            'length': self.length,
+            't': self.wT.t.tolist(),
+            'q': r2q(self.wT.R).tolist()
+        }
+
+        return shape
 
     @property
     def wT(self):
@@ -41,7 +60,7 @@ def wT(self):
     def wT(self, T):
         if not isinstance(T, SE3):
             T = SE3(T)
-        self._wT = self.base * T
+        self._wT = T * self.base
 
         if _fcl:
             tf = fcl.Transform(self._wT.R, self._wT.t)
@@ -67,6 +86,8 @@ def scale(self):
     def scale(self, value):
         if value is not None:
             value = getvector(value, 3)
+        else:
+            value = getvector([1, 1, 1], 3)
         self._scale = value
 
     @property
@@ -108,7 +129,8 @@ def Box(cls, scale, base=None):
         else:
             co = None
 
-        return cls(True, base=base, scale=scale, co=co)
+        return cls(
+            True, base=base, scale=scale, co=co, stype='box')
 
     @classmethod
     def Cylinder(cls, radius, length, base=None):
@@ -119,7 +141,9 @@ def Cylinder(cls, radius, length, base=None):
         else:
             co = None
 
-        return cls(True, base=base, radius=radius, length=length, co=co)
+        return cls(
+            True, base=base, radius=radius, length=length, co=co,
+            stype='cylinder')
 
     @classmethod
     def Sphere(cls, radius, base=None):
@@ -130,11 +154,12 @@ def Sphere(cls, radius, base=None):
         else:
             co = None
 
-        return cls(True, base=base, radius=radius, co=co)
+        return cls(True, base=base, radius=radius, co=co, stype='sphere')
 
     @classmethod
     def Mesh(cls, filename, base=None, scale=None):
-        return cls(False, filename=filename, base=base, scale=scale)
+        return cls(
+            False, filename=filename, base=base, scale=scale, stype='mesh')
 
 
 # class Mesh(Shape):
