From 21334ea704fcee23e3e2523b3511e09b11719ff7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 27 Nov 2023 06:06:56 -0800 Subject: [PATCH] Delete local frame members from MJC objects. PiperOrigin-RevId: 585629699 Change-Id: I12dbd235dee49223fa7b1a097f04561da69550a0 --- src/user/user_model.cc | 131 +++++++++++++++++------------------ src/user/user_objects.cc | 81 ++-------------------- src/user/user_objects.h | 17 ----- src/xml/xml_native_writer.cc | 52 +++++++------- 4 files changed, 96 insertions(+), 185 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 7729d9aa..17c045b1 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -162,11 +162,7 @@ mjCModel::mjCModel() { //------------------------ world body mjCBody* world = new mjCBody(this); mjuu_zerovec(world->pos, 3); - mjuu_zerovec(world->locpos, 3); - mjuu_zerovec(world->locipos, 3); mjuu_setvec(world->quat, 1, 0, 0, 0); - mjuu_setvec(world->locquat, 1, 0, 0, 0); - mjuu_setvec(world->lociquat, 1, 0, 0, 0); world->mass = 0; mjuu_zerovec(world->inertia, 3); world->id = 0; @@ -1414,10 +1410,10 @@ void mjCModel::CopyTree(mjModel* m) { m->body_dofadr[i] = (pb->dofnum ? dofadr : -1); m->body_geomnum[i] = (int)pb->geoms.size(); m->body_geomadr[i] = (!pb->geoms.empty() ? pb->geoms[0]->id : -1); - copyvec(m->body_pos+3*i, pb->locpos, 3); - copyvec(m->body_quat+4*i, pb->locquat, 4); - copyvec(m->body_ipos+3*i, pb->locipos, 3); - copyvec(m->body_iquat+4*i, pb->lociquat, 4); + copyvec(m->body_pos+3*i, pb->pos, 3); + copyvec(m->body_quat+4*i, pb->quat, 4); + copyvec(m->body_ipos+3*i, pb->ipos, 3); + copyvec(m->body_iquat+4*i, pb->iquat, 4); m->body_mass[i] = (mjtNum)pb->mass; copyvec(m->body_inertia+3*i, pb->inertia, 3); m->body_gravcomp[i] = pb->gravcomp; @@ -1492,8 +1488,8 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; - copyvec(m->jnt_pos+3*jid, pj->locpos, 3); - copyvec(m->jnt_axis+3*jid, pj->locaxis, 3); + copyvec(m->jnt_pos+3*jid, pj->pos, 3); + copyvec(m->jnt_axis+3*jid, pj->axis, 3); m->jnt_stiffness[jid] = (mjtNum)pj->stiffness; copyvec(m->jnt_range+2*jid, pj->range, 2); copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); @@ -1506,9 +1502,9 @@ void mjCModel::CopyTree(mjModel* m) { if (rotfound || !IsNullPose(m->jnt_pos+3*jid, NULL) || ((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) && - ((mju_abs(pj->locaxis[0])>mjEPS) + - (mju_abs(pj->locaxis[1])>mjEPS) + - (mju_abs(pj->locaxis[2])>mjEPS)) > 1)) { + ((mju_abs(pj->axis[0])>mjEPS) + + (mju_abs(pj->axis[1])>mjEPS) + + (mju_abs(pj->axis[2])>mjEPS)) > 1)) { m->body_simple[i] = 0; } @@ -1602,8 +1598,8 @@ void mjCModel::CopyTree(mjModel* m) { m->geom_priority[gid] = pg->priority; copyvec(m->geom_size+3*gid, pg->size, 3); copyvec(m->geom_aabb+6*gid, pg->aabb, 6); - copyvec(m->geom_pos+3*gid, pg->locpos, 3); - copyvec(m->geom_quat+4*gid, pg->locquat, 4); + copyvec(m->geom_pos+3*gid, pg->pos, 3); + copyvec(m->geom_quat+4*gid, pg->quat, 4); copyvec(m->geom_friction+3*gid, pg->friction, 3); m->geom_solmix[gid] = (mjtNum)pg->solmix; copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); @@ -1617,13 +1613,13 @@ void mjCModel::CopyTree(mjModel* m) { // determine sameframe if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) { m->geom_sameframe[gid] = 1; - } else if (pg->locpos[0]==pb->locipos[0] && - pg->locpos[1]==pb->locipos[1] && - pg->locpos[2]==pb->locipos[2] && - pg->locquat[0]==pb->lociquat[0] && - pg->locquat[1]==pb->lociquat[1] && - pg->locquat[2]==pb->lociquat[2] && - pg->locquat[3]==pb->lociquat[3]) { + } else if (pg->pos[0]==pb->ipos[0] && + pg->pos[1]==pb->ipos[1] && + pg->pos[2]==pb->ipos[2] && + pg->quat[0]==pb->iquat[0] && + pg->quat[1]==pb->iquat[1] && + pg->quat[2]==pb->iquat[2] && + pg->quat[3]==pb->iquat[3]) { m->geom_sameframe[gid] = 2; } else { m->geom_sameframe[gid] = 0; @@ -1645,21 +1641,21 @@ void mjCModel::CopyTree(mjModel* m) { m->site_matid[sid] = ps->matid; m->site_group[sid] = ps->group; copyvec(m->site_size+3*sid, ps->size, 3); - copyvec(m->site_pos+3*sid, ps->locpos, 3); - copyvec(m->site_quat+4*sid, ps->locquat, 4); + copyvec(m->site_pos+3*sid, ps->pos, 3); + copyvec(m->site_quat+4*sid, ps->quat, 4); copyvec(m->site_user+nuser_site*sid, ps->userdata.data(), nuser_site); copyvec(m->site_rgba+4*sid, ps->rgba, 4); // determine sameframe if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) { m->site_sameframe[sid] = 1; - } else if (ps->locpos[0]==pb->locipos[0] && - ps->locpos[1]==pb->locipos[1] && - ps->locpos[2]==pb->locipos[2] && - ps->locquat[0]==pb->lociquat[0] && - ps->locquat[1]==pb->lociquat[1] && - ps->locquat[2]==pb->lociquat[2] && - ps->locquat[3]==pb->lociquat[3]) { + } else if (ps->pos[0]==pb->ipos[0] && + ps->pos[1]==pb->ipos[1] && + ps->pos[2]==pb->ipos[2] && + ps->quat[0]==pb->iquat[0] && + ps->quat[1]==pb->iquat[1] && + ps->quat[2]==pb->iquat[2] && + ps->quat[3]==pb->iquat[3]) { m->site_sameframe[sid] = 2; } else { m->site_sameframe[sid] = 0; @@ -1676,8 +1672,8 @@ void mjCModel::CopyTree(mjModel* m) { m->cam_bodyid[cid] = pc->body->id; m->cam_mode[cid] = pc->mode; m->cam_targetbodyid[cid] = pc->targetbodyid; - copyvec(m->cam_pos+3*cid, pc->locpos, 3); - copyvec(m->cam_quat+4*cid, pc->locquat, 4); + copyvec(m->cam_pos+3*cid, pc->pos, 3); + copyvec(m->cam_quat+4*cid, pc->quat, 4); m->cam_fovy[cid] = (mjtNum)pc->fovy; m->cam_ipd[cid] = (mjtNum)pc->ipd; copyvec(m->cam_resolution+2*cid, pc->resolution, 2); @@ -1699,8 +1695,8 @@ void mjCModel::CopyTree(mjModel* m) { m->light_directional[lid] = (mjtByte)pl->directional; m->light_castshadow[lid] = (mjtByte)pl->castshadow; m->light_active[lid] = (mjtByte)pl->active; - copyvec(m->light_pos+3*lid, pl->locpos, 3); - copyvec(m->light_dir+3*lid, pl->locdir, 3); + copyvec(m->light_pos+3*lid, pl->pos, 3); + copyvec(m->light_dir+3*lid, pl->dir, 3); copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); m->light_cutoff[lid] = pl->cutoff; m->light_exponent[lid] = pl->exponent; @@ -2419,8 +2415,7 @@ void mjCModel::FuseStatic(void) { if (body->parentid>0 && body->mass>=mjMINVAL) { // body_ipose = body_pose * body_ipose - changeframe(body->locipos, body->lociquat, - body->locpos, body->locquat); + changeframe(body->ipos, body->iquat, body->pos, body->quat); // organize data double mass[2] = { @@ -2432,47 +2427,47 @@ void mjCModel::FuseStatic(void) { {body->inertia[0], body->inertia[1], body->inertia[2]} }; double ipos[2][3] = { - {par->locipos[0], par->locipos[1], par->locipos[2]}, - {body->locipos[0], body->locipos[1], body->locipos[2]} + {par->ipos[0], par->ipos[1], par->ipos[2]}, + {body->ipos[0], body->ipos[1], body->ipos[2]} }; double iquat[2][4] = { - {par->lociquat[0], par->lociquat[1], par->lociquat[2], par->lociquat[3]}, - {body->lociquat[0], body->lociquat[1], body->lociquat[2], body->lociquat[3]} + {par->iquat[0], par->iquat[1], par->iquat[2], par->iquat[3]}, + {body->iquat[0], body->iquat[1], body->iquat[2], body->iquat[3]} }; // compute total mass par->mass = 0; - mjuu_setvec(par->locipos, 0, 0, 0); + mjuu_setvec(par->ipos, 0, 0, 0); for (int j=0; j<2; j++) { par->mass += mass[j]; - par->locipos[0] += mass[j]*ipos[j][0]; - par->locipos[1] += mass[j]*ipos[j][1]; - par->locipos[2] += mass[j]*ipos[j][2]; + par->ipos[0] += mass[j]*ipos[j][0]; + par->ipos[1] += mass[j]*ipos[j][1]; + par->ipos[2] += mass[j]*ipos[j][2]; } // small mass: allow for now, check for errors later if (par->massmass = 0; mjuu_setvec(par->inertia, 0, 0, 0); - mjuu_setvec(par->locipos, 0, 0, 0); - mjuu_setvec(par->lociquat, 1, 0, 0, 0); + mjuu_setvec(par->ipos, 0, 0, 0); + mjuu_setvec(par->iquat, 1, 0, 0, 0); } // proceed with regular computation else { // locipos = center-of-mass - par->locipos[0] /= par->mass; - par->locipos[1] /= par->mass; - par->locipos[2] /= par->mass; + par->ipos[0] /= par->mass; + par->ipos[1] /= par->mass; + par->ipos[2] /= par->mass; // add inertias double toti[6] = {0, 0, 0, 0, 0, 0}; for (int j=0; j<2; j++) { double inertA[6], inertB[6]; double dpos[3] = { - ipos[j][0] - par->locipos[0], - ipos[j][1] - par->locipos[1], - ipos[j][2] - par->locipos[2] + ipos[j][0] - par->ipos[0], + ipos[j][1] - par->ipos[1], + ipos[j][2] - par->ipos[2] }; mjuu_globalinertia(inertA, inertia[j], iquat[j]); @@ -2485,7 +2480,7 @@ void mjCModel::FuseStatic(void) { // compute principal axes of inertia mjCAlternative alt; mjuu_copyvec(alt.fullinertia, toti, 6); - const char* err1 = alt.Set(par->lociquat, par->inertia, degree, euler); + const char* err1 = alt.Set(par->iquat, par->inertia, degree, euler); if (err1) { throw mjCError(NULL, "error '%s' in fusing static body inertias", err1); } @@ -2496,8 +2491,8 @@ void mjCModel::FuseStatic(void) { // change frames of child bodies for (int j=0; jbodies.size(); j++) - changeframe(body->bodies[j]->locpos, body->bodies[j]->locquat, - body->locpos, body->locquat); + changeframe(body->bodies[j]->pos, body->bodies[j]->quat, + body->pos, body->quat); // find body in parent list, insert children before it bool found = false; @@ -2534,7 +2529,7 @@ void mjCModel::FuseStatic(void) { par->geoms.push_back(body->geoms[j]); // change frame - changeframe(body->geoms[j]->locpos, body->geoms[j]->locquat, body->locpos, body->locquat); + changeframe(body->geoms[j]->pos, body->geoms[j]->quat, body->pos, body->quat); } // sites @@ -2544,7 +2539,7 @@ void mjCModel::FuseStatic(void) { par->sites.push_back(body->sites[j]); // change frame - changeframe(body->sites[j]->locpos, body->sites[j]->locquat, body->locpos, body->locquat); + changeframe(body->sites[j]->pos, body->sites[j]->quat, body->pos, body->quat); } //------------- remove from global body list, reduce global counts @@ -3223,10 +3218,10 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; ilocpos, m->body_pos+3*i, 3); - copyvec(pb->locquat, m->body_quat+4*i, 4); - copyvec(pb->locipos, m->body_ipos+3*i, 3); - copyvec(pb->lociquat, m->body_iquat+4*i, 4); + copyvec(pb->pos, m->body_pos+3*i, 3); + copyvec(pb->quat, m->body_quat+4*i, 4); + copyvec(pb->ipos, m->body_ipos+3*i, 3); + copyvec(pb->iquat, m->body_iquat+4*i, 4); pb->mass = (double)m->body_mass[i]; copyvec(pb->inertia, m->body_inertia+3*i, 3); @@ -3241,8 +3236,8 @@ bool mjCModel::CopyBack(const mjModel* m) { pj = joints[i]; // joint data - copyvec(pj->locpos, m->jnt_pos+3*i, 3); - copyvec(pj->locaxis, m->jnt_axis+3*i, 3); + copyvec(pj->pos, m->jnt_pos+3*i, 3); + copyvec(pj->axis, m->jnt_axis+3*i, 3); pj->stiffness = (double)m->jnt_stiffness[i]; copyvec(pj->range, m->jnt_range+2*i, 2); copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); @@ -3268,8 +3263,8 @@ bool mjCModel::CopyBack(const mjModel* m) { pg = geoms[i]; copyvec(pg->size, m->geom_size+3*i, 3); - copyvec(pg->locpos, m->geom_pos+3*i, 3); - copyvec(pg->locquat, m->geom_quat+4*i, 4); + copyvec(pg->pos, m->geom_pos+3*i, 3); + copyvec(pg->quat, m->geom_quat+4*i, 4); copyvec(pg->friction, m->geom_friction+3*i, 3); copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); @@ -3295,8 +3290,8 @@ bool mjCModel::CopyBack(const mjModel* m) { // sites for (int i=0; isize, m->site_size + 3 * i, 3); - copyvec(sites[i]->locpos, m->site_pos+3*i, 3); - copyvec(sites[i]->locquat, m->site_quat+4*i, 4); + copyvec(sites[i]->pos, m->site_pos+3*i, 3); + copyvec(sites[i]->quat, m->site_quat+4*i, 4); copyvec(sites[i]->rgba, m->site_rgba+4*i, 4); if (nuser_site) { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4530b661..3d7ef0b8 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -559,12 +559,8 @@ mjCBody::mjCBody(mjCModel* _model) { mocap = false; mjuu_setvec(quat, 1, 0, 0, 0); mjuu_setvec(iquat, 1, 0, 0, 0); - mjuu_setvec(locquat, 1, 0, 0, 0); - mjuu_setvec(lociquat, 1, 0, 0, 0); mjuu_zerovec(pos+1, 2); mjuu_zerovec(ipos+1, 2); - mjuu_zerovec(locpos, 3); - mjuu_zerovec(locipos, 3); mass = 0; mjuu_setvec(inertia, 0, 0, 0); parentid = -1; @@ -896,13 +892,6 @@ void mjCBody::GeomFrame(void) { -// setup child local frame: pos -void mjCBody::MakeLocal(double* _locpos, double* _locquat, - const double* _pos, const double* _quat) { - mjuu_copyvec(_locpos, _pos, 3); - mjuu_copyvec(_locquat, _quat, 4); -} - // set explicitinertial to true void mjCBody::MakeInertialExplicit() { explicitinertial = true; @@ -1007,21 +996,6 @@ void mjCBody::Compile(void) { mjuu_frameaccum(pos, quat, frame->pos, frame->quat); } - // compute local frame rel. to parent body - if (id>0) { - model->bodies[parentid]->MakeLocal(locpos, locquat, pos, quat); - } - - // make local inertial frame relative to this body - if (id>0) { - MakeLocal(locipos, lociquat, ipos, iquat); - } - - // make local frames of geoms - for (int i=0; ilocpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat); - } - // accumulate rbound, contype, conaffinity over geoms contype = conaffinity = 0; margin = 0; @@ -1071,9 +1045,9 @@ void mjCBody::Compile(void) { // compute body global pose (no joint transformations in qpos0) if (id>0) { mjCBody* par = model->bodies[parentid]; - mju_rotVecQuat(xpos0, locpos, par->xquat0); + mju_rotVecQuat(xpos0, pos, par->xquat0); mju_addTo3(xpos0, par->xpos0); - mju_mulQuat(xquat0, par->xquat0, locquat); + mju_mulQuat(xquat0, par->xquat0, quat); } // compile all sites @@ -1168,8 +1142,6 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { // clear internal variables body = 0; - mjuu_setvec(locpos, 0, 0, 0); - mjuu_setvec(locaxis, 0, 0, 1); urdfeffort = -1; // reset to default if given @@ -1287,20 +1259,13 @@ int mjCJoint::Compile(void) { } // compute local position - if (type!=mjJNT_FREE) { + if (type == mjJNT_FREE) { + mjuu_zerovec(pos, 3); + } else if (frame) { double qunit[4] = {1, 0, 0, 0}; - double qloc[4]; - if (frame) { - mjuu_frameaccum(pos, qunit, frame->pos, frame->quat); - } - body->MakeLocal(locpos, qloc, pos, qunit); - } else { - mjuu_zerovec(locpos, 3); + mjuu_frameaccum(pos, qunit, frame->pos, frame->quat); } - // copy axis to local - mjuu_copyvec(locaxis, axis, 3); - // convert reference angles to radians for hinge joints if (type==mjJNT_HINGE && model->degree) { ref *= mjPI/180.0; @@ -1364,8 +1329,6 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { // clear internal variables mjuu_setvec(quat, 1, 0, 0, 0); mjuu_setvec(pos, 0, 0, 0); - mjuu_setvec(locpos, 0, 0, 0); - mjuu_setvec(locquat, 1, 0, 0, 0); mass = 0; mjuu_setvec(inertia, 0, 0, 0); body = 0; @@ -1940,8 +1903,6 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) { // clear internal variables material.clear(); body = 0; - mjuu_setvec(locpos, 0, 0, 0); - mjuu_setvec(locquat, 1, 0, 0, 0); matid = -1; // reset to default if given @@ -2037,9 +1998,6 @@ void mjCSite::Compile(void) { // check size parameters checksize(size, type, this, name.c_str(), id); - - // ask parent body to compute our local pos and quat relative to itself - body->MakeLocal(locpos, locquat, pos, quat); } @@ -2066,8 +2024,6 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) { // clear private variables body = 0; - mjuu_setvec(locpos, 0, 0, 0); - mjuu_setvec(locquat, 1, 0, 0, 0); targetbodyid = -1; // reset to default if given @@ -2105,9 +2061,6 @@ void mjCCamera::Compile(void) { // normalize quaternion mjuu_normvec(quat, 4); - // ask parent body to compute our local pos and quat relative to itself - body->MakeLocal(locpos, locquat, pos, quat); - // get targetbodyid if (!targetbody.empty()) { mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody); @@ -2186,8 +2139,6 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) { // clear private variables body = 0; - mjuu_setvec(locpos, 0, 0, 0); - mjuu_setvec(locdir, 0, 0, 0); targetbodyid = -1; // reset to default if given @@ -2204,7 +2155,7 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) { // compiler void mjCLight::Compile(void) { - double locquat[4], quat[4]= {1, 0, 0, 0}; + double quat[4]= {1, 0, 0, 0}; // frame if (frame) { @@ -2216,12 +2167,6 @@ void mjCLight::Compile(void) { throw mjCError(this, "zero direction in light '%s' (id = %d)", name.c_str(), id); } - // ask parent body to compute our local pos and quat relative to itself - body->MakeLocal(locpos, locquat, pos, quat); - - // copy dir to local frame - mjuu_copyvec(locdir, dir, 3); - // get targetbodyid if (!targetbody.empty()) { mjCBody* tb = (mjCBody*)model->FindObject(mjOBJ_BODY, targetbody); @@ -3417,7 +3362,6 @@ void mjCEquality::Compile(void) { mjtObj objtype; mjCBase *px1, *px2; mjtJoint jt1, jt2; - double anchor[3], qdummy[4], qunit[4] = {1, 0, 0, 0}; // determine object type if (type==mjEQ_CONNECT || type==mjEQ_WELD) { @@ -3484,17 +3428,6 @@ void mjCEquality::Compile(void) { name.c_str(), id); } } - - // connect: convert anchor to body1 local coordinates - if (type==mjEQ_CONNECT) { - ((mjCBody*)px1)->MakeLocal(anchor, qdummy, data, qunit); - mjuu_copyvec(data, anchor, 3); - } else if (type==mjEQ_WELD) { - if (px2) { - ((mjCBody*)px2)->MakeLocal(anchor, qdummy, data, qunit); - mjuu_copyvec(data, anchor, 3); - } - } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 9be6ae73..ab9a1aa9 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -232,9 +232,6 @@ class mjCBody : public mjCBase { mjCBase* GetObject(mjtObj type, int id); mjCBase* FindObject(mjtObj type, std::string name, bool recursive = true); - // setup child local frame, take into account change - void MakeLocal(double* locpos, double* locquat, const double* pos, const double* quat); - // set explicitinertial to true void MakeInertialExplicit(); @@ -259,10 +256,6 @@ class mjCBody : public mjCBase { void GeomFrame(void); // get inertial info from geoms - double locpos[3]; // position relative to parent - double locquat[4]; // orientation relative to parent - double locipos[3]; // inertial position frame, rel. to local frame - double lociquat[4]; // inertial frame orientation int parentid; // parent index in global array int weldid; // top index of body we are welded to int dofnum; // number of motion dofs for body @@ -359,8 +352,6 @@ class mjCJoint : public mjCBase { int Compile(void); // compiler; return dofnum mjCBody* body; // joint's body - double locpos[3]; // anchor position in child or parent - double locaxis[3]; // joint axis in child or parent }; @@ -436,8 +427,6 @@ class mjCGeom : public mjCBase { int hfieldid; // id of geom's hfield (-1: none) double mass; // mass double inertia[3]; // local diagonal inertia - double locpos[3]; // local position - double locquat[4]; // local orientation double aabb[6]; // axis-aligned bounding box (center, size) mjCBody* body; // geom's body }; @@ -473,8 +462,6 @@ class mjCSite : public mjCBase { void Compile(void); // compiler mjCBody* body; // site's body - double locpos[3]; // local position - double locquat[4]; // local orientation int matid; // material id for rendering }; @@ -512,8 +499,6 @@ class mjCCamera : public mjCBase { void Compile(void); // compiler mjCBody* body; // camera's body - double locpos[3]; // local position - double locquat[4]; // local orientation int targetbodyid; // id of target body; -1: none }; @@ -549,8 +534,6 @@ class mjCLight : public mjCBase { void Compile(void); // compiler mjCBody* body; // light's body - double locpos[3]; // local position - double locdir[3]; // local direction int targetbodyid; // id of target body; -1: none }; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index f33baddb..63d22b72 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -280,10 +280,10 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { WriteAttrTxt(elem, "name", pjoint->name); WriteAttrTxt(elem, "class", pjoint->classname); if (pjoint->type != mjJNT_FREE) { - WriteAttr(elem, "pos", 3, pjoint->locpos); + WriteAttr(elem, "pos", 3, pjoint->pos); } if (pjoint->type != mjJNT_FREE && pjoint->type != mjJNT_BALL) { - WriteAttr(elem, "axis", 3, pjoint->locaxis); + WriteAttr(elem, "axis", 3, pjoint->axis); } } @@ -350,12 +350,12 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { mjCMesh* pmesh = model->meshes[pgeom->meshid]; // write pos/quat if there is a difference - if (!SameVector(pgeom->locpos, pmesh->GetPosPtr(pgeom->typeinertia), 3) || - !SameVector(pgeom->locquat, pmesh->GetQuatPtr(pgeom->typeinertia), 4)) { + if (!SameVector(pgeom->pos, pmesh->GetPosPtr(pgeom->typeinertia), 3) || + !SameVector(pgeom->quat, pmesh->GetQuatPtr(pgeom->typeinertia), 4)) { // recover geom pos/quat before mesh frame transformation double p[3], q[4]; - mjuu_copyvec(p, pgeom->locpos, 3); - mjuu_copyvec(q, pgeom->locquat, 4); + mjuu_copyvec(p, pgeom->pos, 3); + mjuu_copyvec(q, pgeom->quat, 4); mjuu_frameaccuminv(p, q, pmesh->GetPosPtr(pgeom->typeinertia), pmesh->GetQuatPtr(pgeom->typeinertia)); @@ -367,8 +367,8 @@ void mjXWriter::OneGeom(XMLElement* elem, mjCGeom* pgeom, mjCDef* def) { // non-mesh geom else { - WriteAttr(elem, "pos", 3, pgeom->locpos, unitq+1); - WriteAttr(elem, "quat", 4, pgeom->locquat, unitq); + WriteAttr(elem, "pos", 3, pgeom->pos, unitq+1); + WriteAttr(elem, "quat", 4, pgeom->quat, unitq); } } else { WriteAttr(elem, "size", 3, pgeom->size, def->geom.size); @@ -432,8 +432,8 @@ void mjXWriter::OneSite(XMLElement* elem, mjCSite* psite, mjCDef* def) { if (!writingdefaults) { WriteAttrTxt(elem, "name", psite->name); WriteAttrTxt(elem, "class", psite->classname); - WriteAttr(elem, "pos", 3, psite->locpos); - WriteAttr(elem, "quat", 4, psite->locquat, unitq); + WriteAttr(elem, "pos", 3, psite->pos); + WriteAttr(elem, "quat", 4, psite->quat, unitq); if (mjGEOMINFO[psite->type]) { WriteAttr(elem, "size", mjGEOMINFO[psite->type], psite->size, def->site.size); } @@ -468,8 +468,8 @@ void mjXWriter::OneCamera(XMLElement* elem, mjCCamera* pcam, mjCDef* def) { WriteAttrTxt(elem, "name", pcam->name); WriteAttrTxt(elem, "class", pcam->classname); WriteAttrTxt(elem, "target", pcam->targetbody); - WriteAttr(elem, "pos", 3, pcam->locpos); - WriteAttr(elem, "quat", 4, pcam->locquat, unitq); + WriteAttr(elem, "pos", 3, pcam->pos); + WriteAttr(elem, "quat", 4, pcam->quat, unitq); } // defaults and regular @@ -508,8 +508,8 @@ void mjXWriter::OneLight(XMLElement* elem, mjCLight* plight, mjCDef* def) { WriteAttrTxt(elem, "name", plight->name); WriteAttrTxt(elem, "class", plight->classname); WriteAttrTxt(elem, "target", plight->targetbody); - WriteAttr(elem, "pos", 3, plight->locpos); - WriteAttr(elem, "dir", 3, plight->locdir); + WriteAttr(elem, "pos", 3, plight->pos); + WriteAttr(elem, "dir", 3, plight->dir); } // defaults and regular @@ -1466,10 +1466,10 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { WriteAttrTxt(elem, "childclass", body->classname); // write pos if it's not {0, 0, 0} - if (body->locpos[0] || body->locpos[1] || body->locpos[2]) { - WriteAttr(elem, "pos", 3, body->locpos); + if (body->pos[0] || body->pos[1] || body->pos[2]) { + WriteAttr(elem, "pos", 3, body->pos); } - WriteAttr(elem, "quat", 4, body->locquat, unitq); + WriteAttr(elem, "quat", 4, body->quat, unitq); if (body->mocap) { WriteAttrKey(elem, "mocap", bool_map, 2, 1); } @@ -1485,8 +1485,8 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { if (body->explicitinertial && model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) { XMLElement* inertial = InsertEnd(elem, "inertial"); - WriteAttr(inertial, "pos", 3, body->locipos); - WriteAttr(inertial, "quat", 4, body->lociquat, unitq); + WriteAttr(inertial, "pos", 3, body->ipos); + WriteAttr(inertial, "quat", 4, body->iquat, unitq); WriteAttr(inertial, "mass", 1, &body->mass); WriteAttr(inertial, "diaginertia", 3, body->inertia); } @@ -2036,9 +2036,9 @@ void mjXWriter::Keyframe(XMLElement* root) { if (model->bodies[j]->mocap) { mjCBody* pb = model->bodies[j]; int id = pb->mocapid; - if (pb->locpos[0] != pk->mpos[3*id] || - pb->locpos[1] != pk->mpos[3*id+1] || - pb->locpos[2] != pk->mpos[3*id+2]) { + if (pb->pos[0] != pk->mpos[3*id] || + pb->pos[1] != pk->mpos[3*id+1] || + pb->pos[2] != pk->mpos[3*id+2]) { WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos.data()); change = true; break; @@ -2053,10 +2053,10 @@ void mjXWriter::Keyframe(XMLElement* root) { if (model->bodies[j]->mocap) { mjCBody* pb = model->bodies[j]; int id = pb->mocapid; - if (pb->locquat[0] != pk->mquat[4*id] || - pb->locquat[1] != pk->mquat[4*id+1] || - pb->locquat[2] != pk->mquat[4*id+2] || - pb->locquat[3] != pk->mquat[4*id+3]) { + if (pb->quat[0] != pk->mquat[4*id] || + pb->quat[1] != pk->mquat[4*id+1] || + pb->quat[2] != pk->mquat[4*id+2] || + pb->quat[3] != pk->mquat[4*id+3]) { WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat.data()); change = true; break;