I use pybullet for operational space control on 7-dof manipulators. I've noticed some inconsistencies with the mass matrix calculated by pybullet compared to other simulation environments. I've written two small scripts that demonstrate this issue by comparing pybullet and RBDL.
To help recreate the error, I've used the panda.urdf found in bullet3 (commit f40a200) at the following filepath:
bullet3/examples/pybullet/gym/pybullet_data/franka_panda
I use pybullet 2.7.1 in a python3.5 virtual environment. I'd appreciate any help you can offer for this issue.
Pybullet
Code: Select all
import pybullet as pb
import numpy as np
physics_id = pb.connect(pb.DIRECT)
arm_id = pb.loadURDF(
fileName='/home/rohunk-local/bullet3/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf',
basePosition=[0,0,0],
baseOrientation=[0,0,0,1],
useFixedBase=True,
physicsClientId=physics_id)
pb.setGravity(0, 0, -9.8, physicsClientId=physics_id)
neutral_joint_angles= [0., -0.3135, 0., -2.515, 0., 2.226, 0.87]
for index, angle in enumerate(neutral_joint_angles):
pb.resetJointState(
bodyUniqueId=arm_id,
jointIndex=index,
targetValue=angle,
physicsClientId=physics_id)
pb.stepSimulation(physics_id)
joint_positions = []
for joint_index in range(7):
joint_positions.append(pb.getJointState(
arm_id,
joint_index,
physics_id)[0])
print("joint positions")
print(joint_positions)
mass_matrix = pb.calculateMassMatrix(
bodyUniqueId=arm_id,
objPositions=joint_positions,
physicsClientId=physics_id)
print("Mass Matrix")
print(mass_matrix)
Code: Select all
#!/usr/bin/python
#
# rbdl mass matrix
import rbdl
import numpy as np
import csv
mdl = rbdl.loadModel(b'/home/rohunk-local/bullet3/examples/pybullet/gym/pybullet_data/franka_panda/panda.urdf')
mass_matrix = np.zeros((9, 9 ), dtype=np.double)
neutral_joint_angles= [0., -0.3135, 0., -2.515, 0., 2.226, 0.87]
rbdl.CompositeRigidBodyAlgorithm(mdl,
np.asarray(neutral_joint_angles, dtype=np.double),
mass_matrix)