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

Add Signed Distance Field to collision geometries.

Browse Source 
PiperOrigin-RevId: 557507088
Change-Id: I358a642407aee1ba8dfc9d405eb9a4f609435fe9
This commit is contained in:
Alessio Quaglino
2023-08-16 09:14:29 -07:00
committed by Copybara-Service
parent 6245edae28
commit fdb041580c
57 changed files with 3229 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/CMakeLists.txt
+2
View File
@@ -23,6 +23,8 @@ set(MUJOCO_ENGINE_SRCS
engine_collision_driver.h
engine_collision_primitive.c
engine_collision_primitive.h
engine_collision_sdf.c
engine_collision_sdf.h
engine_core_constraint.c
engine_core_constraint.h
engine_core_smooth.c
src/engine/engine_collision_driver.c
+11 -9
View File
@@ -23,6 +23,7 @@
#include "engine/engine_callback.h"
#include "engine/engine_collision_convex.h"
#include "engine/engine_collision_primitive.h"
#include "engine/engine_collision_sdf.h"
#include "engine/engine_core_constraint.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
@@ -34,15 +35,16 @@
// table of pair-wise collision functions
mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = {
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH */
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex},
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField},
/*SHPERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex},
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex},
/*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex},
/*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex},
/*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex},
/*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex}
/* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */
/*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_SDF},
/*HFIELD */ {0, 0, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_ConvexHField, mjc_HFieldSDF},
/*SPHERE */ {0, 0, mjc_SphereSphere, mjc_SphereCapsule, mjc_Convex, mjc_SphereCylinder, mjc_SphereBox, mjc_Convex, mjc_SDF},
/*CAPSULE */ {0, 0, 0, mjc_CapsuleCapsule, mjc_Convex, mjc_Convex, mjc_CapsuleBox, mjc_Convex, mjc_SDF},
/*ELLIPSOID */ {0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_Convex, mjc_SDF},
/*CYLINDER */ {0, 0, 0, 0, 0, mjc_Convex, mjc_Convex, mjc_Convex, mjc_SDF},
/*BOX */ {0, 0, 0, 0, 0, 0, mjc_BoxBox, mjc_Convex, mjc_SDF},
/*MESH */ {0, 0, 0, 0, 0, 0, 0, mjc_Convex, mjc_MeshSDF},
/*SDF */ {0, 0, 0, 0, 0, 0, 0, 0, mjc_SDF}
};
src/engine/engine_collision_sdf.c
+695
View File
@@ -0,0 +1,695 @@
// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "engine/engine_collision_sdf.h"
#include <stdio.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include "engine/engine_collision_primitive.h"
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
#define MAXSDFFACE 1300
//---------------------------- primitives sdf ---------------------------------------------
static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p,
int i, const mjtNum x[3], mjtGeom type) {
mjtNum a[3], b[3];
const mjtNum* size = m->geom_size+3*i;
// see https://iquilezles.org/articles/distfunctions/
switch (type) {
case mjGEOM_PLANE:
return x[2];
case mjGEOM_SPHERE:
return mju_norm3(x) - size[0];
case mjGEOM_BOX:
a[0] = mju_abs(x[0]) - size[0];
a[1] = mju_abs(x[1]) - size[1];
a[2] = mju_abs(x[2]) - size[2];
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
b[2] = mju_max(a[2], 0);
return mju_norm3(b) + mju_min(mju_max(a[0], mju_max(a[1], a[2])), 0);
case mjGEOM_CAPSULE:
a[0] = x[0];
a[1] = x[1];
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
return mju_norm3(a) - size[0];
case mjGEOM_ELLIPSOID:
a[0] = x[0] / size[0];
a[1] = x[1] / size[1];
a[2] = x[2] / size[2];
b[0] = a[0] / size[0];
b[1] = a[1] / size[1];
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
return k0 * (k0 - 1.0) / k1;
case mjGEOM_CYLINDER:
a[0] = mju_sqrt(x[0]*x[0]+x[1]*x[1]) - size[0];
a[1] = mju_abs(x[2]) - size[1];
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2);
case mjGEOM_SDF:
return p->sdf_distance(x, d, i);
default:
mju_error("sdf collisions not available for geom type %d", type);
return 0;
}
}
static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d,
const mjpPlugin* p, int i, const mjtNum x[3],
mjtGeom type) {
mjtNum a[3], b[3], c, e;
const mjtNum* size = m->geom_size+3*i;
// see https://iquilezles.org/articles/distfunctions/
switch (type) {
case mjGEOM_PLANE:
mju_zero3(gradient);
gradient[2] = 1;
break;
case mjGEOM_SPHERE:
mju_copy3(gradient, x);
c = mju_norm3(x);
gradient[0] *= 1. / c;
gradient[1] *= 1. / c;
gradient[2] *= 1. / c;
break;
case mjGEOM_BOX:
mju_zero3(gradient);
a[0] = mju_abs(x[0]) - size[0];
a[1] = mju_abs(x[1]) - size[1];
a[2] = mju_abs(x[2]) - size[2];
int k = a[0] > a[1] ? 0 : 1;
int l = a[2] > a[k] ? 2 : k;
if (a[l]<0) {
gradient[l] = x[l] / mju_abs(x[l]);
} else {
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
b[2] = mju_max(a[2], 0);
c = mju_norm3(b);
gradient[0] = a[0]>0 ? b[0] / c * x[0] / mju_abs(x[0]) : 0;
gradient[1] = a[1]>0 ? b[1] / c * x[1] / mju_abs(x[1]) : 0;
gradient[2] = a[2]>0 ? b[2] / c * x[2] / mju_abs(x[2]) : 0;
}
break;
case mjGEOM_CAPSULE:
a[0] = x[0];
a[1] = x[1];
a[2] = x[2] - mju_clip(x[2], -size[1], size[1]);
c = mju_norm3(a);
gradient[0] = a[0] / c;
gradient[1] = a[1] / c;
gradient[2] = a[2] / c;
break;
case mjGEOM_ELLIPSOID:
a[0] = x[0] / size[0];
a[1] = x[1] / size[1];
a[2] = x[2] / size[2];
b[0] = a[0] / size[0];
b[1] = a[1] / size[1];
b[2] = a[2] / size[2];
mjtNum k0 = mju_norm3(a);
mjtNum k1 = mju_norm3(b);
gradient[0] = a[0]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[0]/(k1*k1);
gradient[1] = a[1]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[1]/(k1*k1);
gradient[2] = a[2]*(2.*k0 - 1.) / k1 + k0*(k0 - 1.) * b[2]/(k1*k1);
break;
case mjGEOM_CYLINDER:
c = mju_sqrt(x[0]*x[0]+x[1]*x[1]);
e = mju_abs(x[2]);
a[0] = c - size[0];
a[1] = e - size[1];
mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e};
int j = a[0] > a[1] ? 0 : 1;
if (a[j] < 0) {
gradient[0] = j==0 ? grada[0] : 0;
gradient[1] = j==0 ? grada[1] : 0;
gradient[2] = j==1 ? grada[2] : 0;
} else {
b[0] = mju_max(a[0], 0);
b[1] = mju_max(a[1], 0);
mjtNum bnorm = mju_norm(b, 2);
gradient[0] = grada[0] * b[0] / bnorm;
gradient[1] = grada[1] * b[0] / bnorm;
gradient[2] = grada[2] * b[1] / bnorm;
}
break;
case mjGEOM_SDF:
p->sdf_gradient(gradient, x, d, i);
break;
default:
mju_error("sdf collisions not available for geom type %d", type);
}
}
//---------------------------- helper functions -------------------------------------------
// signed distance function
mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]) {
mjtNum y[3];
switch (s->type) {
case mjSDFTYPE_SINGLE:
return geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
return mju_max(geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]),
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]));
default:
mju_error("SDF type not available");
return 0;
}
}
// gradient of sdf
void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s,
mjtNum gradient[3], const mjtNum x[3]) {
mjtNum y[3];
const mjtNum* point[2] = {x, y};
switch (s->type) {
case mjSDFTYPE_INTERSECTION:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
int i = geomDistance(m, d, s->plugin[0], s->id[0], x, s->geomtype[0]) >
geomDistance(m, d, s->plugin[1], s->id[1], y, s->geomtype[1]) ? 0 : 1;
geomGradient(gradient, m, d, s->plugin[i], s->id[i], point[i], s->geomtype[i]);
if (i==1) {
mju_rotVecMatT(gradient, gradient, s->relmat);
}
break;
case mjSDFTYPE_AVERAGE:
mju_rotVecMat(y, x, s->relmat);
mju_addTo3(y, s->relpos);
mjtNum grad1[3], grad2[3];
geomGradient(grad1, m, d, s->plugin[0], s->id[0], x, s->geomtype[0]);
mju_normalize3(grad1);
geomGradient(grad2, m, d, s->plugin[1], s->id[1], y, s->geomtype[1]);
mju_rotVecMatT(grad2, grad2, s->relmat);
mju_normalize3(grad2);
mju_sub3(gradient, grad1, grad2);
mju_normalize3(gradient);
break;
case mjSDFTYPE_SINGLE:
geomGradient(gradient, m, d, s->plugin[0], s->id[0], point[0], s->geomtype[0]);
break;
default:
mju_error("SDF type not available");
}
}
// get sdf from geom id
static const mjpPlugin* getSDF(const mjModel* m, int id) {
int instance = m->geom_plugin[id];
const int nslot = mjp_pluginCount();
const int slot = m->plugin[instance];
const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot);
if (!sdf) mju_error("invalid plugin slot: %d", slot);
if (!(sdf->capabilityflags & mjPLUGIN_SDF)) {
mju_error("Plugin is not a signed distance field at slot %d", slot);
}
return sdf;
}
// map (pos12, mat12) as (xpos2, xmat2)^-1 \circ (xpos1, xmat1)
static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
const mjtNum xpos2[3], const mjtNum xquat2[4],
mjtNum pos12[3], mjtNum mat12[9]) {
mjtNum negpos[3], negquat[4], quat12[4];
mju_negPose(negpos, negquat, xpos2, xquat2);
mju_mulPose(pos12, quat12, negpos, negquat, xpos1, xquat1);
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->geom_pos + 3 * g;
mjtNum* quat = m->geom_quat + 4 * 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 -----------------------------------------------
// check if the collision point already exists
static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
for (int i = 0; i < cnt; i++) {
if (mju_dist3(x, points + 3*i) < mjMINVAL) {
return 1;
}
}
return 0;
}
// adds candidate point to result
static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
int cnt, const mjModel* m, const mjSDF* s, mjData* d) {
// check if there is a collision
if (dist > 0 || isknown(points, x, cnt)) {
return cnt;
} else {
mju_copy3(points+3*cnt, x);
}
// compute normal in local coordinates
mjtNum norm[3], vec[3];
mjc_gradient(m, d, s, norm, x);
mju_scl3(norm, norm, -1);
// construct contact
con[cnt].dist = dist;
mju_rotVecQuat(con[cnt].frame, norm, quat2);
mju_zero3(con[cnt].frame+3);
mju_makeFrame(con[cnt].frame);
mju_scl3(vec, con[cnt].frame, -con[cnt].dist/2);
mju_rotVecQuat(con[cnt].pos, x, quat2);
mju_addTo3(con[cnt].pos, pos2);
mju_addTo3(con[cnt].pos, vec);
return cnt+1;
}
// finds minimum of Frank-Wolfe objective
static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
const mjModel* m, const mjSDF* sdf, mjData* d) {
for (int step=0; step<m->opt.sdf_iterations; step++) {
mjtNum best = 1e10, fun, s[3], grad[3];
// evaluate gradient
mjc_gradient(m, d, sdf, grad, x);
// evaluate all corners
for (int i=0; i<ncorners; i++) {
// compute sdf
fun = mju_dot3(corners + 3*i, grad);
// save argmin
if (fun<best) {
best = fun;
mju_copy3(s, corners + 3*i);
}
}
// update collision point
mju_subFrom3(s, x);
mju_addToScl3(x, s, 2. / (step+2.));
}
// compute distance
return mjc_distance(m, d, sdf, x);
}
// finds minimum using gradient descent
static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
mjData* d) {
mjtNum alpha = 0.2; // step along the gradient direction
for (int step=0; step<m->opt.sdf_iterations; step++) {
mjtNum grad[3];
// evaluate gradient
mjc_gradient(m, d, s, grad, x);
// sanity check
if (isnan(grad[0]) || isnan(grad[1]) || isnan(grad[2])) {
return mjMAXVAL;
}
// update solution
mju_addToScl3(x, grad, -alpha/(mjtNum)(step+1));
}
// compute distance
return mjc_distance(m, d, s, x);
}
//---------------------------- bounding box vs sdf -------------------------------------------------
// stricter triangle collision
static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
const mjSDF* sdf, mjData* d) {
mjtNum edges[6];
mjtNum normal[3], center[3];
mjtNum v[9], cross[9], p[3];
mjtNum kDistanceScl = 10.;
// triangle normal
mju_sub3(edges+0, triangle+3, triangle);
mju_sub3(edges+3, triangle+6, triangle);
mju_cross(normal, edges, edges+3);
mju_normalize3(normal);
// fourth point
mju_scl3(p, triangle, 1./3.);
mju_addToScl3(p, triangle+3, 1./3.);
mju_addToScl3(p, triangle+6, 1./3.);
mjtNum h = -mjc_distance(m, d, sdf, p)/kDistanceScl;
mju_addToScl3(p, normal, -h);
// circumsphere center
mju_sub3(v+0, triangle+0, p);
mju_sub3(v+3, triangle+3, p);
mju_sub3(v+6, triangle+6, p);
mju_cross(cross+0, v+3, v+6);
mju_cross(cross+3, v+6, v+0);
mju_cross(cross+6, v+0, v+3);
mju_scl3(center, cross, mju_dot3(v, v));
mju_addToScl3(center, cross+3, mju_dot3(v+3, v+3));
mju_addToScl3(center, cross+6, mju_dot3(v+6, v+6));
mju_scl3(center, center, 1./(2.*mju_dot3(v, cross)));
// circumsphere radius
mjtNum r = mju_sqrt(mju_dot3(center, center));
// coordinate change
mju_addTo3(center, p);
return mjc_distance(m, d, sdf, center)<r;
}
// intersect with circumsphere of bounding box
static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
const mjtNum rotation[9], const mjModel* m,
const mjSDF* s, mjData* d) {
mjtNum candidate[3];
mjtNum r = mju_norm3(bvh+3);
mju_rotVecMat(candidate, bvh, rotation);
mju_addTo3(candidate, offset);
// check if inside the bounding box
return mjc_distance(m, d, s, candidate)<r;
}
//---------------------------- mesh vs sdf broad phase --------------------------------------------
// tree vs sdf binary search
static void collideBVH(const mjModel* m, mjData* d, int g,
const mjtNum offset[3], const mjtNum rotation[9],
int* faces, int* npoints, int* n0,
const mjSDF* sdf) {
const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
const int* faceid = m->bvh_geomid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
mjtByte* visited = d->bvh_active + bvhadr;
mjMARKSTACK;
// TODO(quaglino): Store bvh max depths to make this bound tighter.
int max_stack = m->mesh_bvhnum[m->geom_dataid[g]];
struct CollideTreeArgs_ {
int node;
};
typedef struct CollideTreeArgs_ CollideTreeArgs;
CollideTreeArgs* stack = (CollideTreeArgs*)mj_stackAlloc(
d, max_stack * sizeof(CollideTreeArgs*) / sizeof(mjtNum));
int nstack = 0;
stack[nstack].node = 0;
nstack++;
while (nstack) {
(*n0)++;
// pop from stack
nstack--;
int node = stack[nstack].node;
// node1 is a leaf
if (faceid[node] != -1) {
if (visited[node]) {
continue;
}
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
faces[*npoints] = faceid[node];
if (++(*npoints)==MAXSDFFACE) {
mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed");
mjFREESTACK;
return;
}
visited[node] = 1;
}
continue;
}
// if no intersection at intermediate levels, stop
if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) {
continue;
}
visited[node] = 1;
// recursive call
for (int i=0; i<2; i++) {
if (child[2*node+i] != -1) {
if (nstack >= max_stack) mju_error("BVH stack depth exceeded.");
stack[nstack].node = child[2*node+i];
nstack++;
}
}
}
mjFREESTACK;
}
//------------------------------ collision functions -----------------------------------------------
// collision between a height field and a signed distance field
int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mju_warning("HField vs SDF collision not yet supported!");
return 0;
}
// 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;
mjtNum offset[3], rotation[9];
mjtNum corners[9], points[3*MAXSDFFACE], x[3], dist;
int vertadr = m->mesh_vertadr[m->geom_dataid[g1]];
int faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
int cnt=0, npoints=0, n0=0, faces[MAXSDFFACE]={-1};
// get sdf plugin
int instance = m->geom_plugin[g2];
const mjpPlugin* sdf_ptr = getSDF(m, g2);
mjtGeom geomtype = mjGEOM_SDF;
// copy into data
mjSDF sdf;
sdf.id = &instance;
sdf.type = mjSDFTYPE_SINGLE;
sdf.plugin = &sdf_ptr;
sdf.geomtype = &geomtype;
// compute transformation from g1 to g2
mjtNum pos2true[3], sdf_quat[4], quat1[4];
mju_mat2Quat(quat1, mat1);
undoTransformation(m, d, g2, pos2true, sdf_quat);
mapPose(pos1, quat1, pos2true, sdf_quat, offset, rotation);
// binary tree search
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf);
// Frank-Wolfe algorithm
for (int i=0; i<npoints; i++) {
int face = faceadr + faces[i];
for (int v=0; v<3; v++) {
mjtNum vec[3] = {
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0],
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1],
m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2],
};
// transform local 1 (mesh) to local 2 (sdf)
mju_rotVecMat(corners+3*v, vec, rotation);
mju_addTo3(corners+3*v, offset);
}
// stricter culling
if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) {
continue;
}
// starting point
x[0] = (corners[0]+corners[3]+corners[6])/3;
x[1] = (corners[1]+corners[4]+corners[7])/3;
x[2] = (corners[2]+corners[5]+corners[8])/3;
// SHOULD NOT OCCUR
if (cnt==mjMAXCONPAIR) mju_error("mjc_MeshSDF: too many contact points");
// Frank-Wolfe
dist = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d);
cnt = addContact(points, con, x, pos2true, sdf_quat, dist, cnt, m, &sdf, (mjData*)d);
}
return cnt;
}
// collision between two SDFs
int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO;
size1 = m->geom_aabb + 6*g1;
size2 = m->geom_aabb + 6*g2;
int cnt = 0;
mjtNum x[3], y[3], dist, vec1[3], vec2[3];
mjtNum aabb1[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum aabb2[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum aabb[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
// second geom must be an SDF
if (m->geom_type[g2] != mjGEOM_SDF) {
mju_error("geom is not an SDF");
}
// 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);
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);
// axis-aligned bounding boxes in g1 frame
for (int i=0; i<8; i++) {
vec1[0] = (i&1 ? size1[0]+size1[3] : size1[0]-size1[3]);
vec1[1] = (i&2 ? size1[1]+size1[4] : size1[1]-size1[4]);
vec1[2] = (i&4 ? size1[2]+size1[5] : size1[2]-size1[5]);
vec2[0] = (i&1 ? size2[0]+size2[3] : size2[0]-size2[3]);
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_rotVecMat(vec2, vec2, rotation1);
mju_addTo3(vec2, offset1);
for (int k=0; k<3; k++) {
aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]);
aabb1[3+k] = mju_max(aabb1[3+k], vec1[k]);
aabb2[0+k] = mju_min(aabb2[0+k], vec2[k]);
aabb2[3+k] = mju_max(aabb2[3+k], vec2[k]);
}
}
// intersection of aabbs
for (int k=0; k<3; k++) {
aabb[0+k] = mju_max(aabb1[0+k], aabb2[0+k]);
aabb[3+k] = mju_min(aabb1[3+k], aabb2[3+k]);
}
// no intersection if max < min
if (aabb[3]<aabb[0] || aabb[4]<aabb[1] || aabb[5]<aabb[2]) {
return cnt;
}
// create sdf pointers
int instance[2];
const mjpPlugin* sdf_ptr[2];
mjtGeom geomtypes[2] = {m->geom_type[g2], m->geom_type[g1]};
instance[0] = m->geom_plugin[g2];
sdf_ptr[0] = getSDF(m, g2);
// get sdf plugins
if (m->geom_type[g1] == mjGEOM_SDF) {
instance[1] = m->geom_plugin[g1];
sdf_ptr[1] = getSDF(m, g1);
} else {
instance[1] = g1;
sdf_ptr[1] = NULL;
}
// reset visualization count
sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]);
// copy into sdf
mjSDF sdf;
sdf.id = instance;
sdf.relpos = offset21;
sdf.relmat = rotation21;
sdf.plugin = sdf_ptr;
sdf.geomtype = geomtypes;
// minimize sdf intersection
mjtNum contacts[3*mjMAXCONPAIR];
int i = 0, j = 0;
while (i<m->opt.sdf_initpoints) {
x[0] = aabb[0] + (aabb[3]-aabb[0]) * mju_Halton(j, 2);
x[1] = aabb[1] + (aabb[4]-aabb[1]) * mju_Halton(j, 3);
x[2] = aabb[2] + (aabb[5]-aabb[2]) * mju_Halton(j, 5);
mju_rotVecMat(y, x, rotation2);
mju_addTo3(y, offset2);
mju_rotVecMat(x, y, rotation12);
mju_addTo3(x, offset12);
j++;
// here a criterion for rejecting points could be inserted
i++;
// start counters
sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
// gradient descent
sdf.type = mjSDFTYPE_INTERSECTION;
dist = stepGradient(x, m, &sdf, (mjData*)d);
sdf.type = mjSDFTYPE_AVERAGE;
cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d);
}
return cnt;
}
src/engine/engine_collision_sdf.h
+62
View File
@@ -0,0 +1,62 @@
// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_
#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_
#include <mujoco/mjdata.h>
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef enum mjtSDFType_ {
mjSDFTYPE_SINGLE = 0,
mjSDFTYPE_INTERSECTION,
mjSDFTYPE_AVERAGE,
} mjtSDFType;
struct mjSDF_ {
const mjpPlugin** plugin;
int* id;
mjtSDFType type;
mjtNum* relpos;
mjtNum* relmat;
mjtGeom* geomtype;
};
typedef struct mjSDF_ mjSDF;
// signed distance function
MJAPI mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]);
// gradient of sdf
MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNum gradient[3],
const mjtNum x[3]);
// collision between a height field and a signed distance field
int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
// 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);
// collision between two signed distance fields
int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
#ifdef __cplusplus
}
#endif
#endif // MUJOCO_SRC_ENGINE_ENGINE_COLLISION_SDF_H_
src/engine/engine_io.c
+5 -1
View File
@@ -123,6 +123,10 @@ void mj_defaultOption(mjOption* opt) {
opt->mpr_iterations = 50;
opt->disableflags = 0;
opt->enableflags = 0;
// sdf collisions
opt->sdf_initpoints = 40;
opt->sdf_iterations = 10;
}
@@ -1716,7 +1720,7 @@ const char* mj_validateReferences(const mjModel* m) {
if (m->geom_dataid[i] >= m->nhfield || m->geom_dataid[i] < -1) {
return "Invalid model: geom_dataid out of bounds.";
}
} else if (m->geom_type[i] == mjGEOM_MESH) {
} else if ((m->geom_type[i]==mjGEOM_MESH) || (m->geom_type[i]==mjGEOM_SDF)) {
if (m->geom_dataid[i] >= m->nmesh || m->geom_dataid[i] < -1) {
return "Invalid model: geom_dataid out of bounds.";
}
src/engine/engine_plugin.cc
+1 -1
View File
@@ -55,7 +55,7 @@ namespace {
constexpr int kMaxNameLength = 1024;
constexpr int kMaxAttributes = 255;
constexpr int kCacheLine = 64;
constexpr int kCacheLine = 256;
// vfs prefix
constexpr const char* kVfsPrefix = mjVFS_PREFIX;
src/engine/engine_ray.c
+66
View File
@@ -22,6 +22,7 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjvisualize.h>
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
@@ -721,6 +722,67 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
return x;
}
// intersect ray with signed distance field
mjtNum ray_sdf(const mjModel* m, const mjData* d, int g,
const mjtNum* pnt, const mjtNum* vec) {
mjtNum distance_total = 0;
mjtNum p[3];
// exclude using bounding box
if (ray_box(d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pnt, vec, NULL) < 0) {
return -1;
}
// get sdf
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) mju_error("invalid plugin slot: %d", slot);
if (!(sdf->capabilityflags & mjPLUGIN_SDF)) {
mju_error("Plugin is not a sign distance field at slot %d", slot);
}
// 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->geom_pos + 3*g;
mjtNum* quat = m->geom_quat + 4*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);
// unit direction
mjtNum dir[3] = {vec[0], vec[1], vec[2]};
mju_normalize3(dir);
// ray marching
for (int i=0; i < 40; i++) {
mju_addScl3(p, pnt, dir, distance_total);
mju_subFrom3(p, sdf_xpos);
mju_rotVecMatT(p, p, sdf_xmat);
mjtNum distance = sdf->sdf_distance(p, (mjData*)d, instance);
distance_total += distance;
if (distance < 1e-8) {
return distance_total;
}
}
// reset counter
sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance);
return -1;
}
// intersect ray with mesh
mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
const mjtNum* pnt, const mjtNum* vec) {
@@ -911,6 +973,8 @@ mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum* pnt, const mjtNum
newdist = mj_rayMesh(m, d, i, pnt, vec);
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
newdist = mj_rayHfield(m, d, i, pnt, vec);
} else if (m->geom_type[i] == mjGEOM_SDF) {
newdist = ray_sdf(m, d, i, pnt, vec);
}
// otherwise general dispatch
@@ -1068,6 +1132,8 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
newdist = mj_rayMesh(m, d, i, pnt, vec);
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
newdist = mj_rayHfield(m, d, i, pnt, vec);
} else if (m->geom_type[i] == mjGEOM_SDF) {
newdist = ray_sdf(m, d, i, pnt, vec);
}
// otherwise general dispatch
src/engine/engine_vis_init.c
+2 -1
View File
@@ -92,7 +92,8 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"},
{"Body Tree", "0", "`"},
{"Mesh Tree", "0", ""}
{"Mesh Tree", "0", ""},
{"SDF iters", "0", ""}
};
src/engine/engine_vis_visualize.c
+8 -3
View File
@@ -647,6 +647,10 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjtNum* xpos = d->geom_xpos + 3 * geomid;
const mjtNum* xmat = d->geom_xmat + 9 * geomid;
if (!d->bvh_active[i]) {
continue;
}
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
@@ -1053,7 +1057,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int k=geomadr; k < geomadr+geomnum; k++) {
int geomtype = m->geom_type[k];
// add inflated geom if it is a regular primitive
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD && geomtype != mjGEOM_MESH) {
if (geomtype != mjGEOM_PLANE && geomtype != mjGEOM_HFIELD &&
geomtype != mjGEOM_MESH && geomtype != mjGEOM_SDF) {
START
// inflate sizes by 5%
mju_scl3(sz, m->geom_size+3*k, 1.05);
@@ -1183,7 +1188,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
setMaterial(m, thisgeom, geom_matid, rgba, vopt->flags);
// set texcoord
if (m->geom_type[i] == mjGEOM_MESH &&
if ((m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) &&
m->geom_dataid[i] >= 0 &&
m->mesh_texcoordadr[m->geom_dataid[i]] >= 0) {
thisgeom->texcoord = 1;
@@ -1205,7 +1210,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// mesh: 2*i is original, 2*i+1 is convex hull
if (m->geom_type[i] == mjGEOM_MESH) {
if (m->geom_type[i] == mjGEOM_MESH || m->geom_type[i] == mjGEOM_SDF) {
thisgeom->dataid *= 2;
if (m->mesh_graphadr[m->geom_dataid[i]] >= 0 && vopt->flags[mjVIS_CONVEXHULL] &&
(m->geom_contype[i] || m->geom_conaffinity[i])) {
src/render/render_gl3.c
+1
View File
@@ -356,6 +356,7 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
break;
case mjGEOM_MESH: // mesh
case mjGEOM_SDF:
if (geom->dataid >= 0) {
glCallList(con->baseMesh + geom->dataid);
}
src/user/user_mesh.cc
+78 -1
View File
@@ -27,7 +27,7 @@
#define TINYOBJLOADER_IMPLEMENTATION
#endif
#include <mujoco/mjmacro.h>
#include <MC.h>
#include <mujoco/mjmodel.h>
#include "cc/array_safety.h"
#include "engine/engine_crossplatform.h"
@@ -256,6 +256,78 @@ void mjCMesh::set_needhull(bool needhull) {
// generate mesh using marching cubes
void mjCMesh::LoadSDF() {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for mesh '%s', (id = %d)",
name.c_str(), id);
}
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(plugin->capabilityflags & mjPLUGIN_SDF)) {
throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name);
}
int i=0;
mjtNum attributes[10] = {0};
for (auto const& pair : plugin_instance->config_attribs) {
attributes[i++] = std::stod(pair.second);
}
mjtNum aabb[6] = {0};
plugin->sdf_aabb(aabb, attributes);
mjtNum total = aabb[3] + aabb[4] + aabb[5];
const mjtNum n = 300;
int nx, ny, nz;
nx = floor(n / total * aabb[3]) + 1;
ny = floor(n / total * aabb[4]) + 1;
nz = floor(n / total * aabb[5]) + 1;
MC::MC_FLOAT* field = new MC::MC_FLOAT[nx * ny * nz];
for (int i = 0; i < nx; i++) {
for (int j = 0; j < ny; j++) {
for (int k = 0; k < nz; k++) {
mjtNum point[] = {aabb[0]-aabb[3] + 2 * aabb[3] * i / (nx-1),
aabb[1]-aabb[4] + 2 * aabb[4] * j / (ny-1),
aabb[2]-aabb[5] + 2 * aabb[5] * k / (nz-1)};
field[(k * ny + j) * nx + i] = plugin->sdf_staticdistance(point, attributes);
}
}
}
MC::mcMesh mesh;
MC::marching_cube(field, nx, ny, nz, mesh);
std::vector<float> uservert;
std::vector<float> usernormal;
std::vector<int> userface;
for (size_t i = 0; i < mesh.vertices.size(); i++) {
uservert.push_back(2*aabb[3]*mesh.vertices.at(i).x/(nx-1) + aabb[0]-aabb[3]);
uservert.push_back(2*aabb[4]*mesh.vertices.at(i).y/(ny-1) + aabb[1]-aabb[4]);
uservert.push_back(2*aabb[5]*mesh.vertices.at(i).z/(nz-1) + aabb[2]-aabb[5]);
}
for (size_t i = 0; i < mesh.normals.size(); i++) {
usernormal.push_back(mesh.normals.at(i).x);
usernormal.push_back(mesh.normals.at(i).y);
usernormal.push_back(mesh.normals.at(i).z);
}
for (size_t i = 0; i < mesh.indices.size(); i++) {
userface.push_back(mesh.indices.at(i));
}
set_uservert(uservert);
set_usernormal(usernormal);
set_userface(userface);
delete[] field;
}
// compiler
void mjCMesh::Compile(int vfs_provider) {
// load file
@@ -293,6 +365,11 @@ void mjCMesh::Compile(int vfs_provider) {
}
}
// create using marching cubes
if (is_plugin) {
LoadSDF();
}
// copy user vertex
if (!uservert_.empty()) {
// check repeated
src/user/user_model.cc
+8
View File
@@ -2743,6 +2743,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, int vfs_provider) {
}
}
for (int i = 0; i < ngeom; ++i) {
if (geoms[i]->is_plugin) {
m->geom_plugin[i] = geoms[i]->plugin_instance->id;
} else {
m->geom_plugin[i] = -1;
}
}
std::vector<std::vector<int>> plugin_to_sensors(nplugin);
for (int i = 0; i < nsensor; ++i) {
if (sensors[i]->type == mjSENS_PLUGIN) {
src/user/user_objects.cc
+34 -11
View File
@@ -484,6 +484,12 @@ mjCBase::mjCBase() {
xmlpos[0] = xmlpos[1] = -1;
model = 0;
def = 0;
// plugin variables
is_plugin = false;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
}
@@ -555,12 +561,6 @@ mjCBody::mjCBody(mjCModel* _model) {
gravcomp = 0;
userdata.clear();
// plugin variables
is_plugin = false;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
// clear object lists
bodies.clear();
geoms.clear();
@@ -1284,6 +1284,12 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
// set model, def
model = _model;
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
// plugin variables
is_plugin = false;
plugin_instance = nullptr;
plugin_name = "";
plugin_instance_name = "";
}
@@ -1293,7 +1299,7 @@ double mjCGeom::GetVolume(void) {
double height;
// get from mesh
if (type==mjGEOM_MESH) {
if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
if (meshid<0 || meshid>=(int)model->meshes.size()) {
throw mjCError(this, "invalid meshid in mesh geom '%s' (id = %d)", name.c_str(), id);
}
@@ -1353,7 +1359,7 @@ void mjCGeom::SetInertia(void) {
double height;
// get from mesh
if (type==mjGEOM_MESH) {
if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
if (meshid<0 || meshid>=(int)model->meshes.size()) {
throw mjCError(this, "invalid meshid in mesh geom '%s' (id = %d)", name.c_str(), id);
}
@@ -1440,6 +1446,7 @@ double mjCGeom::GetRBound(void) {
return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]);
case mjGEOM_MESH:
case mjGEOM_SDF:
aabb = model->meshes[meshid]->aabb();
haabb[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3]));
haabb[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4]));
@@ -1586,6 +1593,7 @@ void mjCGeom::ComputeAABB() {
break;
case mjGEOM_MESH:
case mjGEOM_SDF:
mjuu_copyvec(aabb, model->meshes[meshid]->aabb(), 6);
break;
@@ -1647,7 +1655,7 @@ void mjCGeom::Compile(void) {
}
// check mesh
if (type==mjGEOM_MESH && meshid<0) {
if ((type==mjGEOM_MESH || type==mjGEOM_SDF) && meshid<0) {
throw mjCError(this, "mesh geom '%s' (id = %d) must have valid meshid", name.c_str(), id);
}
@@ -1730,7 +1738,7 @@ void mjCGeom::Compile(void) {
// fit geom if type is not mjGEOM_MESH
double meshpos[3];
if (type!=mjGEOM_MESH) {
if (type!=mjGEOM_MESH && type!=mjGEOM_SDF) {
pmesh->FitGeom(this, meshpos);
// remove reference to mesh
@@ -1752,7 +1760,7 @@ void mjCGeom::Compile(void) {
size[0] = model->hfields[hfieldid]->size[0];
size[1] = model->hfields[hfieldid]->size[1];
size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]);
} else if (type==mjGEOM_MESH) {
} else if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
const double* aabb = model->meshes[meshid]->aabb();
size[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3]));
size[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4]));
@@ -1793,6 +1801,21 @@ void mjCGeom::Compile(void) {
if (fluid_switch > 0) {
SetFluidCoefs();
}
// plugin
if (is_plugin) {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for geom '%s', (id = %d)",
name.c_str(), id);
}
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(plugin->capabilityflags & mjPLUGIN_SDF)) {
throw mjCError(this, "plugin '%s' does not support sign distance fields", plugin->name);
}
}
}
src/user/user_objects.h
+1
View File
@@ -610,6 +610,7 @@ class mjCMesh: public mjCBase {
void LoadOBJ(mjResource* resource); // load mesh in wavefront OBJ format
void LoadSTL(mjResource* resource); // load mesh in STL BIN format
void LoadMSH(mjResource* resource); // load mesh in MSH BIN format
void LoadSDF(); // generate mesh using marching cubes
void MakeGraph(void); // make graph of convex hull
void CopyGraph(void); // copy graph into face data
void MakeNormal(void); // compute vertex normals
src/xml/xml_native_reader.cc
+32 -5
View File
@@ -80,7 +80,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 191;
static const int nMJCF = 203;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -95,12 +95,13 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"inttotal", "interval", "tolrange"},
{">"},
{"option", "*", "22",
{"option", "*", "24",
"timestep", "apirate", "impratio", "tolerance", "noslip_tolerance", "mpr_tolerance",
"gravity", "wind", "magnetic", "density", "viscosity",
"o_margin", "o_solref", "o_solimp",
"integrator", "collision", "cone", "jacobian",
"solver", "iterations", "noslip_iterations", "mpr_iterations"},
"solver", "iterations", "noslip_iterations", "mpr_iterations",
"sdf_iterations", "sdf_initpoints"},
{"<"},
{"flag", "?", "19", "constraint", "equality", "frictionloss", "limit", "contact",
"passive", "gravity", "clampctrl", "warmstart",
@@ -220,6 +221,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"hfield", "*", "6", "name", "content_type", "file", "nrow", "ncol", "size"},
{"mesh", "*", "12", "name", "class", "content_type", "file", "vertex", "normal",
"texcoord", "face", "refpos", "refquat", "scale", "smoothnormal"},
{"<"},
{"plugin", "*", "2", "plugin", "instance"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"skin", "*", "9", "name", "file", "material", "rgba", "inflate",
"vertex", "texcoord", "face", "group"},
{"<"},
@@ -249,6 +256,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"shellinertia", "solmix", "solref", "solimp",
"margin", "gap", "fromto", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler",
"hfield", "mesh", "fitscale", "rgba", "fluidshape", "fluidcoef", "user"},
{"<"},
{"plugin", "*", "2", "plugin", "instance"},
{"<"},
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"site", "*", "15", "name", "class", "type", "group", "pos", "quat",
"material", "size", "fromto", "axisangle", "xyaxes", "zaxis", "euler", "rgba", "user"},
{"camera", "*", "13", "name", "class", "fovy", "ipd",
@@ -497,7 +510,8 @@ const mjMap geom_map[mjNGEOMTYPES] = {
{"ellipsoid", mjGEOM_ELLIPSOID},
{"cylinder", mjGEOM_CYLINDER},
{"box", mjGEOM_BOX},
{"mesh", mjGEOM_MESH}
{"mesh", mjGEOM_MESH},
{"sdf", mjGEOM_SDF}
};
@@ -964,6 +978,8 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
ReadAttrInt(section, "iterations", &opt->iterations);
ReadAttrInt(section, "noslip_iterations", &opt->noslip_iterations);
ReadAttrInt(section, "mpr_iterations", &opt->mpr_iterations);
ReadAttrInt(section, "sdf_iterations", &opt->sdf_iterations);
ReadAttrInt(section, "sdf_initpoints", &opt->sdf_initpoints);
// read disable sub-element
XMLElement* elem = FindSubElem(section, "flag");
@@ -1196,6 +1212,11 @@ void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) {
pmesh->set_refquat(ReadAttrArr<double, 4>(elem, "refquat"));
pmesh->set_scale(ReadAttrArr<double, 3>(elem, "scale"));
XMLElement* eplugin = elem->FirstChildElement("plugin");
if (eplugin) {
OnePlugin(eplugin, pmesh);
}
if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) {
pmesh->set_smoothnormal((n==1));
}
@@ -1382,6 +1403,12 @@ void mjXReader::OneGeom(XMLElement* elem, mjCGeom* pgeom) {
// read userdata
ReadVector(elem, "user", pgeom->userdata, text);
// plugin sub-element
XMLElement* eplugin = elem->FirstChildElement("plugin");
if (eplugin) {
OnePlugin(eplugin, pgeom);
}
// remaining attributes
ReadAttr(elem, "mass", 1, &pgeom->_mass, text);
ReadAttr(elem, "density", 1, &pgeom->density, text);
@@ -2101,10 +2128,10 @@ void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) {
ReadAttrTxt(elem, "instance", object->plugin_instance_name);
if (object->plugin_instance_name.empty()) {
object->plugin_instance = model->AddPlugin();
ReadPluginConfigs(elem, object->plugin_instance);
} else {
model->hasImplicitPluginElem = true;
}
ReadPluginConfigs(elem, object->plugin_instance);
}
src/xml/xml_native_writer.cc
+17 -4
View File
@@ -278,7 +278,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
}
// mesh geom
if (pgeom->type==mjGEOM_MESH) {
if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) {
mjCMesh* pmesh = model->meshes[pgeom->meshid];
// write pos/quat if there is a difference
@@ -337,7 +337,7 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
if (pgeom->type==mjGEOM_HFIELD) {
WriteAttrTxt(elem, "hfield", pgeom->hfield);
}
if (pgeom->type==mjGEOM_MESH) {
if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) {
WriteAttrTxt(elem, "mesh", pgeom->mesh);
}
@@ -347,6 +347,11 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) {
} else {
WriteVector(elem, "user", pgeom->userdata, def->geom.userdata);
}
// write plugin
if (pgeom->is_plugin) {
OnePlugin(InsertEnd(elem, "plugin"), pgeom);
}
}
@@ -807,6 +812,8 @@ void mjXWriter::Option(XMLElement* root) {
WriteAttrInt(section, "iterations", model->option.iterations, opt.iterations);
WriteAttrInt(section, "noslip_iterations", model->option.noslip_iterations, opt.noslip_iterations);
WriteAttrInt(section, "mpr_iterations", model->option.mpr_iterations, opt.mpr_iterations);
WriteAttrInt(section, "sdf_iterations", model->option.sdf_iterations, opt.sdf_iterations);
WriteAttrInt(section, "sdf_initpoints", model->option.sdf_initpoints, opt.sdf_initpoints);
// write disable/enable flags if any of them are set; invert while writing
if (model->option.disableflags || model->option.enableflags) {
@@ -1307,8 +1314,14 @@ void mjXWriter::Asset(XMLElement* root) {
for (int i=0; i<nmesh; i++) {
// create element and write
mjCMesh* pmesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
elem = InsertEnd(section, "mesh");
OneMesh(elem, pmesh, pmesh->def);
if (pmesh->is_plugin) {
elem = InsertEnd(section, "mesh");
WriteAttrTxt(elem, "name", pmesh->name);
OnePlugin(InsertEnd(elem, "plugin"), pmesh);
} else{
elem = InsertEnd(section, "mesh");
OneMesh(elem, pmesh, pmesh->def);
}
}
// write skins