I am not sure if it is the right place to ask for help.
I am trying to implement discrete time collision term for Trajopt planner, which I forked from @Omid's work.

I am currently using distanceRobot and distanceSelf of CollisionEnvFCL to get the signed distance which is necessary for Trajopt. However, I am not sure if it works properly, since I sometimes get huge collision distance for the the panda robot (larger than 6 meters). Also, robot_state.getJacobian(...) sometimes returns nan values, but dont know the reason for that.

Is this the standard way of getting the signed distance and Jacobian matrix?

Related distance calculation part:
https://github.com/adam-vonderviszt/moveit/blob/b18b8c66963907aa6d03cbee4450fc3d6e740162/moveit_planners/trajopt/trajopt/src/collision_terms.cpp#L110-L122

Related Jacobian calculation part:
https://github.com/adam-vonderviszt/moveit/blob/b18b8c66963907aa6d03cbee4450fc3d6e740162/moveit_planners/trajopt/trajopt/src/collision_terms.cpp#L48-L50