Collision free path is found but the execution of the trajectory detects contacts.

[nicolasCheron]

Hello, I wrote a RRT-connect (in joint space) algorithm to move the robot into a pybullet environnement.
To know if a point (in joint space) is in free space i do the following:
-resetJointState() for each joint.
-stepSimulation().

• I look if getContactPoints() is empty (no collision) or not.

I check if going from one point to another is possible without collisions doing :
-resetJointState() for each joint (start position)
-SetJointMotorControl2() putting the goal position as targetPosition.

• While(getContactPoints() is empty and currentState is not targetPositon):
  stepSimulation().

Finally I ask for the robot to execute the trajectory returned from my RRT program using setJointMotor2() with the same PD parameters than in the part I find the shortest path in the solution. And I check if there is a collision during the execution of the trajectory.

First of all I would like to know if there is a way to check if a trajectory does not include collisions without having to execute it ( because the PD parameters has a huge impact on the algorithm speed). I thought to use resetJointState() with a small step. It will give me points in [start,goal], but the use of a controller creates point out of this interval (due to inherent overshooting of the controller). Doesn't look like a perfect solution.

Second, when I move the robot through the solution path (that I already check to be collision free), sometimes (one of out 3 or 4) it still collides despite the fact that I move the robot with setJointMotor2() with the same parameters (same goal and same PD).

I know that the use of getClosestPoints() should prevent theses issues (with a proper distance parameter). I will try it in next days.