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

Add circle to flexcomp and remove rope and loop from composite.

Browse Source 
Add pulley example to show a circle use case.

Switch on flex edge rendering by default (no effect when flex skin is rendered).

PiperOrigin-RevId: 700738379
Change-Id: I8469bbf194de01c8dab66c9a51c49879013df141
This commit is contained in:
Alessio Quaglino
2024-11-27 10:13:28 -08:00
committed by Copybara-Service
parent 85c6fc1bdb
commit 300450f8b8
9 changed files with 98 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_init.c
+1 -1
View File
@@ -93,7 +93,7 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"Static Body", "1", "D"},
{"Skin", "1", ";"},
{"Flex Vert", "0", ""},
{"Flex Edge", "0", ""},
{"Flex Edge", "1", ""},
{"Flex Face", "0", ""},
{"Flex Skin", "1", ""},
{"Body Tree", "0", "`"},
src/user/user_composite.cc
+7 -172
View File
@@ -309,15 +309,15 @@ bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz)
case mjCOMPTYPE_ROPE:
return comperr(error,
"The \"rope\" composite type is deprecated. Please use "
"\"cable\" instead.",
error_sz);
"The \"rope\" composite type is deprecated. Please use "
"\"cable\" instead.",
error_sz);
case mjCOMPTYPE_LOOP:
mju_warning(
"The \"loop\" composite type is deprecated. Please use \"cable\" "
"instead.");
return MakeRope(model, body, error, error_sz);
return comperr(error,
"The \"loop\" composite type is deprecated. Please use "
"\"flexcomp\" instead.",
error_sz);
case mjCOMPTYPE_CABLE:
return MakeCable(model, body, error, error_sz);
@@ -729,171 +729,6 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
}
// make rope
bool mjCComposite::MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz) {
// check dim
if (dim!=1) {
return comperr(error, "Rope must be one-dimensional", error_sz);
}
// check root body name prefix
char txt[200];
mju::sprintf_arr(txt, "%sB", prefix.c_str());
std::string body_name = mjs_getString(body->name);
if (std::strncmp(txt, body_name.substr(0, strlen(txt)).c_str(), mju::sizeof_arr(txt))) {
mju::strcat_arr(txt, " must be the beginning of root body name");
return comperr(error, txt, error_sz);
}
// read origin coordinate from root body
mju::strcpy_arr(txt, body_name.substr(strlen(txt)).c_str());
int ox = -1;
if (sscanf(txt, "%d", &ox)!=1) {
return comperr(error, "Root body name must contain X coordinate", error_sz);
}
if (ox<0 || ox>=count[0]) {
return comperr(error, "Root body coordinate out of range", error_sz);
}
// add origin
AddRopeBody(model, body, ox, ox);
// add elements: right
mjsBody* pbody = body;
for (int ix=ox; ix<count[0]-1; ix++) {
pbody = AddRopeBody(model, pbody, ix, ix+1);
}
// add elements: left
pbody = body;
for (int ix=ox; ix>0; ix--) {
pbody = AddRopeBody(model, pbody, ix, ix-1);
}
// close loop
if (type==mjCOMPTYPE_LOOP) {
char txt2[200];
// add equality constraint
mjsEquality* eq = mjs_addEquality(&model->spec, 0);
eq->type = mjEQ_CONNECT;
mju::sprintf_arr(txt, "%sB0", prefix.c_str());
mju::sprintf_arr(txt2, "%sB%d", prefix.c_str(), count[0]-1);
mjs_setString(eq->name1, txt);
mjs_setString(eq->name2, txt2);
mjuu_setvec(eq->data, -0.5*spacing, 0, 0);
mju_copy(eq->solref, solrefsmooth, mjNREF);
mju_copy(eq->solimp, solimpsmooth, mjNIMP);
// remove contact between connected bodies
mjsExclude* pair = mjs_addExclude(&model->spec);
mjs_setString(pair->bodyname1, std::string(txt).c_str());
mjs_setString(pair->bodyname2, std::string(txt2).c_str());
}
return true;
}
// add child body for cloth
mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1) {
char txt[100];
bool isroot = (ix==ix1);
double dx = spacing*(ix1-ix);
// add child if not root
if (!isroot) {
body = mjs_addBody(body, 0);
mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), ix1);
mjs_setString(body->name, txt);
// loop
if (type==mjCOMPTYPE_LOOP) {
double alpha = 2*mjPI/count[0];
double R = 0.5*spacing*sin(mjPI-alpha)/sin(0.5*alpha);
if (ix1>ix) {
mjuu_setvec(body->pos, R*cos(0.5*alpha), R*sin(0.5*alpha), 0);
mjuu_setvec(body->quat, cos(0.5*alpha), 0, 0, sin(0.5*alpha));
} else {
mjuu_setvec(body->pos, -R*cos(0.5*alpha), R*sin(0.5*alpha), 0);
mjuu_setvec(body->quat, cos(-0.5*alpha), 0, 0, sin(-0.5*alpha));
}
}
// no loop
else {
mjuu_setvec(body->pos, dx, 0, 0);
}
}
// add geom
mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
mjs_setDefault(geom->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sG%d", prefix.c_str(), ix1);
mjs_setString(geom->name, txt);
mjuu_setvec(geom->pos, 0, 0, 0);
mjuu_setvec(geom->quat, sqrt(0.5), 0, sqrt(0.5), 0);
// root: no joints
if (isroot) {
return body;
}
// add main joint
for (int i=0; i<2; i++) {
// add joint
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_HINGE;
mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[i+1] = 1;
}
// add twist joint
if (add[mjCOMPKIND_TWIST]) {
// add joint
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_HINGE;
mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
mjuu_setvec(jnt->axis, 1, 0, 0);
// add constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec);
mjs_setDefault(eq->element, &model->Default()->spec);
eq->type = mjEQ_JOINT;
mjs_setString(eq->name1, mjs_getString(jnt->name));
}
// add stretch joint
if (add[mjCOMPKIND_STRETCH]) {
// add joint
mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0].spec);
mjs_setDefault(jnt->element, mjs_getDefault(body->element));
mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1);
mjs_setString(jnt->name, txt);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
mjuu_setvec(jnt->axis, 1, 0, 0);
// add constraint
mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec);
mjs_setDefault(eq->element, &model->Default()->spec);
eq->type = mjEQ_JOINT;
mjs_setString(eq->name1, mjs_getString(jnt->name));
}
return body;
}
// add shear tendons to 2D
void mjCComposite::MakeShear(mjCModel* model) {
src/user/user_composite.h
-2
View File
@@ -70,7 +70,6 @@ class mjCComposite {
bool MakeParticle(mjCModel* model, mjsBody* body, char* error, int error_sz);
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);
void MakeShear(mjCModel* model);
@@ -124,7 +123,6 @@ class mjCComposite {
int dim; // dimensionality
private:
mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1);
mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]);
// temporary skin vectors
src/user/user_flexcomp.cc
+24 -8
View File
@@ -157,6 +157,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
bool res;
switch (type) {
case mjFCOMPTYPE_GRID:
case mjFCOMPTYPE_CIRCLE:
res = MakeGrid(error, error_sz);
break;
@@ -548,15 +549,30 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
// 1D
if (dim == 1) {
for (int ix=0; ix < count[0]; ix++) {
// add point
point.push_back(spacing[0]*(ix - 0.5*(count[0]-1)));
point.push_back(0);
point.push_back(0);
if (type == mjFCOMPTYPE_CIRCLE) {
// add point
double theta = 2*mjPI/(count[0]-1);
double radius = spacing[0]/std::sin(theta/2)/2;
point.push_back(radius*std::cos(theta*ix));
point.push_back(radius*std::sin(theta*ix));
point.push_back(0);
// add element
if (ix < count[0]-1) {
element.push_back(ix);
element.push_back(ix+1);
// add element
if (ix < count[0]-1) {
element.push_back(ix);
element.push_back(ix == count[0]-2 ? 0 : ix+1);
}
} else {
// add point
point.push_back(spacing[0]*(ix - 0.5*(count[0]-1)));
point.push_back(0);
point.push_back(0);
// add element
if (ix < count[0]-1) {
element.push_back(ix);
element.push_back(ix+1);
}
}
}
}
src/user/user_flexcomp.h
+1
View File
@@ -31,6 +31,7 @@ typedef enum _mjtFcompType {
mjFCOMPTYPE_ELLIPSOID,
mjFCOMPTYPE_SQUARE,
mjFCOMPTYPE_DISC,
mjFCOMPTYPE_CIRCLE,
mjFCOMPTYPE_MESH,
mjFCOMPTYPE_GMSH,
mjFCOMPTYPE_DIRECT,
src/xml/xml_native_reader.cc
+1
View File
@@ -794,6 +794,7 @@ const mjMap fcomp_map[mjNFCOMPTYPES] = {
{"ellipsoid", mjFCOMPTYPE_ELLIPSOID},
{"square", mjFCOMPTYPE_SQUARE},
{"disc", mjFCOMPTYPE_DISC},
{"circle", mjFCOMPTYPE_CIRCLE},
{"mesh", mjFCOMPTYPE_MESH},
{"gmsh", mjFCOMPTYPE_GMSH},
{"direct", mjFCOMPTYPE_DIRECT}