Inverse Dynamics for Floating Base Robot Returns Nans in Terms of 6DOF robotic manipulator

[ZJU-PLP]

Hello,

I am attempting to use the pybullet's calculateInverseDynamics() function for a floating base robot with 6 actuated degrees of freedom(UR5), but I am running into a couple of issues:

1. I expect the inverse dynamics to return a list of 6 joint torques, but when we call the function, it returns 15, which is confusing to me. Furthermore, it returns nans in the torques list, just like (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, nan, nan).

2. When I run the inverse dynamics function without the argument flag=0 then the function produces a warning message:
   b3Printf: Inverse Dynamics computations failed pybullet.error: Error in calculateInverseDynamics, please check arguments.

3. I want to get the positions, velocities and accelerations of trajectory waypoints by ROS MoveIt Planner(OMPL). Then, I send the positions, velocities and accelerations to the calculateInverseDynamics() function for getting every joint torques. Moreover, I desire to use these joint torques send setJointMotorControlArray(TORQUE_CONTROL Mode). However, I meet above two problems. Could you mind pointing the mistake of this method to get joint torques?

Ps: I obey the example inverse_dynamics to transfer to my UR5 robot.