progress

jhavl · jhavl · commit 899a766a4763 · 2020-10-23T15:39:03.000+10:00
diff --git a/examples/neo.py b/examples/neo.py
@@ -100,6 +100,28 @@
     # Form the joint limit velocity damper
     Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
 
+    for link in links:
+        if link.jtype == link.VARIABLE:
+            j += 1
+        for col in link.collision:
+            obj = s0
+            l_Ain, l_bin, ret, wTcp = link_calc(link, col, obj, q[:j])
+            if ret < closest:
+                closest = ret
+                closest_obj = obj
+                closest_p = wTcp
+
+            if l_Ain is not None and l_bin is not None:
+                if Ain is None:
+                    Ain = l_Ain
+                else:
+                    Ain = np.r_[Ain, l_Ain]
+
+                if bin is None:
+                    bin = np.array(l_bin)
+                else:
+                    bin = np.r_[bin, l_bin]
+
     # Linear component of objective function: the manipulability Jacobian
     c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)]
 
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1120,11 +1120,14 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
 
         return Ain, bin
 
-    # def link_collision_damper(self, link, col, ob, q):
+    # def link_collision_damper(self, links=None, col, ob, q):
     #     dii = 5
     #     di = 0.3
     #     ds = 0.05
 
+    #     if links is None:
+    #         links = self.links[1:]
+
     #     ret = p.getClosestPoints(col.co, ob.co, dii)
 
     #     if len(ret) > 0:
