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

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

if base_trans is None:
print "FK KDL failure on base transformation."
if end_trans is None:
print "FK KDL failure on end transformation."
return base_trans**-1 * end_trans

j_kdl = kdl.Jacobian(len(q))
q_kdl = joint_list_to_kdl(q)
jac_kdl = kdl.ChainJntToJacSolver(chain)

jac_kdl.JntToJac(q_kdl, j_kdl)
if pos is not None:
pos_kdl = kdl.Vector(pos[0]-ee_pos[0], pos[1]-ee_pos[1],
pos[2]-ee_pos[2])
j_kdl.changeRefPoint(pos_kdl)
J = kdl_to_mat(j_kdl)

return J[:3, :], J[3:, :]

def transfomation_to_(frame_position, frame_orientation, point, inverse=False):
rotation = kdl.Rotation.Quaternion(frame_orientation[0], frame_orientation[1], frame_orientation[2],
frame_orientation[3])
position = kdl.Vector(frame_position[0], frame_position[1], frame_position[2])
frame = kdl.Frame(rotation, position)
point = kdl.Vector(point[0], point[1], point[2])

if inverse:
point_on_frame = frame.Inverse() * point
else:
point_on_frame = frame * point

return [point_on_frame[0], point_on_frame[1], point_on_frame[2]]

def getJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques

if __name__ == '__main__':

# clid = p.connect(p.SHARED_MEMORY)
# p.connect(p.GUI)
p.connect(p.DIRECT)
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True)
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()

rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]

for i in range(numJoints):
p.resetJointState(kukaId, i, rp[i])

p.setGravity(0, 0, -10)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
useNullSpace = 0

count = 0
useOrientation = 1
useSimulation = 1
useRealTimeSimulation = 0
p.setRealTimeSimulation(useRealTimeSimulation)

urdf_file = pybullet_data.getDataPath() + "/kuka_iiwa/model.urdf"
model = URDF.from_xml_file(urdf_file)

q = getJointStates(kukaId)[0]

# print q

computeForwardKinematics=1)

zero_vec = [0] * len(q)
pt = [0, 0, 0]
pt1 = None
pt2 = [0.02, 0, 0]

# jac_t, _ = get_kdl_jacobian(q, chain, 0, 7, pt)
jac_t, _ = get_kdl_jacobian(q, chain, 0, 7, pt1)
# jac_t, _ = get_kdl_jacobian(q, chain, 0, 7, pt2)
# jac_t, _ = get_kdl_jacobian(q, chain, 0, 7, pt3)
# jac_t, _ = get_kdl_jacobian(q, chain, 0, 7, pt4)

jac_t1, _ = p.calculateJacobian(kukaId, 6, pt, q, zero_vec, zero_vec)
# jac_t1, _ = p.calculateJacobian(kukaId, 6, pt1, q, zero_vec, zero_vec)
# jac_t1, _ = p.calculateJacobian(kukaId, 6, pt2, q, zero_vec, zero_vec)
# jac_t1, _ = p.calculateJacobian(kukaId, 6, pt3, q, zero_vec, zero_vec)
# jac_t1, _ = p.calculateJacobian(kukaId, 6, pt4, q, zero_vec, zero_vec)

# only combination giving correct result: pt1 and pt
print("jacobian from kdl")
print (jac_t)
print ("jacobian from pybullet")
print (jac_t1)


Can someone please tell me, what is the mistake I am doing and how can I get correct results.