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

Cosmetic improvements to src/xml/ source files.

Browse Source 
PiperOrigin-RevId: 738010419
Change-Id: Idb90a42a374682450737684e67388329c184c777
This commit is contained in:
Yuval Tassa
2025-03-18 08:11:40 -07:00
committed by Copybara-Service
parent 205599ac1a
commit efe213a7b3
5 changed files with 743 additions and 733 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/xml/xml_urdf.cc
+74 -74
View File
@@ -125,7 +125,7 @@ void mjXURDF::Parse(
while (elem) {
// identify link elements
name = elem->Value();
if (name=="link") {
if (name == "link") {
ReadAttrTxt(elem, "name", text, true);
text = GetPrefixedName(text);
AddBody(text);
@@ -140,7 +140,7 @@ void mjXURDF::Parse(
while (elem) {
// identify joint elements
name = elem->Value();
if (name=="joint") {
if (name == "joint") {
// find parent, get name and id
temp = FindSubElem(elem, "parent", true);
ReadAttrTxt(temp, "link", text, true);
@@ -154,12 +154,12 @@ void mjXURDF::Parse(
id_child = FindName(text, urName);
// make sure parent and child exist
if (id_parent<0 || id_child<0) {
if (id_parent < 0 || id_child < 0) {
throw mjXError(elem, "URDF joint parent or child missing");
}
// check for multiple parents
if (urParent[id_child]>=0) {
if (urParent[id_child] >= 0) {
throw mjXError(elem, "URDF body has multiple parents:", urName[id_child].c_str());
}
@@ -173,7 +173,7 @@ void mjXURDF::Parse(
}
// find all top-level bodies, call recursive tree constructor
for (int i=0; i<(int)urName.size(); i++) {
for (int i=0; i < (int)urName.size(); i++) {
if (urParent[i] < 0) {
AddToTree(i);
}
@@ -184,7 +184,7 @@ void mjXURDF::Parse(
while (elem) {
// identify body/link elements
name = elem->Value();
if (name=="link") {
if (name == "link") {
Body(elem);
}
@@ -197,7 +197,7 @@ void mjXURDF::Parse(
while (elem) {
// identify body/link elements
name = elem->Value();
if (name=="joint") {
if (name == "joint") {
Joint(elem);
}
@@ -297,7 +297,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
name = elem->Value();
// visual element
if (name=="visual") {
if (name == "visual") {
// parse material
if ((temp = FindSubElem(elem, "material"))) {
// if color specified - use directly
@@ -310,7 +310,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
ReadAttrTxt(temp, "name", name, true);
name = GetPrefixedName(name);
int imat = FindName(name, urMat);
if (imat>=0) {
if (imat >= 0) {
std::memcpy(rgba, urRGBA[imat].val, 4*sizeof(float));
}
}
@@ -320,7 +320,7 @@ void mjXURDF::Body(XMLElement* body_elem) {
pgeom = Geom(elem, pbody, false);
// save color
if (rgba[0]>=0) {
if (rgba[0] >= 0) {
std::memcpy(pgeom->rgba, rgba, 4*sizeof(float));
}
@@ -339,11 +339,11 @@ void mjXURDF::Body(XMLElement* body_elem) {
}
// collision element
else if (name=="collision") {
else if (name == "collision") {
pgeom = Geom(elem, pbody, true);
// use color from last visual
if (rgba[0]>=0) {
if (rgba[0] >= 0) {
std::memcpy(pgeom->rgba, rgba, 4*sizeof(float));
}
@@ -418,71 +418,71 @@ void mjXURDF::Joint(XMLElement* joint_elem) {
// create joint (unless fixed)
double mat[9], quat[4], tmpaxis[3];
switch (jointtype) {
case 0: // revolute
case 1: // continuous
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_HINGE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
break;
case 0: // revolute
case 1: // continuous
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_HINGE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
break;
case 2: // prismatic
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_SLIDE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
break;
case 2: // prismatic
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_SLIDE;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
break;
case 3: // fixed- no joint, return
return;
case 3: // fixed- no joint, return
return;
case 4: // floating
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_FREE;
break;
case 4: // floating
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_FREE;
break;
case 5: // planar- construct complex joint
// make frame with axis = z
mjuu_z2quat(quat, axis);
mjuu_quat2mat(mat, quat);
case 5: // planar- construct complex joint
// make frame with axis = z
mjuu_z2quat(quat, axis);
mjuu_quat2mat(mat, quat);
// construct slider along x
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, (jntname + "_TX").c_str());
pjoint->type = mjJNT_SLIDE;
tmpaxis[0] = mat[0];
tmpaxis[1] = mat[3];
tmpaxis[2] = mat[6];
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, tmpaxis, 3);
// construct slider along x
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, (jntname + "_TX").c_str());
pjoint->type = mjJNT_SLIDE;
tmpaxis[0] = mat[0];
tmpaxis[1] = mat[3];
tmpaxis[2] = mat[6];
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, tmpaxis, 3);
// construct slider along y
pjoint1 = mjs_addJoint(pbody, 0);
mjs_setString(pjoint1->name, (jntname + "_TY").c_str());
pjoint1->type = mjJNT_SLIDE;
tmpaxis[0] = mat[1];
tmpaxis[1] = mat[4];
tmpaxis[2] = mat[7];
mjuu_setvec(pjoint1->pos, 0, 0, 0);
mjuu_copyvec(pjoint1->axis, tmpaxis, 3);
// construct slider along y
pjoint1 = mjs_addJoint(pbody, 0);
mjs_setString(pjoint1->name, (jntname + "_TY").c_str());
pjoint1->type = mjJNT_SLIDE;
tmpaxis[0] = mat[1];
tmpaxis[1] = mat[4];
tmpaxis[2] = mat[7];
mjuu_setvec(pjoint1->pos, 0, 0, 0);
mjuu_copyvec(pjoint1->axis, tmpaxis, 3);
// construct hinge around z = locaxis
pjoint2 = mjs_addJoint(pbody, 0);
mjs_setString(pjoint2->name, (jntname + "_RZ").c_str());
pjoint2->type = mjJNT_HINGE;
mjuu_setvec(pjoint2->pos, 0, 0, 0);
mjuu_copyvec(pjoint2->axis, axis, 3);
break;
// construct hinge around z = locaxis
pjoint2 = mjs_addJoint(pbody, 0);
mjs_setString(pjoint2->name, (jntname + "_RZ").c_str());
pjoint2->type = mjJNT_HINGE;
mjuu_setvec(pjoint2->pos, 0, 0, 0);
mjuu_copyvec(pjoint2->axis, axis, 3);
break;
case 6: // ball joint
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_BALL;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
case 6: // ball joint
pjoint = mjs_addJoint(pbody, 0);
mjs_setString(pjoint->name, jntname.c_str());
pjoint->type = mjJNT_BALL;
mjuu_setvec(pjoint->pos, 0, 0, 0);
mjuu_copyvec(pjoint->axis, axis, 3);
}
// dynamics element
@@ -546,7 +546,7 @@ mjsGeom* mjXURDF::Geom(XMLElement* geom_elem, mjsBody* pbody, bool collision) {
// box
if ((temp = FindSubElem(elem, "box"))) {
ReadAttr(temp, "size", 3, pgeom->size, text, true, true);
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
pgeom->size[i] /= 2; // MuJoCo uses half-length
}
}
@@ -673,7 +673,7 @@ void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) {
// find body with given name in list, return -1 if not found
int mjXURDF::FindName(std::string name, std::vector<std::string>& list) {
for (unsigned int i=0; i<list.size(); i++)
for (unsigned int i=0; i < list.size(); i++)
if (list[i] == name) {
return i;
}
@@ -686,7 +686,7 @@ int mjXURDF::FindName(std::string name, std::vector<std::string>& list) {
// add name to list, error if name already exists
void mjXURDF::AddName(std::string name, std::vector<std::string>& list) {
// make sure name is unique
if (FindName(name, list)>=0) {
if (FindName(name, list) >= 0) {
throw mjXError(0, "repeated URDF name: ", name.c_str());
}
@@ -713,7 +713,7 @@ void mjXURDF::AddBody(std::string name) {
void mjXURDF::AddToTree(int n) {
// get pointer to parent in mjCModel tree
mjsBody *parent = 0, *child = 0, *world = 0;
if (urParent[n]>=0) {
if (urParent[n] >= 0) {
world = mjs_findBody(spec, "world");
parent = mjs_findChild(world, urName[urParent[n]].c_str());
@@ -731,7 +731,7 @@ void mjXURDF::AddToTree(int n) {
}
// add children recursively
for (int i=0; i<(int)urChildren[n].size(); i++) {
for (int i=0; i < (int)urChildren[n].size(); i++) {
AddToTree(urChildren[n][i]);
}
}