collision damper to points

Author: jhavl

roboticstoolbox/examples/neo.py

@@ -9,6 +9,7 @@
 import spatialmath as sm
 import numpy as np
 import qpsolvers as qp
+import cProfile
 
 # Launch the simulator Swift
 env = swift.Swift()
@@ -55,10 +56,8 @@
 Tep.A[:3, 3] = target.base.t
 Tep.A[2, 3] += 0.1
 
-arrived = False
-
-while not arrived:
 
+def step():
     # The pose of the Panda's end-effector
     Te = panda.fkine(panda.q)
 
@@ -136,4 +135,17 @@
     panda.qd[:n] = qd[:n]
 
     # Step the simulator by 50 ms
-    env.step(0.05)
+    env.step(0.01)
+
+    return arrived
+
+
+def run():
+    arrived = False
+    while not arrived:
+        arrived = step()
+
+
+step()
+
+cProfile.run('run()')

roboticstoolbox/examples/swift_benchmark.py

@@ -32,19 +32,3 @@ def stepper():
 
 
 cProfile.run('stepper()')
-
-# r = rtb.models.Frankie()
-# r.q = r.qr
-
-# for i in range(1000):
-#     q = np.round(np.random.random(9), 2)
-#     j1 = r.jacobe(q)
-#     # j2 = r.jacobe_new(q)
-#     j2 = r.jacobe(q, fast=True)
-
-#     print(np.round(j1, 2))
-#     print(np.round(j2, 2))
-#     print(q)
-#     print()
-
-#     nt.assert_almost_equal(j1, j2)

roboticstoolbox/robot/ERobot.py

@@ -2018,8 +2018,9 @@ def indiv_calculation(link, link_col, q):
             d, wTlp, wTcp = link_col.closest_point(shape, di)
 
             if d is not None:
-                lpTcp = wTlp.inv() * wTcp
-                norm = lpTcp.t / d
+                lpTcp = -wTlp[:3] + wTcp[:3]
+
+                norm = lpTcp / d
                 norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
 
                 Je = self.jacobe(