fix neo example

jhavl · jhavl · commit 92489f87a89b · 2022-05-11T22:52:08.000+10:00
diff --git a/roboticstoolbox/examples/neo.py b/roboticstoolbox/examples/neo.py
@@ -25,16 +25,16 @@
 n = 7
 
 # Make two obstacles with velocities
-s0 = sg.Sphere(radius=0.05, base=sm.SE3(0.52, 0.4, 0.3))
+s0 = sg.Sphere(radius=0.05, pose=sm.SE3(0.52, 0.4, 0.3))
 s0.v = [0, -0.2, 0, 0, 0, 0]
 
-s1 = sg.Sphere(radius=0.05, base=sm.SE3(0.1, 0.35, 0.65))
+s1 = sg.Sphere(radius=0.05, pose=sm.SE3(0.1, 0.35, 0.65))
 s1.v = [0, -0.2, 0, 0, 0, 0]
 
 collisions = [s0, s1]
 
 # Make a target
-target = sg.Sphere(radius=0.02, base=sm.SE3(0.6, -0.2, 0.0))
+target = sg.Sphere(radius=0.02, pose=sm.SE3(0.6, -0.2, 0.0))
 
 # Add the Panda and shapes to the simulator
 env.add(panda)
@@ -44,7 +44,7 @@
 
 # Set the desired end-effector pose to the location of target
 Tep = panda.fkine(panda.q)
-Tep.A[:3, 3] = target.base.t
+Tep.A[:3, 3] = target.T[:3, -1]
 # Tep.A[2, 3] += 0.1
 
 
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1447,7 +1447,7 @@ def URDF_read(file_path, tld=None, xacro_tld=None):
 
     # --------------------------------------------------------------------- #
 
-    def get_path(self, end=None, start=None, _fknm=False):
+    def get_path(self, end=None, start=None):
         """
         Find a path from start to end. The end must come after
         the start (ie end must be further away from the base link
@@ -1466,20 +1466,17 @@ def get_path(self, end=None, start=None, _fknm=False):
         path = []
         n = 0
 
-        end, start, tool = self._get_limit_links(end, start)
+        end, start, tool = self._get_limit_links(end=end, start=start)
 
         # This is way faster than doing if x in y method
         try:
-            if _fknm:
-                return self._path_cache_fknm[start.name][end.name]
-            else:
-                return self._path_cache[start.name][end.name]
+            return self._path_cache[start.name][end.name]
         except KeyError:
             pass
 
         if start.name not in self._path_cache:
             self._path_cache[start.name] = {}
-            self._path_cache_fknm[start.name] = {}
+            # self._path_cache_fknm[start.name] = {}
 
         link = end
 
@@ -1498,18 +1495,15 @@ def get_path(self, end=None, start=None, _fknm=False):
                 n += 1
 
         path.reverse()
-        path_fknm = [x._fknm for x in path]
+        # path_fknm = [x._fknm for x in path]
 
         if tool is None:
             tool = SE3()
 
         self._path_cache[start.name][end.name] = (path, n, tool)
-        self._path_cache_fknm[start.name][end.name] = (path_fknm, n, tool.A)
+        # self._path_cache_fknm[start.name][end.name] = (path_fknm, n, tool.A)
 
-        if _fknm:
-            return path_fknm, n, tool.A
-        else:
-            return path, n, tool
+        return path, n, tool
 
     def fkine(
         self,
@@ -1915,7 +1909,7 @@ def link_collision_damper(
         :rtype: ndarray(6), ndarray(6)
         """
 
-        start, end, _ = self._get_limit_links()
+        end, start, _ = self._get_limit_links(start=start, end=end)
 
         links, n, _ = self.get_path(start=start, end=end)
 
@@ -1935,11 +1929,9 @@ def indiv_calculation(link, link_col, q):
                 lpTcp = -wTlp + wTcp
 
                 norm = lpTcp / d
-                norm_h = expand_dims(concatenate((norm, 0, 0, 0)), axis=0)
+                norm_h = expand_dims(concatenate((norm, [0, 0, 0])), axis=0)
 
-                Je = self.jacobe(
-                    q, start=self.base_link, end=link, tool=link_col.base.A
-                )
+                Je = self.jacobe(q, start=self.base_link, end=link, tool=link_col.T)
                 n_dim = Je.shape[1]
                 dp = norm_h @ shape.v
                 l_Ain = zeros((1, n))
