Fixed get_path

Author: jhavl

roboticstoolbox/robot/ERobot.py

@@ -1821,7 +1821,7 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
 
     def link_collision_damper(
             self, shape, q=None, di=0.3, ds=0.05, xi=1.0,
-            from_link=None, to_link=None):
+            endlink=None, startlink=None):
         '''
         Formulates an inequality contraint which, when optimised for will
         make it impossible for the robot to run into a collision. Requires
@@ -1847,13 +1847,13 @@ def link_collision_damper(
         :rtype: ndarray(6), ndarray(6)
         '''
 
-        if from_link is None:
-            from_link = self.base_link
+        if startlink is None:
+            startlink = self.base_link
 
-        if to_link is None:
-            to_link = self.ee_link
+        if endlink is None:
+            endlink = self.ee_link
 
-        links, n = self.get_path(from_link, to_link)
+        links, n = self.get_path(startlink=startlink, endlink=endlink)
 
         if q is None:
             q = np.copy(self.q)
@@ -1873,7 +1873,7 @@ def indiv_calculation(link, link_col, q):
                 norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
 
                 Je = self.jacobe(
-                    q, from_link=self.base_link, to_link=link,
+                    q, startlink=self.base_link, endlink=link,
                     offset=link_col.base)
                 n_dim = Je.shape[1]
                 dp = norm_h @ shape.v
@@ -1892,7 +1892,7 @@ def indiv_calculation(link, link_col, q):
 
             for link_col in link.collision:
                 l_Ain, l_bin, d, wTcp = indiv_calculation(
-                                                link, link_col, q[:j])
+                                                link, link_col, q)
 
                 if l_Ain is not None and l_bin is not None:
                     if Ain is None: