erwincoumans
commented
4 years ago That is not clear, how big is the difference? Do you have a full working small reproduction example?
Closed gsutanto closed 4 years ago
erwincoumans
commented
4 years ago That is not clear, how big is the difference? Do you have a full working small reproduction example?
gsutanto
commented
4 years ago 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
commented
4 years ago 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
commented
4 years ago 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
commented
4 years ago 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]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
commented
4 years ago 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 ]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]
Dear PyBullet Authors,
I am computing end-effector velocity using two methods, the first one is:
and the second one is:
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