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

Change from parent ID to parent pointer in mjCBody.

Browse Source 
This is required to have parent body info during attach and during body to frame tranformation for uncompiled models.

PiperOrigin-RevId: 703038426
Change-Id: Ib482059ba419a020860c649d6ea1b1088045f2e1
This commit is contained in:
Alessio Quaglino
2024-12-05 02:31:54 -08:00
committed by Copybara-Service
parent 977fb913cf
commit 10239a673b
4 changed files with 58 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+15 -13
View File
@@ -153,7 +153,7 @@ mjCModel::mjCModel() {
world->mass = 0;
mjuu_zerovec(world->inertia, 3);
world->id = 0;
world->parentid = 0;
world->parent = nullptr;
world->weldid = 0;
world->name = "world";
world->classname = "main";
@@ -2067,10 +2067,10 @@ void mjCModel::CopyTree(mjModel* m) {
for (int i=0; i<nbody; i++) {
// get body and parent pointers
mjCBody* pb = bodies_[i];
mjCBody* par = bodies_[pb->parentid];
mjCBody* par = pb->parent;
// set body fields
m->body_parentid[i] = pb->parentid;
m->body_parentid[i] = pb->parent ? pb->parent->id : 0;
m->body_weldid[i] = pb->weldid;
m->body_mocapid[i] = pb->mocapid;
m->body_jntnum[i] = (int)pb->joints.size();
@@ -2115,19 +2115,19 @@ void mjCModel::CopyTree(mjModel* m) {
if (cntfree>1 || (cntfree==1 && pb->joints.size()>1)) {
throw mjCError(pb, "free joint can only appear by itself");
}
if (cntfree && pb->parentid) {
if (cntfree && par && par->name != "world") {
throw mjCError(pb, "free joint can only be used on top level");
}
// rootid: self if world or child of world, otherwise parent's rootid
if (i==0 || pb->parentid==0) {
if (i==0 || (par && par->name == "world")) {
m->body_rootid[i] = i;
} else {
m->body_rootid[i] = m->body_rootid[pb->parentid];
m->body_rootid[i] = m->body_rootid[par->id];
}
// init lastdof from parent
pb->lastdof = par->lastdof;
pb->lastdof = par ? par->lastdof : -1;
// set sameframe
mjtSameFrame sameframe;
@@ -2447,7 +2447,9 @@ void mjCModel::CopyTree(mjModel* m) {
bodies_[i]->subtreedofs += bodies_[i]->dofnum;
// add to parent count
bodies_[bodies_[i]->parentid]->subtreedofs += bodies_[i]->subtreedofs;
if (bodies_[i]->parent) {
bodies_[i]->parent->subtreedofs += bodies_[i]->subtreedofs;
}
}
// make sure all dofs are in world "subtree", SHOULD NOT OCCUR
@@ -2462,10 +2464,10 @@ void mjCModel::CopyTree(mjModel* m) {
nB += bodies_[i]->subtreedofs;
// add dofs in ancestor bodies
int j = bodies_[i]->parentid;
int j = bodies_[i]->parent ? bodies_[i]->parent->id : 0;
while (j > 0) {
nB += bodies_[j]->dofnum;
j = bodies_[j]->parentid;
j = bodies_[j]->parent ? bodies_[j]->parent->id : 0;
}
}
m->nB = nB;
@@ -3413,7 +3415,7 @@ static void changeframe(double childpos[3], double childquat[4],
void mjCModel::FuseReindex(mjCBody* body) {
// set parentid and weldid of children
for (int i=0; i<body->bodies.size(); i++) {
body->bodies[i]->parentid = body->id;
body->bodies[i]->parent = body;
body->bodies[i]->weldid = (!body->bodies[i]->joints.empty() ?
body->bodies[i]->id : body->weldid);
}
@@ -3450,7 +3452,7 @@ void mjCModel::FuseStatic(void) {
for (int i=1; i<bodies_.size(); i++) {
// get body and parent
mjCBody* body = bodies_[i];
mjCBody* par = bodies_[body->parentid];
mjCBody* par = body->parent;
// skip if body has joints or mocap
if (!body->joints.empty() || body->mocap) {
@@ -3459,7 +3461,7 @@ void mjCModel::FuseStatic(void) {
//------------- add mass and inertia (if parent not world)
if (body->parentid>0 && body->mass>=mjMINVAL) {
if (body->parent && body->parent->name != "world" && body->mass>=mjMINVAL) {
// body_ipose = body_pose * body_ipose
changeframe(body->ipos, body->iquat, body->pos, body->quat);