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

Cleanup: extract local functions in compiler for setting datatype and needstage

Browse Source 
PiperOrigin-RevId: 792566070
Change-Id: I22d397758f5e8b35899c9461543989e352bd2b96
This commit is contained in:
Yuval Tassa
2025-08-08 04:10:14 -07:00
committed by Copybara-Service
parent 3c6e4f1ece
commit 8b5213c557
1 changed files with 130 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_objects.cc
+130 -128
View File
@@ -6816,7 +6816,127 @@ void mjCSensor::ResolveReferences(const mjCModel* m) {
suffix.clear();
}
// return sensor datatype
mjtDataType sensorDatatype(mjtSensor type) {
switch (type) {
case mjSENS_TOUCH:
case mjSENS_RANGEFINDER:
case mjSENS_INSIDESITE:
return mjDATATYPE_POSITIVE;
case mjSENS_FRAMEXAXIS:
case mjSENS_FRAMEYAXIS:
case mjSENS_FRAMEZAXIS:
case mjSENS_GEOMNORMAL:
return mjDATATYPE_AXIS;
case mjSENS_BALLQUAT:
case mjSENS_FRAMEQUAT:
return mjDATATYPE_QUATERNION;
case mjSENS_ACCELEROMETER:
case mjSENS_VELOCIMETER:
case mjSENS_GYRO:
case mjSENS_FORCE:
case mjSENS_TORQUE:
case mjSENS_MAGNETOMETER:
case mjSENS_CAMPROJECTION:
case mjSENS_JOINTPOS:
case mjSENS_JOINTVEL:
case mjSENS_TENDONPOS:
case mjSENS_TENDONVEL:
case mjSENS_ACTUATORPOS:
case mjSENS_ACTUATORVEL:
case mjSENS_ACTUATORFRC:
case mjSENS_JOINTACTFRC:
case mjSENS_TENDONACTFRC:
case mjSENS_BALLANGVEL:
case mjSENS_JOINTLIMITPOS:
case mjSENS_JOINTLIMITVEL:
case mjSENS_JOINTLIMITFRC:
case mjSENS_TENDONLIMITPOS:
case mjSENS_TENDONLIMITVEL:
case mjSENS_TENDONLIMITFRC:
case mjSENS_FRAMEPOS:
case mjSENS_FRAMELINVEL:
case mjSENS_FRAMEANGVEL:
case mjSENS_FRAMELINACC:
case mjSENS_FRAMEANGACC:
case mjSENS_SUBTREECOM:
case mjSENS_SUBTREELINVEL:
case mjSENS_SUBTREEANGMOM:
case mjSENS_GEOMDIST:
case mjSENS_GEOMFROMTO:
case mjSENS_CONTACT:
case mjSENS_TACTILE:
case mjSENS_E_POTENTIAL:
case mjSENS_E_KINETIC:
case mjSENS_CLOCK:
case mjSENS_PLUGIN:
case mjSENS_USER:
return mjDATATYPE_REAL;
}
}
// return sensor needstage
mjtStage sensorNeedstage(mjtSensor type) {
switch (type) {
case mjSENS_TOUCH:
case mjSENS_ACCELEROMETER:
case mjSENS_FORCE:
case mjSENS_TORQUE:
case mjSENS_ACTUATORFRC:
case mjSENS_JOINTACTFRC:
case mjSENS_TENDONACTFRC:
case mjSENS_JOINTLIMITFRC:
case mjSENS_TENDONLIMITFRC:
case mjSENS_FRAMELINACC:
case mjSENS_FRAMEANGACC:
case mjSENS_CONTACT:
case mjSENS_TACTILE:
return mjSTAGE_ACC;
case mjSENS_VELOCIMETER:
case mjSENS_GYRO:
case mjSENS_JOINTVEL:
case mjSENS_TENDONVEL:
case mjSENS_ACTUATORVEL:
case mjSENS_BALLANGVEL:
case mjSENS_JOINTLIMITVEL:
case mjSENS_TENDONLIMITVEL:
case mjSENS_FRAMELINVEL:
case mjSENS_FRAMEANGVEL:
case mjSENS_SUBTREELINVEL:
case mjSENS_SUBTREEANGMOM:
return mjSTAGE_VEL;
case mjSENS_MAGNETOMETER:
case mjSENS_RANGEFINDER:
case mjSENS_CAMPROJECTION:
case mjSENS_JOINTPOS:
case mjSENS_TENDONPOS:
case mjSENS_ACTUATORPOS:
case mjSENS_BALLQUAT:
case mjSENS_JOINTLIMITPOS:
case mjSENS_TENDONLIMITPOS:
case mjSENS_FRAMEPOS:
case mjSENS_FRAMEQUAT:
case mjSENS_FRAMEXAXIS:
case mjSENS_FRAMEYAXIS:
case mjSENS_FRAMEZAXIS:
case mjSENS_SUBTREECOM:
case mjSENS_INSIDESITE:
case mjSENS_GEOMDIST:
case mjSENS_GEOMNORMAL:
case mjSENS_GEOMFROMTO:
case mjSENS_E_POTENTIAL:
case mjSENS_E_KINETIC:
case mjSENS_CLOCK:
case mjSENS_PLUGIN:
case mjSENS_USER:
return mjSTAGE_POS;
}
}
// compiler
void mjCSensor::Compile(void) {
@@ -6841,6 +6961,16 @@ void mjCSensor::Compile(void) {
// Find referenced object
ResolveReferences(model);
// set datatype for non-user sensors
if (type != mjSENS_USER) {
datatype = sensorDatatype(type);
}
// set needstage for non-user and non-plugin sensors
if (type != mjSENS_USER && type != mjSENS_PLUGIN) {
needstage = sensorNeedstage(type);
}
// process according to sensor type
switch (type) {
case mjSENS_TOUCH:
@@ -6857,24 +6987,6 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "sensor must be attached to site");
}
// set datatype
if (type == mjSENS_TOUCH || type == mjSENS_RANGEFINDER) {
datatype = mjDATATYPE_POSITIVE;
} else if (type == mjSENS_CAMPROJECTION) {
datatype = mjDATATYPE_REAL;
} else {
datatype = mjDATATYPE_REAL;
}
// set stage
if (type == mjSENS_MAGNETOMETER || type == mjSENS_RANGEFINDER || type == mjSENS_CAMPROJECTION) {
needstage = mjSTAGE_POS;
} else if (type == mjSENS_GYRO || type == mjSENS_VELOCIMETER) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_ACC;
}
// check for camera resolution for camera projection sensor
if (type == mjSENS_CAMPROJECTION) {
mjCCamera* camref = (mjCCamera*)ref;
@@ -6896,16 +7008,6 @@ void mjCSensor::Compile(void) {
if (((mjCJoint*)obj)->type != mjJNT_SLIDE && ((mjCJoint*)obj)->type != mjJNT_HINGE) {
throw mjCError(this, "joint must be slide or hinge in sensor");
}
// set
datatype = mjDATATYPE_REAL;
if (type == mjSENS_JOINTPOS) {
needstage = mjSTAGE_POS;
} else if (type == mjSENS_JOINTVEL) {
needstage = mjSTAGE_VEL;
} else if (type == mjSENS_JOINTACTFRC) {
needstage = mjSTAGE_ACC;
}
break;
case mjSENS_TENDONACTFRC:
@@ -6913,10 +7015,6 @@ void mjCSensor::Compile(void) {
if (objtype != mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon");
}
// set
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_ACC;
break;
case mjSENS_TENDONPOS:
@@ -6925,14 +7023,6 @@ void mjCSensor::Compile(void) {
if (objtype != mjOBJ_TENDON) {
throw mjCError(this, "sensor must be attached to tendon");
}
// set
datatype = mjDATATYPE_REAL;
if (type == mjSENS_TENDONPOS) {
needstage = mjSTAGE_POS;
} else {
needstage = mjSTAGE_VEL;
}
break;
case mjSENS_ACTUATORPOS:
@@ -6942,16 +7032,6 @@ void mjCSensor::Compile(void) {
if (objtype != mjOBJ_ACTUATOR) {
throw mjCError(this, "sensor must be attached to actuator");
}
// set
datatype = mjDATATYPE_REAL;
if (type == mjSENS_ACTUATORPOS) {
needstage = mjSTAGE_POS;
} else if (type == mjSENS_ACTUATORVEL) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_ACC;
}
break;
case mjSENS_BALLQUAT:
@@ -6965,15 +7045,6 @@ void mjCSensor::Compile(void) {
if (((mjCJoint*)obj)->type != mjJNT_BALL) {
throw mjCError(this, "joint must be ball in sensor");
}
// set
if (type == mjSENS_BALLQUAT) {
datatype = mjDATATYPE_QUATERNION;
needstage = mjSTAGE_POS;
} else {
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_VEL;
}
break;
case mjSENS_JOINTLIMITPOS:
@@ -6988,16 +7059,6 @@ void mjCSensor::Compile(void) {
if (!((mjCJoint*)obj)->is_limited()) {
throw mjCError(this, "joint must be limited in sensor");
}
// set
datatype = mjDATATYPE_REAL;
if (type == mjSENS_JOINTLIMITPOS) {
needstage = mjSTAGE_POS;
} else if (type == mjSENS_JOINTLIMITVEL) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_ACC;
}
break;
case mjSENS_TENDONLIMITPOS:
@@ -7012,16 +7073,6 @@ void mjCSensor::Compile(void) {
if (!((mjCTendon*)obj)->is_limited()) {
throw mjCError(this, "tendon must be limited in sensor");
}
// set
datatype = mjDATATYPE_REAL;
if (type == mjSENS_TENDONLIMITPOS) {
needstage = mjSTAGE_POS;
} else if (type == mjSENS_TENDONLIMITVEL) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_ACC;
}
break;
case mjSENS_FRAMEPOS:
@@ -7038,26 +7089,6 @@ void mjCSensor::Compile(void) {
objtype != mjOBJ_GEOM && objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
throw mjCError(this, "sensor must be attached to (x)body, geom, site or camera");
}
// set datatype
if (type == mjSENS_FRAMEQUAT) {
datatype = mjDATATYPE_QUATERNION;
} else if (type == mjSENS_FRAMEXAXIS ||
type == mjSENS_FRAMEYAXIS ||
type == mjSENS_FRAMEZAXIS) {
datatype = mjDATATYPE_AXIS;
} else {
datatype = mjDATATYPE_REAL;
}
// set needstage
if (type == mjSENS_FRAMELINACC || type == mjSENS_FRAMEANGACC) {
needstage = mjSTAGE_ACC;
} else if (type == mjSENS_FRAMELINVEL || type == mjSENS_FRAMEANGVEL) {
needstage = mjSTAGE_VEL;
} else {
needstage = mjSTAGE_POS;
}
break;
case mjSENS_SUBTREECOM:
@@ -7067,14 +7098,6 @@ void mjCSensor::Compile(void) {
if (objtype != mjOBJ_BODY) {
throw mjCError(this, "sensor must be attached to body");
}
// set
datatype = mjDATATYPE_REAL;
if (type == mjSENS_SUBTREECOM) {
needstage = mjSTAGE_POS;
} else {
needstage = mjSTAGE_VEL;
}
break;
case mjSENS_INSIDESITE:
@@ -7085,8 +7108,6 @@ void mjCSensor::Compile(void) {
if (reftype != mjOBJ_SITE) {
throw mjCError(this, "sensor must be associated with a site");
}
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_POS;
break;
case mjSENS_GEOMDIST:
@@ -7108,16 +7129,6 @@ void mjCSensor::Compile(void) {
(reftype == mjOBJ_GEOM && static_cast<mjCGeom*>(ref)->Type() == mjGEOM_HFIELD)) {
throw mjCError(this, "height fields are not supported in geom distance sensors");
}
// set
needstage = mjSTAGE_POS;
if (type == mjSENS_GEOMDIST) {
datatype = mjDATATYPE_POSITIVE;
} else if (type == mjSENS_GEOMNORMAL) {
datatype = mjDATATYPE_AXIS;
} else {
datatype = mjDATATYPE_REAL;
}
break;
case mjSENS_CONTACT:
@@ -7176,16 +7187,11 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "num (intprm[2]) must be positive in sensor, got %d", nullptr, dim);
}
}
needstage = mjSTAGE_ACC;
datatype = mjDATATYPE_REAL;
break;
case mjSENS_E_POTENTIAL:
case mjSENS_E_KINETIC:
case mjSENS_CLOCK:
needstage = mjSTAGE_POS;
datatype = mjDATATYPE_REAL;
break;
case mjSENS_USER:
@@ -7204,8 +7210,6 @@ void mjCSensor::Compile(void) {
break;
case mjSENS_TACTILE:
needstage = mjSTAGE_ACC;
datatype = mjDATATYPE_REAL;
if (objtype != mjOBJ_MESH) {
throw mjCError(this, "sensor must be associated with a mesh");
}
@@ -7215,8 +7219,6 @@ void mjCSensor::Compile(void) {
break;
case mjSENS_PLUGIN:
datatype = mjDATATYPE_REAL; // no noise added to plugin sensors, this attribute is unused
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(this, "neither 'plugin' nor 'instance' is specified for sensor");
}