Simulation crash when using calculateinverseDynamics

[Onur4229]

Hi all,
I am very new to pybullet, so sorry, if I am asking a unnecessary question.
I am currently having the issue that my simulation crashes as soon as I want to use calculateMassMatrix, or calculateInverseDynamics. There are no errors from pybullet itself. Instead, what I get is: 'Process finished with exit code 139 (interrupted by signal 11: SIGSEGV)'
I do not know, why I am getting this error. I attached the urdf file with the meshes, I am using for the Panda robot arm.
Note that I have added information to the urdf (such as mass and inertia to some links), in order to make sure that the error is not due to missing information and therefore, the added information are not correct.
However, when I start simulating, the simulation crashes and I get the error mention earlier.

panda.zip

[erwincoumans]

Thanks for the report, a crash shouldn't happen. Are there any warnings when loading the URDF?
Could you modify the URDF until it doesnt' crash to isolate the issue? Some other URDF's should work fine with calculateMassMatrix.
If I find time, I'll look into it.

[Onur4229]

Hi Erwin,
thanks for your reply.
I just recognized that there was a part in the URDF which was duplicated (material definition). I removed it and there were no warnings left, but this didn't change anything.
However, I then recognized that the fingers of the panda are the reason for the crash.
In addition, I gave numpy arrays to the method 'calculateInverseDynamics' as arguments. After removing the fingers from the URDF and sending the position and the velocities of the joints as a list instead of a numpy array, it worked out for me. Thanks a lot!

[erwincoumans]

Thanks for reporting back. It is still good to fix the crash on unsupported input (numpy etc), so let's keep it open.

[erwincoumans]

Could you share a small script that reproduces the crash (the panda.zip only contains URDF).

[Onur4229]

Hi Erwin,
I attached a new zip file: the needed meshes are in the folder 'panda_meshes'. You can find the visual and the collision meshes for the robot there.
The URDF files for the panda arm, the table as well as the plane are in the folder urdfs. the plane and the table are those which we already get from pybullet. I attached them anyways.

import numpy as np
from pandapybullet_minimal.panda_sim import PandaSim
import pybullet as p


dt = 0.001
duration = 5
n_steps = np.int(duration/dt)


physics_client_id = p.connect(p.GUI)  
p.setPhysicsEngineParameter(enableFileCaching=0)
p.setTimeStep(dt)

# Load scene
import os.path as path
module_path = path.dirname(path.abspath(__file__))
scene_id = p.loadURDF(module_path + "/envs/plane/plane.urdf")
# p.setGravity(0, 0, -10.)

# Load table
table_urdf = module_path + "/envs/table/table.urdf"
table_start_position = [0.35, 0.0, 0.0]
table_start_orientation = [0.0, 0.0, 0.0]
table_start_orientation_quat = p.getQuaternionFromEuler(table_start_orientation)
table_id = p.loadURDF(table_urdf,
                      table_start_position,
                      table_start_orientation_quat,
                      flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_INERTIA_FROM_FILE
                      )

robot_urdf = module_path + "/envs/frankaemika/robots/panda_arm_hand_without_cam.urdf"
robot_start_position = [0.0, 0.0, 0.88]
robot_start_orientation = [0.0, 0.0, 0.0]
robot_start_orientation_quat = p.getQuaternionFromEuler(robot_start_orientation)
robot_id = p.loadURDF(
    robot_urdf,
    robot_start_position, robot_start_orientation_quat,
    useFixedBase=1,
    flags=p.URDF_USE_SELF_COLLISION   #| p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT
)

p.setGravity(0,0,-9.81)

joints = (3.57795216e-09,  1.74532920e-01,  3.30500960e-08, -8.72664630e-01,
                    -1.14096181e-07,  1.22173047e+00,  7.85398126e-01)

for k in range(7):
    # set the joints (skip base which has jointIndex 0
    p.resetJointState(robot_id, jointIndex=k+1,targetValue=joints[k])
# set the panda fingers position
p.resetJointState(robot_id,jointIndex=10, targetValue=0.05)
p.resetJointState(robot_id,jointIndex=11, targetValue=0.05)



# for torque control
p.setJointMotorControlArray(robot_id, list(np.arange(1,8)), p.VELOCITY_CONTROL, forces =list(np.zeros(7)))

joint_indices = [1,2,3,4,5,6,7,10,11]

for t in range(n_steps):
    qdq_matrix = np.array([np.array(
        p.getJointState(bodyUniqueId=robot_id, jointIndex=jointIndex)[:2]) for
        jointIndex in np.arange(1, 8)])
    q = qdq_matrix[:, 0]
    dq = qdq_matrix[:, 1]
    q = list(q)
    dq = list(dq)

    f1_info = p.getJointState(bodyUniqueId=robot_id, jointIndex=10)
    f2_info = p.getJointState(bodyUniqueId=robot_id, jointIndex=11)

    fing_pos = [f1_info[0], f2_info[0]]
    fing_vel = [f1_info[1], f2_info[1]]

    q.append(fing_pos[0])
    q.append(fing_pos[1])
    dq.append(fing_vel[0])
    dq.append(fing_vel[1])

    # q = np.asarray(q)                   # if we uncomment this , this will lead to a segmentation fault
    torques = p.calculateInverseDynamics(robot_id,q,dq,list(np.zeros(9,)))
    p.setJointMotorControlArray(bodyUniqueId=robot_id, jointIndices=joint_indices,
                                controlMode=p.TORQUE_CONTROL, forces=torques)
    p.stepSimulation()

If you use

   q = np.asarray(q) 

and send it to the inverse dynamics function, this leads to the already mentioned crash. But if I comment it out and just send it as a list, the robot just stays at the initialized position, which we want to have, since we set the desired accelerations to zero for each joint when we call the inverse dynamics function.
The pathes to the urdf files in the code will have to be adjusted I guess.

files.zip

[yohanjk]

This issue still exist on the "calculateInverseKinematics" function. Simulation is crashing when numpy arrays are passed for the parameters "lowerLimits", "upperLimits" , "jointRanges", "restPoses"

[shaido987]

Thanks, changing to normal python lists fixed this problem for me.