diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3896df33..b80ab60c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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(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"); }