Cartesian impedance Controller - Jacobian Transpose Issue

[vk4151]

Hi,

I am trying to implement cartesian Impedance Controller to control KUKA LWR robot in Torque Control mode simillar to #2427. In order to do this, I would want to convert cartesian Forces and Torques (Fx,Fy,Fz,Tx,Ty,Tz) to Joint torques by premultiplying with Jacobian transpose. I am extracting Jacobian by using Pybullet.getJacobian(robot_id, EE_link, [0,0,0], [0,0,0,0,0,0,0],[0,0,0,0,0,0,0]). However, When I tried to premultipy with Jacobian transpose, result was not as expected and robot was quite unstable.

Would any one please help me to resolve this issue that the best way to convert Cartesian forces and torques to joint torques using Jacobian obtained from pybullet.calculateJacobian ?