End-Effector Velocity Computation: getLinkState() versus calculateJacobian() #2429

gsutanto · 2019-10-03T02:24:22Z

Dear PyBullet Authors,

I am computing end-effector velocity using two methods, the first one is:

ee_state = sim.getLinkState(robot_id, ee_idx-1,
                            computeLinkVelocity=True, computeForwardKinematics=True)
ee_lin_vel = np.array(ee_state[6]) # worldLinkLinearVelocity
ee_ang_vel = np.array(ee_state[7]) # worldLinkAngularVelocity

and the second one is:

# q is joint configuration/position, and qd is joint velocity, 
# both are computed from sim.getJointStates()
[jac_t, jac_r
 ] = sim.calculateJacobian(robot_id, ee_idx-1, [0] * 3, 
                           list(q), [0] * n_dofs, [0] * n_dofs)
ee_lin_vel_via_jacobian = np.asarray(np.matmul(jac_t, qd).T[0])
ee_ang_vel_via_jacobian = np.asarray(np.matmul(jac_r, qd).T[0])

I found out that: while ee_ang_vel and ee_ang_vel_via_jacobian are identical, the ee_lin_vel and ee_lin_vel_via_jacobian is slightly different. Why is this?

Best Regards,

Giovanni

The text was updated successfully, but these errors were encountered:

erwincoumans · 2019-10-04T01:08:52Z

That is not clear, how big is the difference?
Do you have a full working small reproduction example?

gsutanto · 2019-10-04T04:15:35Z

Dear @erwincoumans ,

Thanks for your response. This is the runnable code snippet:

import pybullet_utils.bullet_client as bc
import pybullet
import pybullet_data

import time
import numpy as np

hz = 240.0
dt = 1.0 / hz

sim = bc.BulletClient(connection_mode=pybullet.GUI)

sim.setGravity(0, 0, -9.81)
sim.setTimeStep(dt)
sim.setRealTimeSimulation(0)

controlled_joints = [0, 1, 2, 3, 4, 5, 6]
addtnl_torque = np.array([0,0,0,1.0,0,0,0])
n_dofs = len(controlled_joints)

pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
urdf_path = "kuka_iiwa/model.urdf"

world_id = pybullet.loadURDF("plane.urdf", [0, 0, 0])
robot_id = sim.loadURDF(urdf_path, basePosition=[0.0, 0.0, 0.0], 
                        useFixedBase=True,
                        flags=pybullet.URDF_USE_INERTIA_FROM_FILE)

def getCurrentJointPosVel():
    cur_joint_states = sim.getJointStates(robot_id, controlled_joints)
    cur_joint_pos = [cur_joint_states[i][0] for i in range(n_dofs)]
    cur_joint_vel = [cur_joint_states[i][1] for i in range(n_dofs)]
    return cur_joint_pos, cur_joint_vel

def getCurrentEEVel():
    ee_state = sim.getLinkState(robot_id, n_dofs-1,
    	                        computeLinkVelocity=True, 
                                computeForwardKinematics=True)
    ee_lin_vel = np.array(ee_state[6]) # worldLinkLinearVelocity
    ee_ang_vel = np.array(ee_state[7]) # worldLinkAngularVelocity
    return ee_lin_vel, ee_ang_vel

def computeGravityCompensationControlPolicy():
    [cur_joint_pos, cur_joint_vel] = getCurrentJointPosVel()
    grav_comp_torque = sim.calculateInverseDynamics(robot_id, cur_joint_pos, 
                                                    cur_joint_vel, 
                                                    [0] * n_dofs)
    return np.array(grav_comp_torque)

# activating torque control
sim.setJointMotorControlArray(
            bodyIndex=robot_id,
            jointIndices=controlled_joints,
            controlMode=pybullet.VELOCITY_CONTROL,
            forces=np.zeros(n_dofs))

while True:
    [q, qd] = getCurrentJointPosVel()
    [ee_lin_vel, ee_ang_vel] = getCurrentEEVel()
    [jac_t, jac_r
     ] = sim.calculateJacobian(robot_id, n_dofs-1, [0] * 3, 
                               q, [0] * n_dofs, [0] * n_dofs)
    ee_lin_vel_via_jac = np.asarray(np.matmul(jac_t, np.array(qd).
                                                     reshape((n_dofs, 1))).T[0])
    ee_ang_vel_via_jac = np.asarray(np.matmul(jac_r, np.array(qd).
                                                     reshape((n_dofs, 1))).T[0])
    print("ee_lin_vel              = ", ee_lin_vel)
    print("ee_lin_vel_via_jacobian = ", ee_lin_vel_via_jac)
    print("ee_ang_vel              = ", ee_ang_vel)
    print("ee_ang_vel_via_jacobian = ", ee_ang_vel_via_jac)
    print("")
    grav_comp_torque = computeGravityCompensationControlPolicy()
    sim.setJointMotorControlArray(
            bodyIndex=robot_id,
            jointIndices=controlled_joints,
            controlMode=pybullet.TORQUE_CONTROL,
            forces=grav_comp_torque+addtnl_torque)
    sim.stepSimulation()
    time.sleep(dt)

and the example print-outs are:

ee_lin_vel              =  [ 0.41264624 -0.41627541 -1.02505126]
ee_lin_vel_via_jacobian =  [ 0.41526568 -0.3873048  -0.99356107]
ee_ang_vel              =  [-30.01574004  62.96066879 -55.42630029]
ee_ang_vel_via_jacobian =  [-30.01574004  62.96066879 -55.42630029]

Do you know why ee_lin_vel and ee_lin_vel_via_jacobian is slightly different, while ee_ang_vel is identical to ee_ang_vel_via_jacobian ?

Thanks!

Best Regards,

Giovanni

erwincoumans · 2019-10-04T14:53:59Z

Hello Giovianni,

The method via Jacobian computes the link velocity at the joint, while the 'getLinkState' returns the velocity at the center of mass: lbr_iiwa_link_7 has an inertial frame at location xyz="0 0 0.02".
If you add an additional end-effector with zero inertial frame origin like this, or modify lbr_iiwa_link_7 to have the inertial frame at the joint like this:

 <link name="lbr_iiwa_link_7">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>

both linear and angular velocity will match:

ee_lin_vel              =  [-0.3048944   0.1884439  -0.13362494]
ee_lin_vel_via_jacobian =  [-0.3048944   0.1884439  -0.13362494]
ee_ang_vel              =  [ 16.20880996  -2.22107191 -86.05255472]
ee_ang_vel_via_jacobian =  [ 16.20880996  -2.22107191 -86.05255472]

Can we use your snippet as a PyBullet example?

Also, for comparison, do you have a similar example using Pinocchio using the same URDF?
We did comparisons of forward dynamics with RBDL and KDL library (see issue) and it matches up to 11 decimals.
Thanks!
Erwin

gsutanto · 2019-10-04T18:02:19Z

Dear @erwincoumans ,

I see. That makes sense.

Sure, you can use the snippet as a PyBullet example.

Yes, actually I compared the result between PyBullet versus Pinocchio. I can work on a simple code snippet that compares between the two. Please give me a couple days for this, and I will post it here, once it is ready.

Thanks!

Best Regards,

Giovanni Sutanto

gsutanto · 2019-10-05T00:06:56Z

Dear @erwincoumans ,

Here's the code comparing PyBullet vs. Pinocchio:

import pybullet_utils.bullet_client as bc
import pybullet
import pybullet_data

import pinocchio as pin

import os,time,copy
import numpy as np

hz = 240.0
dt = 1.0 / hz

sim = bc.BulletClient(connection_mode=pybullet.GUI)

sim.setGravity(0, 0, -9.81)
sim.setTimeStep(dt)
sim.setRealTimeSimulation(0)

controlled_joints = [0, 1, 2, 3, 4, 5, 6]
addtnl_torque = np.array([0,0,0,1.0,0,0,0])
n_dofs = len(controlled_joints)

pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
urdf_path = "kuka_iiwa/model.urdf"

world_id = pybullet.loadURDF("plane.urdf", [0, 0, 0])
robot_id = sim.loadURDF(urdf_path, basePosition=[0.0, 0.0, 0.0], 
                        useFixedBase=True,
                        flags=pybullet.URDF_USE_INERTIA_FROM_FILE)

pin_model = pin.buildModelFromUrdf(pybullet_data.getDataPath()+'/'+urdf_path)
pin_data = pin_model.createData()

def getBaseLocalInertialPosAndOri():
    dyn_info = sim.getDynamicsInfo(robot_id, -1)
    return np.array(dyn_info[3]), np.array(dyn_info[4])

def getCurrentJointPosVel():
    cur_joint_states = sim.getJointStates(robot_id, controlled_joints)
    cur_joint_pos = [cur_joint_states[i][0] for i in range(n_dofs)]
    cur_joint_vel = [cur_joint_states[i][1] for i in range(n_dofs)]
    return cur_joint_pos, cur_joint_vel

def getCurrentEEVel():
    ee_state = sim.getLinkState(robot_id, n_dofs-1,
    	                        computeLinkVelocity=True, 
                                computeForwardKinematics=True)
    ee_lin_vel = np.array(ee_state[6]) # worldLinkLinearVelocity
    ee_ang_vel = np.array(ee_state[7]) # worldLinkAngularVelocity
    return ee_lin_vel, ee_ang_vel

def getCurrentLinkPosAndOri(link_id):
    link_state = sim.getLinkState(robot_id, link_id, 
                                  computeForwardKinematics=True)
    return np.array(link_state[4]), np.array(link_state[5])

def computeGravityCompensationControlPolicy():
    [cur_joint_pos, cur_joint_vel] = getCurrentJointPosVel()
    grav_comp_torque = sim.calculateInverseDynamics(robot_id, cur_joint_pos, 
                                                    cur_joint_vel, 
                                                    [0] * n_dofs)
    return np.array(grav_comp_torque)

# activating torque control
sim.setJointMotorControlArray(
            bodyIndex=robot_id,
            jointIndices=controlled_joints,
            controlMode=pybullet.VELOCITY_CONTROL,
            forces=np.zeros(n_dofs))

prev_qd = [0.0] * n_dofs

while True:
    [q, qd] = getCurrentJointPosVel()
    qdd = list((np.array(qd) - np.array(prev_qd))/dt)
    
    pin.forwardKinematics(pin_model, pin_data, np.array(q).reshape((n_dofs, 1)))
    for i in range(n_dofs):
        pin_x = pin_data.oMi[i+1].translation
        pin_x = np.array([pin_x[j,0] for j in range(3)])
        print('Pinocchio: link_x[%d] = ' % i, pin_x)

        [bullet_x, _] = getCurrentLinkPosAndOri(i)
        print('PyBullet:  link_x[%d] = ' % i, bullet_x.reshape([3]))
        
        print('')

    pin_tau = pin.rnea(pin_model, pin_data,
                       np.array(q).reshape([n_dofs, 1]),
                       np.array(qd).reshape([n_dofs, 1]),
                       np.array(qdd).reshape([n_dofs, 1]))
    pin_tau = np.array([pin_tau[j,0] for j in range(n_dofs)])
    bullet_tau = np.array(sim.calculateInverseDynamics(robot_id, q, qd, qdd))
    print("q   = ", np.array(q))
    print("qd  = ", np.array(qd))
    print("qdd = ", np.array(qdd))
    print('Pinocchio: Inv. Dyn. Torque = ', pin_tau)
    print('PyBullet:  Inv. Dyn. Torque = ', bullet_tau)
    print('')

    grav_comp_torque = computeGravityCompensationControlPolicy()
    sim.setJointMotorControlArray(
            bodyIndex=robot_id,
            jointIndices=controlled_joints,
            controlMode=pybullet.TORQUE_CONTROL,
            forces=grav_comp_torque+addtnl_torque)
    prev_qd = copy.deepcopy(qd)
    sim.stepSimulation()
    time.sleep(dt)

and some of the print-outs are:

Pinocchio: link_x[0] =  [0.     0.     0.1575]
PyBullet:  link_x[0] =  [ 2.32830644e-10 -1.86264515e-09  1.57499999e-01]

Pinocchio: link_x[1] =  [0.   0.   0.36]
PyBullet:  link_x[1] =  [-3.72529030e-09  0.00000000e+00  3.60000014e-01]

Pinocchio: link_x[2] =  [0.17596353 0.01276873 0.46341202]
PyBullet:  link_x[2] =  [0.17596354 0.01276873 0.46341205]

Pinocchio: link_x[3] =  [0.36139209 0.02622428 0.57238654]
PyBullet:  link_x[3] =  [0.36139208 0.02622429 0.57238656]

Pinocchio: link_x[4] =  [0.30791425 0.16577065 0.46418681]
PyBullet:  link_x[4] =  [0.30791426 0.16577065 0.46418682]

Pinocchio: link_x[5] =  [0.24545098 0.32876383 0.33780718]
PyBullet:  link_x[5] =  [0.24545097 0.32876384 0.33780715]

Pinocchio: link_x[6] =  [0.22021603 0.38895958 0.28984189]
PyBullet:  link_x[6] =  [0.22021602 0.38895959 0.28984189]

q   =  [ 0.07243766  1.04062375 -2.03419747  2.09293735  0.02410786 -0.02602246
 -0.19487212]
qd  =  [-3.12175174e-02  1.09737590e+00 -2.18815876e+00  4.41436197e-01
  1.27696555e+01  7.12270980e-01 -1.00000000e+02]
qdd =  [-6.60591406e+00 -8.50066820e+00 -5.12938984e+00  1.19207024e+02
  6.14778773e+03  3.09753177e+02 -4.80000000e+04]
Pinocchio: Inv. Dyn. Torque =  [ -8.73676732 -19.84664612   2.33862941   0.39936623  14.61306156
   0.25875462 -41.85486271]
PyBullet:  Inv. Dyn. Torque =  [ -8.73676732 -19.84664612   2.33862941   0.39936623  14.61306156
   0.25875462 -41.85486271]

Pinocchio: link_x[0] =  [0.     0.     0.1575]
PyBullet:  link_x[0] =  [2.32830644e-10 0.00000000e+00 1.57499999e-01]

Pinocchio: link_x[1] =  [0.   0.   0.36]
PyBullet:  link_x[1] =  [0.         0.         0.36000001]

Pinocchio: link_x[2] =  [0.1764516  0.0127925  0.46257404]
PyBullet:  link_x[2] =  [0.17645161 0.01279251 0.46257403]

Pinocchio: link_x[3] =  [0.36239448 0.02627311 0.57066552]
PyBullet:  link_x[3] =  [0.36239448 0.02627311 0.57066554]

Pinocchio: link_x[4] =  [0.30913122 0.16519962 0.46156602]
PyBullet:  link_x[4] =  [0.30913121 0.16519961 0.46156603]

Pinocchio: link_x[5] =  [0.24691857 0.32746879 0.33413544]
PyBullet:  link_x[5] =  [0.24691857 0.32746881 0.33413544]

Pinocchio: link_x[6] =  [0.22166758 0.38724468 0.28565627]
PyBullet:  link_x[6] =  [0.22166757 0.38724467 0.28565627]

q   =  [ 0.07237204  1.04536688 -2.04321556  2.09276842 -0.02942063 -0.02843321
  0.22179455]
qd  =  [-1.57480020e-02  1.13835055e+00 -2.16434105e+00 -4.05439318e-02
 -1.28468393e+01 -5.78579636e-01  1.00000000e+02]
qdd =  [ 3.71268371e+00  9.83391674e+00  5.71625140e+00 -1.15675231e+02
 -6.14795875e+03 -3.09804148e+02  4.80000000e+04]
Pinocchio: Inv. Dyn. Torque =  [  1.43087527 -32.81982987  11.14560209   0.26086383 -14.59369822
  -0.27647739  41.85665893]
PyBullet:  Inv. Dyn. Torque =  [  1.43087527 -32.81982987  11.14560209   0.26086383 -14.59369822
  -0.27647739  41.85665893]

erwincoumans · 2019-10-16T17:50:24Z

Thanks a lot for sharing your code, that is very useful. Also, I'm glad to see that PyBullet and Pinocchio agree on the results. If you have any further results of comparisons involving PyBullet and other simulators, please share it if you can.

erwincoumans · 2019-11-27T21:20:58Z

I tried running this example recently (without Pinocchio, since the pip install version lacks buildModelFromUrdf ), but I don't get the same results. How did you create the print output exactly? Can you still reproduce this with the latest PyBullet version? Do you have the (modified) URDF that you used? Here is my output (from the start)

PyBullet:  link_x[0] =  [0.     0.     0.1575]

PyBullet:  link_x[1] =  [0.         0.         0.36000001]

PyBullet:  link_x[2] =  [3.38813179e-20 0.00000000e+00 5.64500034e-01]

PyBullet:  link_x[3] =  [4.45440194e-14 0.00000000e+00 7.79999971e-01]

PyBullet:  link_x[4] =  [0.         0.         0.96450007]

PyBullet:  link_x[5] =  [1.27224267e-13 0.00000000e+00 1.18000007e+00]

PyBullet:  link_x[6] =  [1.43967019e-13 2.40913886e-12 1.26100004e+00]

q   =  [0. 0. 0. 0. 0. 0. 0.]
qd  =  [0. 0. 0. 0. 0. 0. 0.]
qdd =  [0. 0. 0. 0. 0. 0. 0.]
PyBullet:  Inv. Dyn. Torque =  [-3.28166137e-28  1.34397000e-02 -7.45802160e-14 -1.66770000e-03
 -5.45980475e-14 -5.14638712e-14  0.00000000e+00]

PyBullet:  link_x[0] =  [2.84217094e-14 0.00000000e+00 1.57499999e-01]

PyBullet:  link_x[1] =  [0.         0.         0.35999998]

PyBullet:  link_x[2] =  [8.97089740e-06 0.00000000e+00 5.64500034e-01]

PyBullet:  link_x[3] =  [1.84243381e-05 3.72529030e-09 7.79999971e-01]

PyBullet:  link_x[4] =  [2.12970917e-06 0.00000000e+00 9.64500070e-01]

PyBullet:  link_x[5] =  [-1.69027644e-05 -2.91038305e-11  1.18000007e+00]

PyBullet:  link_x[6] =  [-1.23716436e-05  1.20168472e-10  1.26100004e+00]

q   =  [ 8.93643421e-06  4.38674697e-05 -2.25652743e-06  1.32185227e-04
  9.51764879e-06  1.44257527e-04 -1.61975556e-05]
qd  =  [ 0.00214474  0.01052819 -0.00054157  0.03172445  0.00228424  0.03462181
 -0.00388741]
qdd =  [ 0.51473861  2.52676626 -0.12997598  7.61386906  0.54821657  8.30923354
 -0.9329792 ]

gsutanto · 2019-11-28T03:28:31Z

Hi Erwin, I am using Pinocchio version 2.1.9 and PyBullet version 2.5.8, and it still produces the same result with the script I provided above. The model is:

pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
urdf_path = "kuka_iiwa/model.urdf"
robot_id = sim.loadURDF(urdf_path, basePosition=[0.0, 0.0, 0.0], 
                        useFixedBase=True,
                        flags=pybullet.URDF_USE_INERTIA_FROM_FILE)

For the pairs of q, qd, and qdd you asked:

q   =  [0. 0. 0. 0. 0. 0. 0.]
qd  =  [0. 0. 0. 0. 0. 0. 0.]
qdd =  [0. 0. 0. 0. 0. 0. 0.]
Pinocchio: Inv. Dyn. Torque =  [-3.37162077e-28  1.34397000e-02 -7.46244027e-14 -1.66770000e-03
 -5.46400717e-14 -6.36679703e-14  0.00000000e+00]
PyBullet:  Inv. Dyn. Torque =  [-3.28166137e-28  1.34397000e-02 -7.46244027e-14 -1.66770000e-03
 -5.46400717e-14 -6.36679703e-14  0.00000000e+00]

q   =  [ 8.93643421e-06  4.38674697e-05 -2.25652743e-06  1.32185227e-04
  9.51764879e-06  1.44257527e-04 -1.61975556e-05]
qd  =  [ 0.00214474  0.01052819 -0.00054157  0.03172445  0.00228424  0.03462181
 -0.00388741]
qdd =  [ 0.51473861  2.52676626 -0.12997598  7.61386906  0.54821657  8.30923354
 -0.9329792 ]
Pinocchio: Inv. Dyn. Torque =  [-1.27253838e-08  2.65099358e-02 -1.58206684e-05  9.91674563e-01
  3.03057826e-05 -2.34024783e-03 -4.06517081e-11]
PyBullet:  Inv. Dyn. Torque =  [-1.27253838e-08  2.65099358e-02 -1.58206684e-05  9.91674563e-01
  3.03057826e-05 -2.34024783e-03 -4.06517082e-11]

gsutanto closed this as completed Oct 16, 2019

Berk-Tosun mentioned this issue Nov 23, 2022

End-Effector Velocity in Bullet vs Pinocchio stack-of-tasks/pinocchio#1795

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

End-Effector Velocity Computation: getLinkState() versus calculateJacobian() #2429

End-Effector Velocity Computation: getLinkState() versus calculateJacobian() #2429

gsutanto commented Oct 3, 2019 •

edited

erwincoumans commented Oct 4, 2019

gsutanto commented Oct 4, 2019 •

edited

erwincoumans commented Oct 4, 2019 •

edited

gsutanto commented Oct 4, 2019

gsutanto commented Oct 5, 2019

erwincoumans commented Oct 16, 2019

erwincoumans commented Nov 27, 2019 •

edited

gsutanto commented Nov 28, 2019

End-Effector Velocity Computation: getLinkState() versus calculateJacobian() #2429

End-Effector Velocity Computation: getLinkState() versus calculateJacobian() #2429

Comments

gsutanto commented Oct 3, 2019 • edited

erwincoumans commented Oct 4, 2019

gsutanto commented Oct 4, 2019 • edited

erwincoumans commented Oct 4, 2019 • edited

gsutanto commented Oct 4, 2019

gsutanto commented Oct 5, 2019

erwincoumans commented Oct 16, 2019

erwincoumans commented Nov 27, 2019 • edited

gsutanto commented Nov 28, 2019

gsutanto commented Oct 3, 2019 •

edited

gsutanto commented Oct 4, 2019 •

edited

erwincoumans commented Oct 4, 2019 •

edited

erwincoumans commented Nov 27, 2019 •

edited