search

tesseract-robotics / tesseract_planning

Contains packages related to motion and process planning for the Tesseract Motion Planning Environment
http://tesseract-docs.rtfd.io
Other
46 stars 35 forks source link

After adding point clouds to the environment, the solving speed significantly slows down. #506

Open Jm20010201 opened 4 weeks ago

Jm20010201 commented 4 weeks ago

I suspect this is due to slow collision detection. The first picture is the point cloud I added, and the second picture is the collision detection code of my inverse kinematic solution. I am sure that the collision detection slows down the solution. Do you have any good suggestions ? Thanks you very much ! 2024-08-18 17-47-08屏幕截图 2024-08-18 17-49-44屏幕截图

Levi-Armstrong commented 4 weeks ago

Yea, I would agree with your assessment. I have not tested something this significant when it comes to point cloud data. I would recommend trying FCL to see if it is faster in this case. Also I would recommend creating a local state solver outside the for loop and use this for calculate the states which may be a little faster because it avoid a mutex lock.

Jm20010201 commented 4 weeks ago

Yea, I would agree with your assessment. I have not tested something this significant when it comes to point cloud data. I would recommend trying FCL to see if it is faster in this case. Also I would recommend creating a local state solver outside the for loop and use this for calculate the states which may be a little faster because it avoid a mutex lock.

After changing the default collision detection plug-in to FCLDiscreteBVHManager, the program does indeed execute faster.

Jm20010201 commented 4 weeks ago

After I modified the settings of trajopt using your previous suggestions, trajopt can converge with very few iterations. But when I added point clouds, trajopt took significantly longer to execute each iteration. Is there any way to improve this? Or can you recommend a trajectory optimizer that can run in real time?

Levi-Armstrong commented 3 weeks ago

I have not done any testing with something this large so I would need to investigate how to improve the performance. There are several things that could be done depending on what the underlying issues like the following.

  1. Try OSQP GPU version
  2. Try switching OSQP internal QP solver
  3. Try a tesseract's PhysX GPU collision checker. (This has not been updated to the latest so would require some work first to test.)
  4. Try a commercial solver like Gurobi which is supported by TrajOpt.
  5. Implement tooling to simplify the world into fewer primitive shapes.
  6. Might also try passing the point cloud to the convex decomposition algorithm to see how it does at generating multiple convex shapes which represent the environment.
Jm20010201 commented 3 weeks ago

I have not done any testing with something this large so I would need to investigate how to improve the performance. There are several things that could be done depending on what the underlying issues like the following.

  1. Try OSQP GPU version
  2. Try switching OSQP internal QP solver
  3. Try a tesseract's PhysX GPU collision checker. (This has not been updated to the latest so would require some work first to test.)
  4. Try a commercial solver like Gurobi which is supported by TrajOpt.
  5. Implement tooling to simplify the world into fewer primitive shapes.
  6. Might also try passing the point cloud to the convex decomposition algorithm to see how it does at generating multiple convex shapes which represent the environment.

Thank you for your suggestion. Recently, I would like to use ESDF MAP for collision detection and then conduct modification testing.