When solving a KinematicTrajectoryOptimization problem with a minimum distance constraint, and box geometries on both the "robot" and world, I'm consistently getting the following failure when I try to solve the problem:
Traceback (most recent call last):
File "...", line 301, in <module>
result = Solve(trajopt.prog())
RuntimeError: Exception while evaluating SNOPT costs and constraints: 'Failure at geometry/proximity/distance_to_shape_touching.cc:123 in BoxBoxGradient(): condition '(X_WA * p_ACa - X_WB * p_BCb).norm() < kEps' failed.'
Looks like that norm ends up being larger than 1e-14, which is the value used for kEps in BoxBoxGradient. But shouldn't this be caught by the check in CalcDistanceFallback, which uses the same kEps?
I'm able to consistently replicate the bug with this gist on master -- you'll just have to unzip the models folder to run it. Unfortunately, I haven't been able to distill a simpler example. And I'm not familiar enough with Drake's collision engine to debug further.
RussTedrake
commented
3 months ago
What happened?
When solving a KinematicTrajectoryOptimization problem with a minimum distance constraint, and box geometries on both the "robot" and world, I'm consistently getting the following failure when I try to solve the problem:
Looks like that norm ends up being larger than
1e-14, which is the value used forkEpsin BoxBoxGradient. But shouldn't this be caught by the check inCalcDistanceFallback, which uses the samekEps?I'm able to consistently replicate the bug with this gist on master -- you'll just have to unzip the models folder to run it. Unfortunately, I haven't been able to distill a simpler example. And I'm not familiar enough with Drake's collision engine to debug further.
Version
af0551e35f
What operating system are you using?
No response
What installation option are you using?
No response
Relevant log output
No response