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

edit

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

def get_kdl_tree(robot_model, base_link, end_link):

    status, tree = kdl_parser_py.urdf.treeFromUrdfModel(robot_model, quiet=True)
    chain = tree.getChain(base_link, end_link)
    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

def do_kdl_fk(q, chain, link_number):
    endeffec_frame = kdl.Frame()
    fk_kdl = kdl.ChainFkSolverPos_recursive(chain)
    kinematics_status = fk_kdl.JntToCart(joint_list_to_kdl(q),
                                               endeffec_frame,
                                               link_number)
    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

def forward(q, chain, base_link, end_link):
    base_trans = do_kdl_fk(q, chain, base_link)
    if base_trans is None:
        print "FK KDL failure on base transformation."
    end_trans = do_kdl_fk(q, chain, end_link)
    if end_trans is None:
        print "FK KDL failure on end transformation."
    return base_trans**-1 * end_trans ...