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

Add spaces around comparison operators in engine source files.

Browse Source 
PiperOrigin-RevId: 535989348
Change-Id: I883f7e82351299933c49b35a31842b5d8d6aea04
This commit is contained in:
Yuval Tassa
2023-05-28 05:01:55 -07:00
committed by Copybara-Service
parent d40c395917
commit 455b1cd2e2
29 changed files with 2224 additions and 2219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_setconst.c
+66 -66
View File
@@ -42,14 +42,14 @@ static void set0(mjModel* m, mjData* d) {
// save camera and light mode, set to fixed
if (m->ncam) {
cammode = (int*) mj_stackAlloc(d, m->ncam);
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
cammode[i] = m->cam_mode[i];
m->cam_mode[i] = mjCAMLIGHT_FIXED;
}
}
if (m->nlight) {
lightmode = (int*) mj_stackAlloc(d, m->nlight);
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
lightmode[i] = m->light_mode[i];
m->light_mode[i] = mjCAMLIGHT_FIXED;
}
@@ -63,7 +63,7 @@ static void set0(mjModel* m, mjData* d) {
mj_crbSkip(m, d, 0);
// save dof_M0
for (int i=0; i<nv; i++) {
for (int i=0; i < nv; i++) {
m->dof_M0[i] = d->qM[m->dof_Madr[i]];
}
@@ -73,10 +73,10 @@ static void set0(mjModel* m, mjData* d) {
mj_transmission(m, d);
// restore camera and light mode
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
m->cam_mode[i] = cammode[i];
}
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
m->light_mode[i] = lightmode[i];
}
@@ -86,7 +86,7 @@ static void set0(mjModel* m, mjData* d) {
// compute body_invweight0
m->body_invweight0[0] = m->body_invweight0[1] = 0.0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
if (nv) {
// inverse spatial inertia: A = J*inv(M)*J'
mj_jacBodyCom(m, d, jac, jac+3*nv, i);
@@ -100,13 +100,13 @@ static void set0(mjModel* m, mjData* d) {
}
// compute dof_invweight0
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
id = m->jnt_dofadr[i];
// get number of components
if (m->jnt_type[i]==mjJNT_FREE) {
if (m->jnt_type[i] == mjJNT_FREE) {
dnum = 6;
} else if (m->jnt_type[i]==mjJNT_BALL) {
} else if (m->jnt_type[i] == mjJNT_BALL) {
dnum = 3;
} else {
dnum = 1;
@@ -115,7 +115,7 @@ static void set0(mjModel* m, mjData* d) {
// inverse joint inertia: A = J*inv(M)*J'
if (nv) {
mju_zero(jac, dnum*nv);
for (int j=0; j<dnum; j++) {
for (int j=0; j < dnum; j++) {
jac[j*(nv+1) + id] = 1;
}
mj_solveM(m, d, tmp, jac, dnum);
@@ -123,14 +123,14 @@ static void set0(mjModel* m, mjData* d) {
}
// average diagonal and assign
if (dnum==6) {
if (dnum == 6) {
m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] =
(A[0] + A[7] + A[14])/3;
(A[0] + A[7] + A[14])/3;
m->dof_invweight0[id+3] = m->dof_invweight0[id+4] = m->dof_invweight0[id+5] =
(A[21] + A[28] + A[35])/3;
} else if (dnum==3)
(A[21] + A[28] + A[35])/3;
} else if (dnum == 3)
m->dof_invweight0[id] = m->dof_invweight0[id+1] = m->dof_invweight0[id+2] =
(A[0] + A[4] + A[8])/3;
(A[0] + A[4] + A[8])/3;
else {
m->dof_invweight0[id] = A[0];
}
@@ -138,12 +138,12 @@ static void set0(mjModel* m, mjData* d) {
// compute tendon_invweight0
if (nv) {
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
int end = d->ten_J_rowadr[i] + d->ten_J_rownnz[i];
for (int j=d->ten_J_rowadr[i]; j<end; j++) {
for (int j=d->ten_J_rowadr[i]; j < end; j++) {
tmp[d->ten_J_colind[j]] = d->ten_J[j];
}
} else {
@@ -156,24 +156,24 @@ static void set0(mjModel* m, mjData* d) {
}
// compute actuator_acc0
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
mj_solveM(m, d, tmp, d->actuator_moment+i*nv, 1);
m->actuator_acc0[i] = mju_norm(tmp, nv);
}
} else {
for (int i=0; i<m->nu; i++) {
for (int i=0; i < m->nu; i++) {
m->actuator_acc0[i] = 0;
}
}
// compute missing eq_data for body constraints
for (int i=0; i<m->neq; i++) {
for (int i=0; i < m->neq; i++) {
// get ids
id1 = m->eq_obj1id[i];
id2 = m->eq_obj2id[i];
// connect constraint
if (m->eq_type[i]==mjEQ_CONNECT) {
if (m->eq_type[i] == mjEQ_CONNECT) {
// pos = anchor position in global frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
@@ -183,7 +183,7 @@ static void set0(mjModel* m, mjData* d) {
}
// weld constraint
else if (m->eq_type[i]==mjEQ_WELD) {
else if (m->eq_type[i] == mjEQ_WELD) {
// skip if user has set any quaternion data
if (m->eq_data[mjNEQDATA*i+6] ||
m->eq_data[mjNEQDATA*i+7] ||
@@ -208,28 +208,28 @@ static void set0(mjModel* m, mjData* d) {
}
// camera compos0, pos0, mat0
for (int i=0; i<m->ncam; i++) {
for (int i=0; i < m->ncam; i++) {
// get body ids
id = m->cam_bodyid[i]; // camera body
id1 = m->cam_targetbodyid[i]; // target body
// compute positional offsets
mju_sub3(m->cam_pos0+3*i, d->cam_xpos+3*i, d->xpos+3*id);
mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1>=0 ? 3*id1 : 3*id));
mju_sub3(m->cam_poscom0+3*i, d->cam_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
// copy mat
mju_copy(m->cam_mat0+9*i, d->cam_xmat+9*i, 9);
}
// light compos0, pos0, dir0
for (int i=0; i<m->nlight; i++) {
for (int i=0; i < m->nlight; i++) {
// get body ids
id = m->light_bodyid[i]; // light body
id1 = m->light_targetbodyid[i]; // target body
// compute positional offsets
mju_sub3(m->light_pos0+3*i, d->light_xpos+3*i, d->xpos+3*id);
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1>=0 ? 3*id1 : 3*id));
mju_sub3(m->light_poscom0+3*i, d->light_xpos+3*i, d->subtree_com+ (id1 >= 0 ? 3*id1 : 3*id));
// copy dir
mju_copy3(m->light_dir0+3*i, d->light_xdir+3*i);
@@ -242,7 +242,7 @@ static void set0(mjModel* m, mjData* d) {
// accumulate bounding box
static void updateBox(mjtNum* xmin, mjtNum* xmax, mjtNum* pos, mjtNum radius) {
for (int i=0; i<3; i++) {
for (int i=0; i < 3; i++) {
xmin[i] = mjMIN(xmin[i], pos[i] - radius);
xmax[i] = mjMAX(xmax[i], pos[i] + radius);
}
@@ -258,22 +258,22 @@ static void setStat(mjModel* m, mjData* d) {
mjtNum* body = mj_stackAlloc(d, m->nbody);
// compute bounding box of bodies, joint centers, geoms and sites
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
updateBox(xmin, xmax, d->xpos+3*i, 0);
updateBox(xmin, xmax, d->xipos+3*i, 0);
}
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
updateBox(xmin, xmax, d->xanchor+3*i, 0);
}
for (int i=0; i<m->nsite; i++) {
for (int i=0; i < m->nsite; i++) {
updateBox(xmin, xmax, d->site_xpos+3*i, 0);
}
for (int i=0; i<m->ngeom; i++) {
for (int i=0; i < m->ngeom; i++) {
// set rbound: regular geom rbound, or 0.1 of plane or hfield max size
rbound = 0;
if (m->geom_rbound[i] > 0) {
rbound = m->geom_rbound[i];
} else if (m->geom_type[i]==mjGEOM_PLANE) {
} else if (m->geom_type[i] == mjGEOM_PLANE) {
// finite in at least one direction
if (m->geom_size[3*i] || m->geom_size[3*i+1]) {
rbound = mjMAX(m->geom_size[3*i], m->geom_size[3*i+1]) * 0.1;
@@ -283,7 +283,7 @@ static void setStat(mjModel* m, mjData* d) {
else {
rbound = 1;
}
} else if (m->geom_type[i]==mjGEOM_HFIELD) {
} else if (m->geom_type[i] == mjGEOM_HFIELD) {
int j = m->geom_dataid[i];
rbound = mjMAX(m->hfield_size[4*j],
mjMAX(m->hfield_size[4*j+1],
@@ -298,13 +298,13 @@ static void setStat(mjModel* m, mjData* d) {
mju_scl3(m->stat.center, m->stat.center, 0.5);
// compute bounding box size
if (xmax[0]>xmin[0])
if (xmax[0] > xmin[0])
m->stat.extent = mju_max(1E-5,
mju_max(xmax[0]-xmin[0], mju_max(xmax[1]-xmin[1], xmax[2]-xmin[2])));
// set body size to max com-joint distance
mju_zero(body, m->nbody);
for (int i=0; i<m->njnt; i++) {
for (int i=0; i < m->njnt; i++) {
// handle this body
int id = m->jnt_bodyid[i];
body[id] = mju_max(body[id], mju_dist3(d->xipos+3*id, d->xanchor+3*i));
@@ -316,18 +316,18 @@ static void setStat(mjModel* m, mjData* d) {
body[0] = 0;
// set body size to max of old value, and geom rbound + com-geom dist
for (int i=1; i<m->nbody; i++) {
for (int id=m->body_geomadr[i]; id<m->body_geomadr[i]+m->body_geomnum[i]; id++) {
if (m->geom_rbound[id]>0) {
for (int i=1; i < m->nbody; i++) {
for (int id=m->body_geomadr[i]; id < m->body_geomadr[i]+m->body_geomnum[i]; id++) {
if (m->geom_rbound[id] > 0) {
body[i] = mju_max(body[i], m->geom_rbound[id] + mju_dist3(d->xipos+3*i, d->geom_xpos+3*id));
}
}
}
// compute meansize, make sure all sizes are above min
if (m->nbody>1) {
if (m->nbody > 1) {
m->stat.meansize = 0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
body[i] = mju_max(body[i], 1E-5);
m->stat.meansize += body[i]/(m->nbody-1);
}
@@ -337,9 +337,9 @@ static void setStat(mjModel* m, mjData* d) {
m->stat.extent = mju_max(m->stat.extent, 2 * m->stat.meansize);
// compute meanmass
if (m->nbody>1) {
if (m->nbody > 1) {
m->stat.meanmass = 0;
for (int i=1; i<m->nbody; i++) {
for (int i=1; i < m->nbody; i++) {
m->stat.meanmass += m->body_mass[i];
}
m->stat.meanmass /= (m->nbody-1);
@@ -348,7 +348,7 @@ static void setStat(mjModel* m, mjData* d) {
// compute meaninertia
if (m->nv) {
m->stat.meaninertia = 0;
for (int i=0; i<m->nv; i++) {
for (int i=0; i < m->nv; i++) {
m->stat.meaninertia += d->qM[m->dof_Madr[i]];
}
m->stat.meaninertia /= m->nv;
@@ -369,7 +369,7 @@ static void setSpring(mjModel* m, mjData* d) {
mj_transmission(m, d);
// copy if model spring length is -1
for (int i=0; i<m->ntendon; i++) {
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_lengthspring[2*i] == -1 && m->tendon_lengthspring[2*i+1] == -1) {
// explicit springlength unused, set equal to ten_length
m->tendon_lengthspring[2*i] = m->tendon_lengthspring[2*i+1] = d->ten_length[i];
@@ -382,10 +382,10 @@ static void setSpring(mjModel* m, mjData* d) {
// entry point: set all constant fields of mjModel, except for lengthrange
void mj_setConst(mjModel* m, mjData* d) {
// compute subtreemass
for (int i=0; i<m->nbody; i++) {
for (int i=0; i < m->nbody; i++) {
m->body_subtreemass[i] = m->body_mass[i];
}
for (int i=m->nbody-1; i>0; i--) {
for (int i=m->nbody-1; i > 0; i--) {
m->body_subtreemass[m->body_parentid[i]] += m->body_subtreemass[i];
}
@@ -418,7 +418,7 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
// impose maxforce
nrm = mju_norm(d->qfrc_applied, nv);
if (opt->maxforce>0 && nrm>opt->maxforce) {
if (opt->maxforce > 0 && nrm > opt->maxforce) {
mju_scl(d->qfrc_applied, d->qfrc_applied, opt->maxforce/mjMAX(mjMINVAL, nrm), nv);
}
@@ -435,18 +435,18 @@ static mjtNum evalAct(const mjModel* m, mjData* d, int index, int side,
int mj_setLengthRange(mjModel* m, mjData* d, int index,
const mjLROpt* opt, char* error, int error_sz) {
// check index
if (index<0 || index>=m->nu) {
if (index < 0 || index >= m->nu) {
mju_error("Invalid actuator index in mj_setLengthRange");
}
// skip depending on mode and type
int ismuscle = (m->actuator_gaintype[index]==mjGAIN_MUSCLE ||
m->actuator_biastype[index]==mjBIAS_MUSCLE);
int isuser = (m->actuator_gaintype[index]==mjGAIN_USER ||
m->actuator_biastype[index]==mjBIAS_USER);
if ((opt->mode==mjLRMODE_NONE) ||
(opt->mode==mjLRMODE_MUSCLE && !ismuscle) ||
(opt->mode==mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) {
int ismuscle = (m->actuator_gaintype[index] == mjGAIN_MUSCLE ||
m->actuator_biastype[index] == mjBIAS_MUSCLE);
int isuser = (m->actuator_gaintype[index] == mjGAIN_USER ||
m->actuator_biastype[index] == mjBIAS_USER);
if ((opt->mode == mjLRMODE_NONE) ||
(opt->mode == mjLRMODE_MUSCLE && !ismuscle) ||
(opt->mode == mjLRMODE_MUSCLEUSER && !ismuscle && !isuser)) {
return 1;
}
@@ -461,8 +461,8 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
// use joint and tendon limits if available
if (opt->uselimit) {
// joint or jointinparent
if (m->actuator_trntype[index]==mjTRN_JOINT ||
m->actuator_trntype[index]==mjTRN_JOINTINPARENT) {
if (m->actuator_trntype[index] == mjTRN_JOINT ||
m->actuator_trntype[index] == mjTRN_JOINTINPARENT) {
// make sure joint is limited
if (m->jnt_limited[threadid]) {
// copy range
@@ -475,7 +475,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// tendon
if (m->actuator_trntype[index]==mjTRN_TENDON) {
if (m->actuator_trntype[index] == mjTRN_TENDON) {
// make sure tendon is limited
if (m->tendon_limited[threadid]) {
// copy range
@@ -491,7 +491,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
// optimize in both directions
mjtNum lmin[2] = {0, 0}, lmax[2] = {0, 0};
int side;
for (side=0; side<2; side++) {
for (side=0; side < 2; side++) {
// init at qpos0
mj_resetData(m, d);
@@ -502,17 +502,17 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
mjtNum len = evalAct(m, d, index, side, opt);
// reset: cannot proceed
if (d->time==0) {
if (d->time == 0) {
snprintf(error, error_sz, "Unstable lengthrange simulation in actuator %d", index);
return 0;
}
// update limits
if (d->time > opt->inttotal-opt->interval) {
if (len<lmin[side] || !updated) {
if (len < lmin[side] || !updated) {
lmin[side] = len;
}
if (len>lmax[side] || !updated) {
if (len > lmax[side] || !updated) {
lmax[side] = len;
}
@@ -521,12 +521,12 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// assign
m->actuator_lengthrange[2*index+side] = (side==0 ? lmin[side] : lmax[side]);
m->actuator_lengthrange[2*index+side] = (side == 0 ? lmin[side] : lmax[side]);
}
// check range
mjtNum dif = m->actuator_lengthrange[2*index+1] - m->actuator_lengthrange[2*index];
if (dif<=0) {
if (dif <= 0) {
snprintf(error, error_sz,
"Invalid lengthrange (%g, %g) in actuator %d",
m->actuator_lengthrange[2*index],
@@ -535,7 +535,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// check convergence, side 0
if (lmax[0]-lmin[0]>opt->tolrange*dif) {
if (lmax[0]-lmin[0] > opt->tolrange*dif) {
snprintf(error, error_sz,
"Lengthrange computation did not converge in actuator %d:\n"
" eval (%g, %g)\n range (%g, %g)",
@@ -546,7 +546,7 @@ int mj_setLengthRange(mjModel* m, mjData* d, int index,
}
// check convergence, side 1
if (lmax[1]-lmin[1]>opt->tolrange*dif) {
if (lmax[1]-lmin[1] > opt->tolrange*dif) {
snprintf(error, error_sz,
"Lengthrange computation did not converge in actuator %d:\n"
" eval (%g, %g)\n range (%g, %g)",