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

[Jm20010201]

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 !

[Levi-Armstrong]

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]

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]

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]

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]

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.