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

Remove box, cylinder and ellipsoid composite types.

Browse Source 
Also, fix a bug with Flex textures that was causing an incorrect allocation of the textures in mjModel. Fixes #2013.

PiperOrigin-RevId: 692510858
Change-Id: If781716216974e085244da27bc3aa57d91c3458c
This commit is contained in:
Alessio Quaglino
2024-11-02 11:13:16 -07:00
committed by Copybara-Service
parent ebe60b9ad5
commit 831d9881d3
12 changed files with 54 additions and 547 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_composite.cc
-453
View File
@@ -192,25 +192,6 @@ void mjCComposite::SetDefault(void) {
case mjCOMPTYPE_CLOTH: // cloth
break;
case mjCOMPTYPE_BOX: // 3D
case mjCOMPTYPE_CYLINDER:
case mjCOMPTYPE_ELLIPSOID:
// no self-collisions
def[0].spec.geom->contype = 0;
// soft smoothing
AdjustSoft(solrefsmooth, solimpsmooth, 1);
// soft fix everywhere
for (int i=0; i<mjNCOMPKINDS; i++) {
AdjustSoft(def[i].spec.equality->solref, def[i].spec.equality->solimp, 1);
}
// hard main tendon fix
AdjustSoft(def[mjCOMPKIND_TENDON].spec.equality->solref,
def[mjCOMPKIND_TENDON].spec.equality->solimp, 0);
break;
default:
// SHOULD NOT OCCUR
mju_error("Invalid composite type: %d", type);
@@ -347,11 +328,6 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
"\"shell\" instead.",
error_sz);
case mjCOMPTYPE_BOX:
case mjCOMPTYPE_CYLINDER:
case mjCOMPTYPE_ELLIPSOID:
return MakeBox(model, body, error, error_sz);
default:
return comperr(error, "Unknown shape in composite", error_sz);
}
@@ -919,166 +895,6 @@ mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int i
// project from box to other shape
void mjCComposite::BoxProject(double* pos) {
// determine sizes
double size[3] = {
0.5*spacing*(count[0]-1),
0.5*spacing*(count[1]-1),
0.5*spacing*(count[2]-1)
};
// box
if (type==mjCOMPTYPE_BOX) {
pos[0] *= size[0];
pos[1] *= size[1];
pos[2] *= size[2];
}
// cylinder
else if (type==mjCOMPTYPE_CYLINDER) {
double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
mjuu_normvec(pos, 2);
pos[0] *= size[0]*L0;
pos[1] *= size[1]*L0;
pos[2] *= size[2];
}
// ellipsoid
else if (type==mjCOMPTYPE_ELLIPSOID) {
mjuu_normvec(pos, 3);
pos[0] *= size[0];
pos[1] *= size[1];
pos[2] *= size[2];
}
}
// make 3d box, ellipsoid or cylinder
bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz) {
char txt[100];
// check dim
if (dim!=3) {
return comperr(error, "Box and ellipsoid must be three-dimensional", error_sz);
}
// center geom: two times bigger
mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
mjs_setDefault(geom->element, mjs_getDefault(body->element));
geom->type = mjGEOM_SPHERE;
mju::sprintf_arr(txt, "%sGcenter", prefix.c_str());
mjs_setString(geom->name, txt);
mjuu_setvec(geom->pos, 0, 0, 0);
geom->size[0] *= 2;
geom->size[1] = 0;
geom->size[2] = 0;
// fixed tendon for all joints
mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
ten->classname = model->Default()->name;
mju::sprintf_arr(txt, "%sT", prefix.c_str());
ten->name = txt;
// create bodies, geoms and joints: outside shell only
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
if (ix==0 || ix==count[0]-1 ||
iy==0 || iy==count[1]-1 ||
iz==0 || iz==count[2]-1) {
// create body
mjsBody* b = mjs_addBody(body, NULL);
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
mjs_setString(b->name, txt);
// set body position (+/- 1)
b->pos[0] = 2.0*ix/(count[0]-1) - 1;
b->pos[1] = 2.0*iy/(count[1]-1) - 1;
b->pos[2] = 2.0*iz/(count[2]-1) - 1;
// reshape
BoxProject(b->pos);
// reorient body
b->alt.type = mjORIENTATION_ZAXIS;
mjuu_copyvec(b->alt.zaxis, b->pos, 3);
mjuu_normvec(b->alt.zaxis, 3);
// add geom
mjsGeom* g = mjs_addGeom(b, &def[0].spec);
mjs_setDefault(g->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sG%d_%d_%d", prefix.c_str(), ix, iy, iz);
mjs_setString(g->name, txt);
// offset inwards, enforce sphere or capsule
if (g->type==mjGEOM_CAPSULE) {
g->pos[2] = -(g->size[0] + g->size[1]);
} else {
g->type = mjGEOM_SPHERE;
g->pos[2] = -g->size[0];
}
// add slider joint
mjsJoint* jnt = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 1);
// add fix constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec);
mjs_setDefault(eq->element, &model->Default()->spec);
eq->type = mjEQ_JOINT;
mjs_setString(eq->name1, mjs_getString(jnt->name));
// add joint to tendon
ten->WrapJoint(std::string(mjs_getString(jnt->name)), 1);
// add neighbor constraints
for (int i=0; i<3; i++) {
int ix1 = mjMIN(ix+(i==0), count[0]-1);
int iy1 = mjMIN(iy+(i==1), count[1]-1);
int iz1 = mjMIN(iz+(i==2), count[2]-1);
if ((ix1==0 || ix1==count[0]-1 ||
iy1==0 || iy1==count[1]-1 ||
iz1==0 || iz1==count[2]-1) &&
(ix!=ix1 || iy!=iy1 || iz!=iz1)) {
char txt2[200];
mju::sprintf_arr(txt2,
"%sJ%d_%d_%d", prefix.c_str(), ix1, iy1, iz1);
mjsEquality* eqn = mjs_addEquality(&model->spec, 0);
mju_copy(eqn->solref, solrefsmooth, mjNREF);
mju_copy(eqn->solimp, solimpsmooth, mjNIMP);
eqn->type = mjEQ_JOINT;
mjs_setString(eqn->name1, txt);
mjs_setString(eqn->name2, txt2);
}
}
}
}
}
}
// finalize fixed tendon
mjsEquality* eqt = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
mjs_setDefault(eqt->element, &model->Default()->spec);
eqt->type = mjEQ_TENDON;
mjs_setString(eqt->name1, ten->name.c_str());
// skin
if (skin) {
MakeSkin3(model);
}
return true;
}
// add shear tendons to 2D
void mjCComposite::MakeShear(mjCModel* model) {
char txt[100], txt1[100], txt2[100];
@@ -1838,272 +1654,3 @@ void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
mju_free(D);
}
// add skin to 3D
void mjCComposite::MakeSkin3(mjCModel* model) {
int vcnt = 0;
std::map<std::string, int> vmap;
char txt[100], cnt0[10], cnt1[10], cnt2[10];
std::string fmt;
// string counts
mju::sprintf_arr(cnt0, "%d", count[0]-1);
mju::sprintf_arr(cnt1, "%d", count[1]-1);
mju::sprintf_arr(cnt2, "%d", count[2]-1);
// add skin, set name and material
mjsSkin* skin = mjs_addSkin(&model->spec);
mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
mjs_setString(skin->name, txt);
mjs_setString(skin->material, skinmaterial.c_str());
mjuu_copyvec(skin->rgba, skinrgba, 4);
skin->inflate = skininflate;
skin->group = skingroup;
// box
if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
// z-faces
MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
fmt = "%sB%d_%d_" + std::string(cnt2);
MakeSkin3Box(skin, count[0], count[1], 0, vcnt, fmt.c_str());
// y-faces
MakeSkin3Box(skin, count[0], count[2], 0, vcnt, "%sB%d_0_%d");
fmt = "%sB%d_" + std::string(cnt1) + "_%d";
MakeSkin3Box(skin, count[0], count[2], 1, vcnt, fmt.c_str());
// x-faces
MakeSkin3Box(skin, count[1], count[2], 1, vcnt, "%sB0_%d_%d");
fmt = "%sB" + std::string(cnt0) + "_%d_%d";
MakeSkin3Box(skin, count[1], count[2], 0, vcnt, fmt.c_str());
}
// cylinder
else if (type==mjCOMPTYPE_CYLINDER) {
// generate vertices in map
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
bool xedge = (ix==0 || ix==count[0]-1);
bool yedge = (iy==0 || iy==count[1]-1);
if (xedge || yedge) {
// body name
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
// add vertex
vert.push_back(0);
vert.push_back(0);
vert.push_back(0);
// texture coordinate
if (skintexcoord) {
float X = 0, Y = 0;
if (xedge) {
X = iy/(float)(count[1]-1);
Y = iz/(float)(count[2]-1);
} else {
X = ix/(float)(count[0]-1);
Y = iz/(float)(count[2]-1);
}
texcoord.push_back(X);
texcoord.push_back(Y);
}
// save vertex id in map
vmap[txt] = vcnt;
vcnt++;
}
}
}
}
// y-faces
MakeSkin3Smooth(skin, count[0], count[2], 0, vmap, "%sB%d_0_%d");
fmt = "%sB%d_" + std::string(cnt1) + "_%d";
MakeSkin3Smooth(skin, count[0], count[2], 1, vmap, fmt.c_str());
// x-faces
MakeSkin3Smooth(skin, count[1], count[2], 1, vmap, "%sB0_%d_%d");
fmt = "%sB" + std::string(cnt0) + "_%d_%d";
MakeSkin3Smooth(skin, count[1], count[2], 0, vmap, fmt.c_str());
// z-faces, boxy-type
MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
fmt = "%sB%d_%d_" + std::string(cnt2);
MakeSkin3Box(skin, count[0], count[1], 0, vcnt, fmt.c_str());
}
// smooth
else {
// generate vertices in map
for (int ix=0; ix<count[0]; ix++) {
for (int iy=0; iy<count[1]; iy++) {
for (int iz=0; iz<count[2]; iz++) {
bool xedge = (ix==0 || ix==count[0]-1);
bool yedge = (iy==0 || iy==count[1]-1);
bool zedge = (iz==0 || iz==count[2]-1);
if (xedge || yedge || zedge) {
// body name
mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
// add vertex
vert.push_back(0);
vert.push_back(0);
vert.push_back(0);
// texture coordinate
if (skintexcoord) {
float X = 0, Y = 0;
if (xedge) {
X = iy/(float)(count[1]-1);
Y = iz/(float)(count[2]-1);
} else if (yedge) {
X = ix/(float)(count[0]-1);
Y = iz/(float)(count[2]-1);
} else {
X = ix/(float)(count[0]-1);
Y = iy/(float)(count[1]-1);
}
texcoord.push_back(X);
texcoord.push_back(Y);
}
// save vertex id in map
vmap[txt] = vcnt;
vcnt++;
}
}
}
}
// z-faces
MakeSkin3Smooth(skin, count[0], count[1], 1, vmap, "%sB%d_%d_0");
fmt = "%sB%d_%d_" + std::string(cnt2);
MakeSkin3Smooth(skin, count[0], count[1], 0, vmap, fmt.c_str());
// y-faces
MakeSkin3Smooth(skin, count[0], count[2], 0, vmap, "%sB%d_0_%d");
fmt = "%sB%d_" + std::string(cnt1) + "_%d";
MakeSkin3Smooth(skin, count[0], count[2], 1, vmap, fmt.c_str());
// x-faces
MakeSkin3Smooth(skin, count[1], count[2], 1, vmap, "%sB0_%d_%d");
fmt = "%sB" + std::string(cnt0) + "_%d_%d";
MakeSkin3Smooth(skin, count[1], count[2], 0, vmap, fmt.c_str());
}
CopyIntoSkin(skin);
}
// make one face of 3D skin, box
void mjCComposite::MakeSkin3Box(mjsSkin* skin, int c0, int c1, int side,
int& vcnt, const char* format) {
char txt[100];
// loop over bodies/vertices of specified face
for (int i0=0; i0<c0; i0++) {
for (int i1=0; i1<c1; i1++) {
// vertex
vert.push_back(0);
vert.push_back(0);
vert.push_back(0);
// texture coordinate
if (skintexcoord) {
texcoord.push_back(i0/(float)(c0-1));
texcoord.push_back(i1/(float)(c1-1));
}
// face
if (i0<c0-1 && i1<c1-1) {
face.push_back(vcnt + i0*c1+i1);
face.push_back(vcnt + (i0+1)*c1+i1+(side==1));
face.push_back(vcnt + (i0+1)*c1+i1+(side==0));
face.push_back(vcnt + i0*c1+i1);
face.push_back(vcnt + (i0+(side==0))*c1+i1+1);
face.push_back(vcnt + (i0+(side==1))*c1+i1+1);
}
// body name
mju::sprintf_arr(txt, format, prefix.c_str(), i0, i1);
// bind pose: origin
mjs_appendString(skin->bodyname, txt);
bindpos.push_back(0);
bindpos.push_back(0);
bindpos.push_back(0);
bindquat.push_back(1);
bindquat.push_back(0);
bindquat.push_back(0);
bindquat.push_back(0);
// vertid and vertweight
vertid.push_back({vcnt + i0*c1+i1});
vertweight.push_back({1});
}
}
// update vertex count
vcnt += c0*c1;
}
// make one face of 3D skin, smooth
void mjCComposite::MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap,
const char* format) {
char txt00[100], txt01[100], txt10[100], txt11[100];
// loop over bodies/vertices of specified face
for (int i0=0; i0<c0; i0++) {
for (int i1=0; i1<c1; i1++) {
// body names
mju::sprintf_arr(txt00, format, prefix.c_str(), i0, i1);
mju::sprintf_arr(txt01, format, prefix.c_str(), i0, i1+1);
mju::sprintf_arr(txt10, format, prefix.c_str(), i0+1, i1);
mju::sprintf_arr(txt11, format, prefix.c_str(), i0+1, i1+1);
// face
if (i0<c0-1 && i1<c1-1) {
if (side==0) {
face.push_back(vmap.find(txt00)->second);
face.push_back(vmap.find(txt10)->second);
face.push_back(vmap.find(txt11)->second);
face.push_back(vmap.find(txt00)->second);
face.push_back(vmap.find(txt11)->second);
face.push_back(vmap.find(txt01)->second);
} else {
face.push_back(vmap.find(txt00)->second);
face.push_back(vmap.find(txt01)->second);
face.push_back(vmap.find(txt11)->second);
face.push_back(vmap.find(txt00)->second);
face.push_back(vmap.find(txt11)->second);
face.push_back(vmap.find(txt10)->second);
}
}
// bind pose: origin
mjs_appendString(skin->bodyname, txt00);
bindpos.push_back(0);
bindpos.push_back(0);
bindpos.push_back(0);
bindquat.push_back(1);
bindquat.push_back(0);
bindquat.push_back(0);
bindquat.push_back(0);
// vertid and vertweight
vertid.push_back({vmap.find(txt00)->second});
vertweight.push_back({1});
}
}
}
src/user/user_composite.h
-11
View File
@@ -31,9 +31,6 @@ typedef enum _mjtCompType {
mjCOMPTYPE_ROPE,
mjCOMPTYPE_LOOP,
mjCOMPTYPE_CLOTH,
mjCOMPTYPE_BOX,
mjCOMPTYPE_CYLINDER,
mjCOMPTYPE_ELLIPSOID,
mjNCOMPTYPES
} mjtCompType;
@@ -75,7 +72,6 @@ class mjCComposite {
bool MakeGrid(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz);
bool MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz);
void MakeShear(mjCModel* model);
void MakeSkin2(mjCModel* model, mjtNum inflate);
@@ -85,13 +81,6 @@ class mjCComposite {
void MakeCableBones(mjCModel* model, mjsSkin* skin);
void MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin);
void MakeSkin3(mjCModel* model);
void MakeSkin3Box(mjsSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
void MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side,
const std::map<std::string, int>& vmap, const char* format);
void BoxProject(double* pos);
// common properties
std::string prefix; // name prefix
mjtCompType type; // composite type
src/user/user_flexcomp.cc
+25
View File
@@ -763,6 +763,7 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) {
// make 3d box, ellipsoid or cylinder
bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
double pos[3];
bool needtex = texcoord.empty() && !std::string(mjs_getString(def.spec.flex->material)).empty();
// set 3D
def.spec.flex->dim = 3;
@@ -772,6 +773,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
point.push_back(0);
point.push_back(0);
// add texture coordinates, if not specified explicitly
if (needtex) {
texcoord.push_back(0);
texcoord.push_back(0);
}
// iz=0/max
for (int iz=0; iz < count[2]; iz+=count[2]-1) {
for (int ix=0; ix < count[0]; ix++) {
@@ -782,6 +789,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
point.push_back(pos[1]);
point.push_back(pos[2]);
// add texture coordinates, if not specified explicitly
if (needtex) {
texcoord.push_back(ix/(float)std::max(count[0]-1, 1));
texcoord.push_back(iy/(float)std::max(count[1]-1, 1));
}
// add elements
if (ix < count[0]-1 && iy < count[1]-1) {
element.push_back(0);
@@ -808,6 +821,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
point.push_back(pos[0]);
point.push_back(pos[1]);
point.push_back(pos[2]);
// add texture coordinates
if (needtex) {
texcoord.push_back(ix/(float)std::max(count[0]-1, 1));
texcoord.push_back(iz/(float)std::max(count[2]-1, 1));
}
}
// add elements
@@ -836,6 +855,12 @@ bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
point.push_back(pos[0]);
point.push_back(pos[1]);
point.push_back(pos[2]);
// add texture coordinates
if (needtex) {
texcoord.push_back(iy/(float)std::max(count[1]-1, 1));
texcoord.push_back(iz/(float)std::max(count[2]-1, 1));
}
}
// add elements
src/user/user_model.cc
+1
View File
@@ -1613,6 +1613,7 @@ void mjCModel::SetSizes() {
nflexelemedge += flexes_[i]->nelem * mjCFlex::kNumEdges[flexes_[i]->dim - 1];
nflexshelldata += (int)flexes_[i]->shell.size();
nflexevpair += (int)flexes_[i]->evpair.size()/2;
nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0);
}
// mesh counts
src/xml/xml_native_reader.cc
+1 -4
View File
@@ -742,10 +742,7 @@ const mjMap comp_map[mjNCOMPTYPES] = {
{"rope", mjCOMPTYPE_ROPE},
{"loop", mjCOMPTYPE_LOOP},
{"cable", mjCOMPTYPE_CABLE},
{"cloth", mjCOMPTYPE_CLOTH},
{"box", mjCOMPTYPE_BOX},
{"cylinder", mjCOMPTYPE_CYLINDER},
{"ellipsoid", mjCOMPTYPE_ELLIPSOID}
{"cloth", mjCOMPTYPE_CLOTH}
};