chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Use the geom frame for SDF collisions.

Browse Source 
Also disables mesh reorientation for meshes autogenerated with marching cubes from analytic SDFs.

PiperOrigin-RevId: 781487195
Change-Id: I36b66384f240a764823c489f78f22ba9fe32f15f
This commit is contained in:
Alessio Quaglino
2025-07-10 05:14:09 -07:00
committed by Copybara-Service
parent 7e57a6bc30
commit 2d69610abd
4 changed files with 52 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_sdf.c
+13 -36
View File
@@ -480,24 +480,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
mju_quat2Mat(mat12, quat12);
}
// subtract mesh position from sdf transformation
static void undoTransformation(const mjModel* m, const mjData* d, int g,
mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) {
mjtNum* xpos = d->geom_xpos + 3 * g;
mjtNum* xmat = d->geom_xmat + 9 * g;
if (m->geom_type[g] == mjGEOM_MESH || m->geom_type[g] == mjGEOM_SDF) {
mjtNum negpos[3], negquat[4], xquat[4];
mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g];
mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g];
mju_mat2Quat(xquat, xmat);
mju_negPose(negpos, negquat, pos, quat);
mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
} else {
mju_copy3(sdf_xpos, xpos);
mju_mat2Quat(sdf_quat, xmat);
}
}
//---------------------------- narrow phase -----------------------------------------------
// comparison function for contact sorting
@@ -767,8 +749,7 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int
// collision between a mesh and a signed distance field
int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjtNum* pos1 = d->geom_xpos + 3 * g1;
mjtNum* mat1 = d->geom_xmat + 9 * g1;
mjGETINFO;
mjtNum offset[3], rotation[9], corners[9], x[3], depth;
mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT];
@@ -790,10 +771,10 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
sdf.geomtype = &geomtype;
// compute transformation from g1 to g2
mjtNum pos2true[3], sdf_quat[4], quat1[4];
mjtNum sdf_quat[4], quat1[4];
mju_mat2Quat(quat1, mat1);
undoTransformation(m, d, g2, pos2true, sdf_quat);
mapPose(pos1, quat1, pos2true, sdf_quat, offset, rotation);
mju_mat2Quat(sdf_quat, mat2);
mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation);
// binary tree search
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
@@ -845,7 +826,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
// add only the first mjMAXCONPAIR pairs
for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) {
cnt = addContact(points, con, candidate + 3*index[i], pos2true, sdf_quat,
cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat,
dist[index[i]], cnt, m, &sdf, (mjData*)d);
}
@@ -871,17 +852,13 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
// compute transformations from/to g1 to/from g2
mjtNum quat1[4], quat2[4];
mjtNum pos1true[3], offset21[3], rotation21[9], rotation12[9];
mjtNum pos2true[3], offset1[3], rotation1[9], offset12[3];
mjtNum offset2[3], rotation2[9], squat1[4], squat2[4];
undoTransformation(m, d, g1, pos1true, squat1);
undoTransformation(m, d, g2, pos2true, squat2);
mjtNum offset21[3], rotation21[9], rotation12[9];
mjtNum offset12[3], offset2[3], rotation2[9];
mju_mat2Quat(quat1, mat1);
mju_mat2Quat(quat2, mat2);
mapPose(pos2, quat2, pos1, quat1, offset1, rotation1);
mapPose(pos1, quat1, pos1true, squat1, offset2, rotation2);
mapPose(pos2true, squat2, pos1true, squat1, offset21, rotation21);
mapPose(pos1true, squat1, pos2true, squat2, offset12, rotation12);
mapPose(pos1, quat1, pos1, quat1, offset2, rotation2);
mapPose(pos2, quat2, pos1, quat1, offset21, rotation21);
mapPose(pos1, quat1, pos2, quat2, offset12, rotation12);
// axis-aligned bounding boxes in g1 frame
for (int i=0; i < 8; i++) {
@@ -893,8 +870,8 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
vec2[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]);
vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]);
mju_mulMatVec3(vec2, rotation1, vec2);
mju_addTo3(vec2, offset1);
mju_mulMatVec3(vec2, rotation21, vec2);
mju_addTo3(vec2, offset21);
for (int k=0; k < 3; k++) {
aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]);
@@ -983,7 +960,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
// contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
sdf.type = mjSDFTYPE_MIDSURFACE;
cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d);
cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, (mjData*)d);
// SHOULD NOT OCCUR
if (cnt > mjMAXCONPAIR) {
src/engine/engine_ray.c
+20 -23
View File
@@ -22,6 +22,7 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjsan.h> // IWYU pragma: keep
#include <mujoco/mjvisualize.h>
#include "engine/engine_collision_sdf.h"
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h"
@@ -743,33 +744,27 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
return -1;
}
// get sdf
// get sdf plugin
int instance = m->geom_plugin[g];
const int nslot = mjp_pluginCount();
const int slot = m->plugin[instance];
const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!sdf) mjERROR("invalid plugin slot: %d", slot);
if (!(sdf->capabilityflags & mjPLUGIN_SDF)) {
mjERROR("Plugin is not a sign distance field at slot %d", slot);
}
const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g);
instance = instance == -1 ? m->geom_dataid[g] : instance;
mjtGeom geomtype = mjGEOM_SDF;
// construct sdf struct
mjSDF sdf;
sdf.id = &instance;
sdf.type = mjSDFTYPE_SINGLE;
sdf.plugin = &sdf_ptr;
sdf.geomtype = &geomtype;
// reset counter
sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
// compute transformation
mjtNum sdf_quat[4], sdf_xmat[9], sdf_xpos[9];
mjtNum negpos[3], negquat[4], xquat[4];
mjtNum* xpos = d->geom_xpos + 3*g;
mjtNum* pos = m->mesh_pos + 3*m->geom_dataid[g];
mjtNum* quat = m->mesh_quat + 4*m->geom_dataid[g];
mju_mat2Quat(xquat, d->geom_xmat + 9*g);
mju_negPose(negpos, negquat, pos, quat);
mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);
mju_quat2Mat(sdf_xmat, sdf_quat);
if (sdf_ptr) {
sdf_ptr->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
}
// map to local frame
mjtNum lpnt[3], lvec[3];
ray_map(sdf_xpos, sdf_xmat, pnt, vec, lpnt, lvec);
ray_map(d->geom_xpos + 3*g, d->geom_xmat + 9*g, pnt, vec, lpnt, lvec);
// unit direction
mju_normalize3(lvec);
@@ -777,7 +772,7 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
// ray marching, see e.g. https://en.wikipedia.org/wiki/Ray_marching
for (int i=0; i < 40; i++) {
mju_addScl3(p, lpnt, lvec, distance_total);
mjtNum distance = sdf->sdf_distance(p, (mjData*)d, instance);
mjtNum distance = mjc_distance(m, d, &sdf, p);
distance_total += distance;
if (mju_abs(distance) < kMinDist) {
return distance_total;
@@ -789,7 +784,9 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
}
// reset counter
sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
if (sdf_ptr) {
sdf_ptr->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
}
return -1;
}
src/user/user_mesh.cc
+18 -20
View File
@@ -141,6 +141,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
processed_ = false;
visual_ = true;
needoct_ = false;
needreorient_ = true;
// reset to default if given
if (_def) {
@@ -360,6 +361,7 @@ void mjCMesh::LoadSDF() {
userface.push_back(index);
}
needreorient_ = false;
normal_ = std::move(usernormal);
face_ = std::move(userface);
ProcessVertices(uservert);
@@ -1533,22 +1535,26 @@ void mjCMesh::Process() {
boxsz_[1] = 0.5 * std::sqrt(6*(eigval[0] + eigval[2] - eigval[1])/volume);
boxsz_[2] = 0.5 * std::sqrt(6*(eigval[0] + eigval[1] - eigval[2])/volume);
// make octree in the geom frame
// TODO: make octree in the mesh frame, update engine_collision_sdf
// prevent reorientation if the mesh was autogenerated using marching cubes
if (!needreorient_) {
mjuu_setvec(CoM, 0, 0, 0);
mjuu_setvec(quattmp, 1, 0, 0, 0);
}
// transform CoM to origin
for (int i=0; i < nvert(); i++) {
vert_[3*i + 0] -= CoM[0];
vert_[3*i + 1] -= CoM[1];
vert_[3*i + 2] -= CoM[2];
}
Rotate(quattmp);
// make octree in mesh frame
if (!needoct_) {
octree_.Clear();
} else if (octree_.Nodes().empty()) {
double aamm[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
for (int i = 0; i < nvert(); i++) {
aamm[0] = std::min(aamm[0], vert_[3*i + 0]);
aamm[3] = std::max(aamm[3], vert_[3*i + 0]);
aamm[1] = std::min(aamm[1], vert_[3*i + 1]);
aamm[4] = std::max(aamm[4], vert_[3*i + 1]);
aamm[2] = std::min(aamm[2], vert_[3*i + 2]);
aamm[5] = std::max(aamm[5], vert_[3*i + 2]);
}
octree_.SetFace(vert_, face_);
octree_.CreateOctree(aamm);
octree_.CreateOctree(aamm_);
// compute sdf coefficients
if (!plugin.active) {
@@ -1569,14 +1575,6 @@ void mjCMesh::Process() {
}
}
// transform CoM to origin
for (int i=0; i < nvert(); i++) {
vert_[3*i + 0] -= CoM[0];
vert_[3*i + 1] -= CoM[1];
vert_[3*i + 2] -= CoM[2];
}
Rotate(quattmp);
// save the pos and quat that was used to transform the mesh
mjuu_copyvec(pos_, CoM, 3);
mjuu_copyvec(quat_, quattmp, 4);
src/user/user_objects.h
+1
View File
@@ -1003,6 +1003,7 @@ class mjCMesh_ : public mjCBase {
std::vector<int> spec_facetexcoord_;
// used by the compiler
bool needreorient_; // needs reorientation
bool needoct_; // needs octree
bool visual_; // true: the mesh is only visual
std::vector< std::pair<int, int> > halfedge_; // half-edge data