fix jacobian

neka-nat · Feb 5, 2021 · 671731e · 671731e
1 parent d63effc
commit 671731e
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 2 deletions.
diff --git a/kinpy/jacobian.py b/kinpy/jacobian.py
@@ -12,7 +12,7 @@ def calc_jacobian(serial_chain, th, tool=transform.Transform()):
         if f.joint.joint_type == "revolute":
             cnt += 1
             delta = np.dot(f.joint.axis, cur_transform[:3, :3])
-            d = np.cross(delta, cur_transform[:3, 3])
+            d = np.dot(np.cross(f.joint.axis, cur_transform[:3, 3]), cur_transform[:3, :3])
             j_fl[:, -cnt] = np.hstack((d, delta))
         elif f.joint.joint_type == "prismatic":
             cnt += 1

diff --git a/tests/test_jacobian.py b/tests/test_jacobian.py
@@ -53,8 +53,17 @@ def test_jacobian2(self):
                                           [0.0, 0.0],
                                           [0.0, 0.0],
                                           [1.0, 0.0]]), jc)
-
 
+    def test_jacobian3(self):
+        chain = kp.build_serial_chain_from_urdf(open("examples/kuka_iiwa/model.urdf").read(), "lbr_iiwa_link_7")
+        th = [0.0, -np.pi / 4.0, 0.0, np.pi / 2.0, 0.0, np.pi / 4.0, 0.0]
+        jc = chain.jacobian(th)
+        np.testing.assert_almost_equal(np.array([[0, 1.41421356e-02, 0, 2.82842712e-01, 0, 0, 0],
+                                                 [-6.60827561e-01, 0, -4.57275649e-01, 0, 5.72756493e-02, 0, 0],
+                                                 [0, 6.60827561e-01, 0, -3.63842712e-01, 0, 8.10000000e-02, 0],
+                                                 [0, 0, -7.07106781e-01, 0, -7.07106781e-01, 0, -1],
+                                                 [0, 1, 0, -1, 0, 1, 0],
+                                                 [1, 0, 7.07106781e-01, 0, -7.07106781e-01, 0, 0]]), jc)
 
 if __name__ == "__main__":
     unittest.main()