New geometry strategy

jhavl · jhavl · commit 66a04d14aac3 · 2020-09-14T15:02:26.000+10:00
diff --git a/examples/Swift.py b/examples/Swift.py
@@ -17,23 +17,23 @@
 Tep = panda.fkine() * sm.SE3.Tx(-0.2) * sm.SE3.Ty(0.2) * sm.SE3.Tz(0.2)
 
 arrived = False
-env.add(panda)
+env.add(panda, show_collision=True)
 time.sleep(1)
 
 dt = 0.05
 
 # 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()
 
diff --git a/roboticstoolbox/backend/Swift/Swift.py b/roboticstoolbox/backend/Swift/Swift.py
@@ -90,7 +90,7 @@ def close(self):
     #  Methods to interface with the robots created in other environemnts
     #
 
-    def add(self, ob):
+    def add(self, ob, show_robot=True, show_collision=False):
         '''
         id = add(robot) adds the robot to the external environment. robot must
         be of an appropriate class. This adds a robot object to a list of
@@ -102,6 +102,8 @@ def add(self, ob):
 
         if isinstance(ob, rp.ETS):
             robot = ob.to_dict()
+            robot['show_robot'] = show_robot
+            robot['show_collision'] = show_collision
             id = self.swift.robot(robot)
             self.robots.append(ob)
             return id
diff --git a/roboticstoolbox/models/xacro/franka_description/robots/panda_arm.urdf.xacro b/roboticstoolbox/models/xacro/franka_description/robots/panda_arm.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
   <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
-  <xacro:panda_arm safety_distance="0.03"/>
+  <xacro:panda_arm safety_distance="0.00"/>
 </robot>
diff --git a/roboticstoolbox/models/xacro/franka_description/robots/panda_arm_hand.urdf.xacro b/roboticstoolbox/models/xacro/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -2,6 +2,6 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
   <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
   <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
-  <xacro:panda_arm safety_distance="0.03"/>
-  <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8" safety_distance="0.03"/>
+  <xacro:panda_arm safety_distance="0.00"/>
+  <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8" safety_distance="0.00"/>
 </robot>
diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py
@@ -198,30 +198,18 @@ def to_dict(self):
                 'eta': [],
                 'q_idx': link.q_idx,
                 'geometry': [],
-                'collision': [],
-                't': link._fk.t.tolist(),
-                'q': r2q(link._fk.R).tolist()
+                'collision': []
             }
 
             for et in link.ets:
                 li['axis'].append(et.axis)
                 li['eta'].append(et.eta)
 
-            # for gi in link.geometry:
-            #     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()
-            #     })
+            for gi in link.geometry:
+                li['geometry'].append(gi.to_dict())
 
             for gi in link.collision:
-                li['geometry'].append(gi.to_dict(link._fk))
+                li['collision'].append(gi.to_dict())
 
             ob['links'].append(li)
 
@@ -238,10 +226,16 @@ def fk_dict(self):
         for link in self.ets:
 
             li = {
-                't': link._fk.t.tolist(),
-                'q': r2q(link._fk.R).tolist()
+                'geometry': [],
+                'collision': []
             }
 
+            for gi in link.geometry:
+                li['geometry'].append(gi.fk_dict())
+
+            for gi in link.collision:
+                li['collision'].append(gi.fk_dict())
+
             ob['links'].append(li)
 
         return ob
@@ -582,6 +576,9 @@ def fkine_all(self, q=None):
             for col in self.ets[i].collision:
                 col.wT = self.ets[i]._fk
 
+            for gi in self.ets[i].geometry:
+                gi.wT = self.ets[i]._fk
+
     def jacob0(self, q=None):
         """
         J0 = jacob0(q) is the manipulator Jacobian matrix which maps joint
@@ -947,8 +944,8 @@ def scollision(self):
                                 print(l1.name + ' -> ' + l2.name)
                                 # import code
                                 # code.interact(local=dict(globals(), **locals()))
-                                print(ob1)
-                                print(ob2)
+                                # print(ob1)
+                                # print(ob2)
                             # print(ret)
 
                 
diff --git a/roboticstoolbox/robot/Shape.py b/roboticstoolbox/robot/Shape.py
@@ -6,6 +6,7 @@
 from spatialmath import SE3
 from spatialmath.base.argcheck import getvector
 from spatialmath.base import r2q
+import numpy as np
 
 try:
     import fcl
@@ -27,27 +28,45 @@ def __init__(
             co=None,
             stype=None):
 
+        self.co = co
         self.base = base
+        self.wT = None
         self.primitive = primitive
         self.scale = scale
         self.radius = radius
         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
+    def to_dict(self):
+
+        if self.stype == 'cylinder':
+            fk = self.wT * SE3.Rx(np.pi/2)
+        else:
+            fk = self.wT
 
         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()
+            't': fk.t.tolist(),
+            'q': r2q(fk.R).tolist()
+        }
+
+        return shape
+        
+    def fk_dict(self):
+
+        if self.stype == 'cylinder':
+            fk = self.wT * SE3.Rx(np.pi/2)
+        else:
+            fk = self.wT
+
+        shape = {
+            't': fk.t.tolist(),
+            'q': r2q(fk.R).tolist()
         }
 
         return shape
@@ -62,9 +81,9 @@ def wT(self, T):
             T = SE3(T)
         self._wT = T * self.base
 
-        if _fcl:
+        if _fcl and self.co is not None:
             tf = fcl.Transform(self._wT.R, self._wT.t)
-            # self.co.setTransform(tf)
+            self.co.setTransform(tf)
 
     @property
     def base(self):
