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_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);
}
}
}