# How to find Jacobian using kdl that matches with bullet physics jacobian

Hi,

I am trying to calculate jacobian with kdl (pyKDL) and bullet physics (pybullet). With bullet physics I can only find Jacobian of whole robot, but I want to find Jacobian for a kinematic chain. So I wanted use kdl (pykdl) for the same.

I use pykdl_utils, which uses pykdl to calculate Jacobian for the given chain and to understand how kdl works, I am playing with a 7 DOF arm, after which I can try with the complex robot to get Jacobian for a given chain.

For, now I calculate Jacobian for the entire arm with origin point [0, 0, 0] of an end-effector in pybullet and using JntToJac(q_kdl, j_kdl) in pykdl.

I get the following results, which are approximately same,

jacobian from kdl
[[  3.93204983e-13   3.50279896e-01  -6.08147439e-13   6.97201042e-02
-3.26970957e-13  -6.97201042e-02   0.00000000e+00]
[ -4.41232355e-01  -2.08808746e-12  -4.41232355e-01  -2.14611370e-12
-6.97201042e-02   1.96005964e-13   0.00000000e+00]
[  0.00000000e+00   4.41232355e-01   1.44669450e-25  -4.41232355e-01
5.57708303e-13   4.12323547e-02  -0.00000000e+00]]
jacobian from pybullet
((4.21021431342817e-08, 0.35027993596935714, 5.104112371956412e-08, 0.06972006581877463, 9.142687868923103e-09, -0.06972010078788882, 0.0), (-0.4412323331119607, 4.990932440307452e-08, -0.44123234585644516, 2.5382000887679235e-08, -0.06972010439253574, -4.292801227882336e-09, 0.0), (6.452008777913616e-23, 0.44123233311196214, -1.0899318209780877e-15, -0.4412323519515711, 1.2452388596192707e-09, 0.041232360892270135, 0.0))


But I don't get correct results when I try to calculate a Jacobian for a point other than origin, with j_kdl.changeRefPoint(pos_kdl) and through pybullet.

I use the following code, where I have copied few necessary functions from pykdl_utils, to post the code here, so that its installation can be avoided.

import pybullet as p
import math
import pybullet_data
import kdl_parser_py.urdf
import PyKDL as kdl
from urdf_parser_py.urdf import URDF
import numpy as np

status, tree = kdl_parser_py.urdf.treeFromUrdfModel(robot_model, quiet=True)
return tree, chain

def joint_list_to_kdl(q):
if q is None:
return None
if type(q) == np.matrix and q.shape[1] == 0:
q = q.T.tolist()[0]
q_kdl = kdl.JntArray(len(q))
for i, q_i in enumerate(q):
q_kdl[i] = q_i
return q_kdl

def kdl_to_mat(m):
mat =  np.mat(np.zeros((m.rows(), m.columns())))
for i in range(m.rows()):
for j in range(m.columns()):
mat[i,j] = m[i,j]
return mat

endeffec_frame = kdl.Frame()
fk_kdl = kdl.ChainFkSolverPos_recursive(chain)
kinematics_status = fk_kdl.JntToCart(joint_list_to_kdl(q),
endeffec_frame,
if kinematics_status >= 0:
p = endeffec_frame.p
M = endeffec_frame.M
return np.mat([[M[0,0], M[0,1], M[0,2], p.x()],
[M[1,0], M[1,1], M[1,2], p.y()],
[M[2,0], M[2,1], M[2,2], p.z()],
[     0,      0,      0,     1]])
else:
return None

return base_trans**-1 * end_trans ...