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

Delete local frame members from MJC objects.

Browse Source 
PiperOrigin-RevId: 585629699
Change-Id: I12dbd235dee49223fa7b1a097f04561da69550a0
This commit is contained in:
Yuval Tassa
2023-11-27 06:06:56 -08:00
committed by Copybara-Service
parent ca046cbf64
commit 21334ea704
4 changed files with 96 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+63 -68
View File
@@ -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->mass<mjMINVAL) {
par->mass = 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; j<body->bodies.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; i<nbody; i++) {
pb = bodies[i];
copyvec(pb->locpos, 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; i<nsite; i++) {
copyvec(sites[i]->size, 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) {
src/user/user_objects.cc
+7 -74
View File
@@ -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; i<geoms.size(); i++) {
MakeLocal(geoms[i]->locpos, 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);
}
}
}
src/user/user_objects.h
-17
View File
@@ -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
};
src/xml/xml_native_writer.cc
+26 -26
View File
@@ -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;