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

Make switch statements over enums in engine source use a typed expression to support compiler checking of full coverage.

Browse Source 
PiperOrigin-RevId: 556854229
Change-Id: I36a5d146d776e10b946e2f9b51ace4ef3866471b
This commit is contained in:
Yuval Tassa
2023-08-14 11:16:53 -07:00
committed by Copybara-Service
parent 13490962a8
commit e4dddea42a
15 changed files with 71 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+9 -6
View File
@@ -58,7 +58,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mju_rotVecMatT(dir, _dir->v, d->geom_xmat+9*g);
// compute result according to geom type
switch (m->geom_type[g]) {
switch ((mjtGeom) m->geom_type[g]) {
case mjGEOM_SPHERE:
mju_scl3(res, dir, size[0]);
break;
@@ -877,21 +877,21 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// get geom ids and types
int gid[2] = {g1, g2};
int type[2];
mjtGeom type[2];
for (int i=0; i < 2; i++) {
type[i] = m->geom_type[gid[i]];
// set to -1 if type cannot be processed
// set to mjGEOM_NONE if type cannot be processed
if (type[i] != mjGEOM_SPHERE &&
type[i] != mjGEOM_CAPSULE &&
type[i] != mjGEOM_ELLIPSOID &&
type[i] != mjGEOM_CYLINDER) {
type[i] = -1;
type[i] = mjGEOM_NONE;
}
}
// neither type can be processed: nothing to do
if (type[0] < 0 && type[1] < 0) {
if (type[0] == mjGEOM_NONE && type[1] == mjGEOM_NONE) {
return;
}
@@ -904,7 +904,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
// process geoms in type range
int processed[2] = {0, 0};
for (int i=0; i < 2; i++) {
if (type[i] >= 0) {
if (type[i] != mjGEOM_NONE) {
// get geom mat and size
mjtNum* mat = d->geom_xmat + 9*gid[i];
mjtNum* size = m->geom_size + 3*gid[i];
@@ -984,6 +984,9 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
nrm[2] = 0;
processed[i] = 1;
break;
default:
// do nothing: only sphere, capsule, ellipsoid and cylinder are processed
break;
}
// normalize and map normal to global frame
src/engine/engine_core_constraint.c
+12 -5
View File
@@ -408,7 +408,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
NV2 = 0;
// process according to type
switch (m->eq_type[i]) {
switch ((mjtEq) m->eq_type[i]) {
case mjEQ_CONNECT: // connect bodies with ball joint
// find global points
for (int j=0; j < 2; j++) {
@@ -898,7 +898,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
}
// process according to constraint type
switch (d->efc_type[i]) {
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_EQUALITY:
// process according to equality-constraint type
switch (m->eq_type[id]) {
@@ -1003,7 +1003,7 @@ static void getsolparam(const mjModel* m, const mjData* d, int i,
mju_zero(solreffriction, mjNREF);
// extract solver parameters from corresponding model element
switch (d->efc_type[i]) {
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_EQUALITY:
mju_copy(solref, m->eq_solref+mjNREF*id, mjNREF);
mju_copy(solimp, m->eq_solimp+mjNIMP*id, mjNIMP);
@@ -1079,7 +1079,7 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int
*pos = d->efc_pos[i];
// change (dim, distance) for special cases
switch (d->efc_type[i]) {
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_CONTACT_ELLIPTIC:
*dim = d->contact[id].dim;
break;
@@ -1104,6 +1104,10 @@ static void getposdim(const mjModel* m, const mjData* d, int i, mjtNum* pos, int
*dim = 3;
*pos = mju_norm(d->efc_pos+i, 3);
}
break;
default:
// already handled
break;
}
}
@@ -1368,7 +1372,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
NV2 = 0;
// process according to type
switch (m->eq_type[i]) {
switch ((mjtEq) m->eq_type[i]) {
case mjEQ_CONNECT:
size = 3;
if (!nnz) {
@@ -1419,6 +1423,9 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
NV = 2;
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown constraint type type %d", m->eq_type[i]); // SHOULD NOT OCCUR
}
ne += mj_addConstraintCount(m, size, NV);
nnze += size*NV;
src/engine/engine_core_smooth.c
+11 -7
View File
@@ -99,7 +99,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
// get joint id, qpos address, joint type
int jid = m->body_jntadr[i] + j;
int qadr = m->jnt_qposadr[jid];
int jtype = m->jnt_type[jid];
mjtJoint jtype = m->jnt_type[jid];
// compute axis in global frame; ball jnt_axis is (0,0,1), set by compiler
mju_rotVecQuat(xaxis, m->jnt_axis+3*jid, quat);
@@ -228,7 +228,7 @@ void mj_comPos(const mjModel* m, mjData* d) {
// create motion dof
int skip = 0;
switch (m->jnt_type[j]) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// translation components: x, y, z in global frame
mju_zero(d->cdof+da, 18);
@@ -281,7 +281,9 @@ void mj_camlight(const mjModel* m, mjData* d) {
int id1 = m->cam_targetbodyid[i];
// adjust for mode
switch (m->cam_mode[i]) {
switch ((mjtCamLight) m->cam_mode[i]) {
case mjCAMLIGHT_FIXED:
break;
case mjCAMLIGHT_TRACK:
case mjCAMLIGHT_TRACKCOM:
// fixed global orientation
@@ -341,7 +343,9 @@ void mj_camlight(const mjModel* m, mjData* d) {
int id1 = m->light_targetbodyid[i];
// adjust for mode
switch (m->light_mode[i]) {
switch ((mjtCamLight) m->light_mode[i]) {
case mjCAMLIGHT_FIXED:
break;
case mjCAMLIGHT_TRACK:
case mjCAMLIGHT_TRACKCOM:
// fixed global orientation
@@ -644,7 +648,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
gear = m->actuator_gear+6*i;
// process according to transmission type
switch (m->actuator_trntype[i]) {
switch ((mjtTrn) m->actuator_trntype[i]) {
case mjTRN_JOINT: // joint
case mjTRN_JOINTINPARENT: // joint, force in parent frame
// slide and hinge joint: scalar gear
@@ -1269,7 +1273,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
// cvel = cvel_parent + cdof * qvel, cdofdot = cvel x cdof
for (int j=0; j < m->body_dofnum[i]; j++) {
// compute cvel and cdofdot
switch (m->jnt_type[m->dof_jntid[bda+j]]) {
switch ((mjtJoint) m->jnt_type[m->dof_jntid[bda+j]]) {
case mjJNT_FREE:
// cdofdot = 0
mju_zero(cdofdot, 18);
@@ -1506,7 +1510,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
mjtNum* eq_data = m->eq_data + mjNEQDATA*id;
mjtNum pos[3];
int k;
switch (m->eq_type[id]) {
switch ((mjtEq) m->eq_type[id]) {
case mjEQ_CONNECT:
case mjEQ_WELD:
// cfrc = world-oriented torque:force vector
src/engine/engine_derivative.c
+2 -2
View File
@@ -344,7 +344,7 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt
// Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof)
for (int j=m->body_dofadr[i]; j < m->body_dofadr[i]+m->body_dofnum[i]; j++) {
switch (m->jnt_type[m->dof_jntid[j]]) {
switch ((mjtJoint) m->jnt_type[m->dof_jntid[j]]) {
case mjJNT_FREE:
// Dcdofdot = 0
mju_zero(Dcdofdot+j*6*nv, 18*nv);
@@ -554,7 +554,7 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
int Jadr = (j < nv - 1 ? m->dof_Madr[j + 1] : m->nM) - (m->dof_Madr[j] + 1);
// Dcvel += D(cdof * qvel), Dcdofdot = D(cvel x cdof)
switch (m->jnt_type[m->dof_jntid[j]]) {
switch ((mjtJoint) m->jnt_type[m->dof_jntid[j]]) {
case mjJNT_FREE:
// Dcdofdot = 0 (already cleared)
src/engine/engine_derivative_fd.c
+1 -1
View File
@@ -153,7 +153,7 @@ void mj_stepSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) {
}
// use selected integrator
switch (m->opt.integrator) {
switch ((mjtIntegrator) m->opt.integrator) {
case mjINT_EULER:
mj_EulerSkip(m, d, skipstage >= mjSTAGE_POS);
break;
src/engine/engine_forward.c
+5 -5
View File
@@ -235,7 +235,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
prm = m->actuator_dynprm + i*mjNDYN;
// compute act_dot according to dynamics type
switch (m->actuator_dyntype[i]) {
switch ((mjtDyn) m->actuator_dyntype[i]) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[j] = ctrl[i];
break;
@@ -276,7 +276,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
prm = m->actuator_gainprm + mjNGAIN*i;
// handle according to gain type
switch (m->actuator_gaintype[i]) {
switch ((mjtGain) m->actuator_gaintype[i]) {
case mjGAIN_FIXED: // fixed gain: prm = gain
gain = prm[0];
break;
@@ -321,7 +321,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
prm = m->actuator_biasprm + mjNBIAS*i;
// handle according to bias type
switch (m->actuator_biastype[i]) {
switch ((mjtBias) m->actuator_biastype[i]) {
case mjBIAS_NONE: // none
bias = 0.0;
break;
@@ -511,7 +511,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
d->solver_iter = 0;
// run main solver
switch (m->opt.solver) {
switch ((mjtSolver) m->opt.solver) {
case mjSOL_PGS: // PGS
mj_solPGS(m, d, m->opt.iterations);
break;
@@ -883,7 +883,7 @@ void mj_step(const mjModel* m, mjData* d) {
}
// use selected integrator
switch (m->opt.integrator) {
switch ((mjtIntegrator) m->opt.integrator) {
case mjINT_EULER:
mj_Euler(m, d);
break;
src/engine/engine_io.c
+9 -3
View File
@@ -1746,7 +1746,7 @@ const char* mj_validateReferences(const mjModel* m) {
for (int i=0; i < m->neq; i++) {
int obj1id = m->eq_obj1id[i];
int obj2id = m->eq_obj2id[i];
switch (m->eq_type[i]) {
switch ((mjtEq) m->eq_type[i]) {
case mjEQ_JOINT:
if (obj1id >= m->njnt || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
@@ -1778,12 +1778,13 @@ const char* mj_validateReferences(const mjModel* m) {
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown equality constraint type.");
}
}
for (int i=0; i < m->nwrap; i++) {
int wrap_objid = m->wrap_objid[i];
switch (m->wrap_type[i]) {
switch ((mjtWrap) m->wrap_type[i]) {
case mjWRAP_NONE:
case mjWRAP_PULLEY:
// wrap_objid not used.
@@ -1810,7 +1811,7 @@ const char* mj_validateReferences(const mjModel* m) {
int actuator_trntype = m->actuator_trntype[i];
int id = m->actuator_trnid[2*i];
int idslider = m->actuator_trnid[2*i+1];
switch (actuator_trntype) {
switch ((mjtTrn) actuator_trntype) {
case mjTRN_JOINT:
case mjTRN_JOINTINPARENT:
if (id < 0 || id >= m->njnt) {
@@ -1835,6 +1836,11 @@ const char* mj_validateReferences(const mjModel* m) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_BODY:
if (id < 0 || id >= m->nbody) {
return "Invalid model: actuator_trnid out of bounds.";
}
break;
case mjTRN_UNDEFINED:
// actuator_trnid not used.
break;
src/engine/engine_passive.c
+1 -1
View File
@@ -54,7 +54,7 @@ void mj_passive(const mjModel* m, mjData* d) {
int padr = m->jnt_qposadr[i];
int dadr = m->jnt_dofadr[i];
switch (m->jnt_type[i]) {
switch ((mjtJoint) m->jnt_type[i]) {
case mjJNT_FREE:
// apply force
d->qfrc_passive[dadr+0] -= stiffness*(d->qpos[padr+0] - m->qpos_spring[padr+0]);
src/engine/engine_ray.c
+1 -1
View File
@@ -742,7 +742,7 @@ mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int id,
// intersect ray with pure geom, no meshes or hfields
mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
const mjtNum* pnt, const mjtNum* vec, int geomtype) {
switch (geomtype) {
switch ((mjtGeom) geomtype) {
case mjGEOM_PLANE:
return ray_plane(pos, mat, size, pnt, vec);
src/engine/engine_sensor.c
+8 -8
View File
@@ -134,7 +134,7 @@ static void apply_cutoff(const mjModel* m, mjData* d, mjtStage stage) {
// get xpos and xmat pointers to an object in mjData
static void get_xpos_xmat(const mjData* d, int type, int id, int sensor_id,
static void get_xpos_xmat(const mjData* d, mjtObj type, int id, int sensor_id,
mjtNum **xpos, mjtNum **xmat) {
switch (type) {
case mjOBJ_XBODY:
@@ -163,7 +163,7 @@ static void get_xpos_xmat(const mjData* d, int type, int id, int sensor_id,
}
// get global quaternion of an object in mjData
static void get_xquat(const mjModel* m, const mjData* d, int type, int id, int sensor_id,
static void get_xquat(const mjModel* m, const mjData* d, mjtObj type, int id, int sensor_id,
mjtNum *quat) {
switch (type) {
case mjOBJ_XBODY:
@@ -216,7 +216,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
adr = m->sensor_adr[i];
// process according to type
switch (m->sensor_type[i]) {
switch ((mjtSensor) m->sensor_type[i]) {
case mjSENS_MAGNETOMETER: // magnetometer
mju_mulMatTVec(d->sensordata+adr, d->site_xmat+9*objid, m->opt.magnetic, 3, 3);
break;
@@ -376,7 +376,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
// velocity-dependent sensors
void mj_sensorVel(const mjModel* m, mjData* d) {
int type, objtype, objid, reftype, refid, adr, nusersensor = 0;
int objtype, objid, reftype, refid, adr, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum xvel[6];
@@ -395,7 +395,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
if (m->sensor_needstage[i] == mjSTAGE_VEL) {
// get sensor info
type = m->sensor_type[i];
mjtSensor type = m->sensor_type[i];
objtype = m->sensor_objtype[i];
objid = m->sensor_objid[i];
refid = m->sensor_refid[i];
@@ -564,7 +564,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// acceleration/force-dependent sensors
void mj_sensorAcc(const mjModel* m, mjData* d) {
int rootid, bodyid, type, objtype, objid, body1, body2, adr, nusersensor = 0;
int rootid, bodyid, objtype, objid, body1, body2, adr, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum tmp[6], conforce[6], conray[3];
mjContact* con;
@@ -584,7 +584,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
if (m->sensor_needstage[i] == mjSTAGE_ACC) {
// get sensor info
type = m->sensor_type[i];
mjtSensor type = m->sensor_type[i];
objtype = m->sensor_objtype[i];
objid = m->sensor_objid[i];
adr = m->sensor_adr[i];
@@ -803,7 +803,7 @@ void mj_energyPos(const mjModel* m, mjData* d) {
stiffness = m->jnt_stiffness[i];
padr = m->jnt_qposadr[i];
switch (m->jnt_type[i]) {
switch ((mjtJoint) m->jnt_type[i]) {
case mjJNT_FREE:
mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr);
d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif);
src/engine/engine_support.c
+2 -2
View File
@@ -1342,7 +1342,7 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
switch (m->jnt_type[j]) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
for (int i=0; i < 3; i++) {
qvel[vadr+i] = (qpos2[padr+i] - qpos1[padr+i]) / dt;
@@ -1376,7 +1376,7 @@ void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum
int padr = m->jnt_qposadr[j];
int vadr = m->jnt_dofadr[j];
switch (m->jnt_type[j]) {
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_FREE:
// position update
for (int i=0; i < 3; i++) {
src/engine/engine_util_misc.c
+3 -3
View File
@@ -423,7 +423,7 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
// all 3 semi-axes of a geom
void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) {
mjtNum* size = m->geom_size + 3*geom_id;
switch (m->geom_type[geom_id]) {
switch ((mjtGeom) m->geom_type[geom_id]) {
case mjGEOM_SPHERE:
semiaxes[0] = size[0];
semiaxes[1] = size[0];
@@ -782,7 +782,7 @@ int mju_round(mjtNum x) {
// convert type id to type name
const char* mju_type2Str(int type) {
switch (type) {
switch ((mjtObj) type) {
case mjOBJ_BODY:
return "body";
@@ -992,7 +992,7 @@ const char* mju_writeNumBytes(size_t nbytes) {
const char* mju_warningText(int warning, size_t info) {
static mjTHREADLOCAL char str[1000];
switch (warning) {
switch ((mjtWarning) warning) {
case mjWARN_INERTIA:
mjSNPRINTF(str, "Inertia matrix is too close to singular at DOF %zu. Check model.", info);
break;
src/engine/engine_vis_init.c
+1 -1
View File
@@ -354,7 +354,7 @@ float mjv_rbound(const mjvGeom* geom) {
// compute rbound according to type
const float* s = geom->size;
switch (geom->type) {
switch ((mjtMouse) geom->type) {
case mjGEOM_SPHERE:
return s[0];
src/engine/engine_vis_interact.c
+4 -4
View File
@@ -266,7 +266,7 @@ static void convert2D(mjtNum* res, int action, mjtNum dx, mjtNum dy, const mjtNu
mjtNum vec[3];
// construct 3D vector
switch (action) {
switch ((mjtMouse) action) {
case mjMOUSE_ROTATE_V:
vec[0] = dy;
vec[1] = 0;
@@ -316,7 +316,7 @@ void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
}
// process action
switch (action) {
switch ((mjtMouse) action) {
case mjMOUSE_ROTATE_V:
case mjMOUSE_ROTATE_H:
cam->azimuth -= reldx * 180.0;
@@ -384,7 +384,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
convert2D(vec, action, reldx, reldy, forward);
// process action
switch (action) {
switch ((mjtMouse) action) {
case mjMOUSE_MOVE_V:
case mjMOUSE_MOVE_H:
mju_addToScl3(pert->refpos, vec, pert->scale);
@@ -463,7 +463,7 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
mju_normalize3(roomright);
// process action
switch (action) {
switch ((mjtMouse) action) {
case mjMOUSE_ROTATE_V:
case mjMOUSE_ROTATE_H:
// construct rotation vector
src/engine/engine_vis_visualize.c
+2 -2
View File
@@ -343,7 +343,7 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size,
// set size (for XYZ scaling)
if (size) {
switch (type) {
switch ((mjtGeom) type) {
case mjGEOM_SPHERE:
geom->size[0] = (float)size[0];
geom->size[1] = (float)size[0];
@@ -869,7 +869,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
int j = m->jnt_bodyid[i];
mjtNum* from;
mjtNum to[3];
switch (m->jnt_type[i]) {
switch ((mjtJoint) m->jnt_type[i]) {
case mjJNT_FREE:
thisgeom->type = mjGEOM_BOX;
thisgeom->size[0] = thisgeom->size[1] = thisgeom->size[2] = 0.3*sz[1];