What is the standard way of getting the signed distance and Jacobian matrix (Trajopt)?

asked 2019-12-18 20:42:51 -0500

blabla gravatar image

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/m...

Related Jacobian calculation part:
https://github.com/adam-vonderviszt/m...

edit retag flag offensive close merge delete

Comments

hi , did you able to solve this? if yes , can you explain please.

prajwal98 gravatar image prajwal98  ( 2022-05-10 13:24:41 -0500 )edit