calculateMassMatrix inconsistent with RBDL

[rohun]

Hi Erwin,

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)

RBDL

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) 

[steven]

Do you have any progress? Could you tell me why did you compare the MassMatrix between them? thanks.

[rohun]

Hi Steven,

No updates at the moment. We're trying to use the python wrapper for RBDL directly as a short term solution.

For our research we'd like to use similar controllers for both similar robots and real robots for experimentation. We've used an internal physics simulator (that uses RBDL) for real robot control and wanted to verify that pybullet produces the same mass matrix for the same robot state.

I'd appreciate it if anyone else can try running the scripts I've attached to verify the issue.

-Rohun

[steven]

thanks for your reply, i have found the orignal paper for bullet and i will check it and try to find the cause.

[steven]

adding this 'flags=pb.URDF_USE_INERTIA_FROM_FILE' when loading the urdf can fix this.

[rohun]

Thanks Steven!

I've confirmed this works. You've helped us simplify our infrastructure significantly. If you don't mind, could you please link the original paper for bullet as well?

-rohun

[steven]

http://www.doc88.com/p-9833547094235.html
not sure if you can access this link, also wanted to attach the pdf for your reference but it is too large about 32M for this web. i can email to you if you need.