Remove MuJoCo stack calls in nativeccd.

1. There is now thread local static memory for use with a conservative max number of iterations needed for contact.
2. For possible needs of high precision contact recovery with large iteration numbers (i.e. ellipsoid-ellipsoid collisions), there are callbacks to allocate memory when needed.

PiperOrigin-RevId: 739187134
Change-Id: I3d210f75218922d969c78465da3fe50da4fe3080
This commit is contained in:
Kyle Bayes
2025-03-21 08:19:42 -07:00
committed by Copybara-Service
parent dc96ed6bdb
commit 12b40b948e
4 changed files with 123 additions and 74 deletions
+12
View File
@@ -17,6 +17,7 @@
#include "src/engine/engine_collision_gjk.h"
#include <array>
#include <cstddef>
#include <vector>
#include <ccd/ccd.h>
@@ -61,6 +62,14 @@ constexpr char kEllipoid[] = R"(
</keyframe>
</mujoco>)";
void* CCDAllocate(void* data, std::size_t nbytes) {
return new std::byte[nbytes];
}
void CCDFree(void* data, void* buffer) {
delete [] (std::byte*)buffer;
}
mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
mjtNum x2[3], mjtNum cutoff = mjMAXVAL) {
mjCCDConfig config;
@@ -127,6 +136,9 @@ int Penetration(mjtNum& depth, std::vector<mjtNum>& dir,
config.max_contacts = max_contacts;
config.dist_cutoff = 0; // no geom distances needed
config.max_contacts = max_contacts;
config.context = nullptr;
config.alloc = CCDAllocate;
config.free = CCDFree;
mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2);
if (dist < 0) {