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

Add function mjs_sensorDim to user API.

Browse Source 
Note: change to (unreleased) contact sensor mjSpec API
PiperOrigin-RevId: 788507423
Change-Id: Ie639afb7ed01dc3f7bab43a3e812e8e0b0c67d07
This commit is contained in:
Yuval Tassa
2025-07-29 10:19:28 -07:00
committed by Copybara-Service
parent a771fc6c09
commit 46dc67b7eb
10 changed files with 149 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+73
View File
@@ -26,6 +26,7 @@
#include <vector>
#include <mujoco/mujoco.h>
#include "engine/engine_support.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_cache.h"
@@ -1043,6 +1044,78 @@ void mjs_deleteUserValue(mjsElement* element, const char* key) {
// return sensor dimension
int mjs_sensorDim(const mjsSensor* sensor) {
switch (sensor->type) {
case mjSENS_TOUCH:
case mjSENS_RANGEFINDER:
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_JOINTLIMITPOS:
case mjSENS_JOINTLIMITVEL:
case mjSENS_JOINTLIMITFRC:
case mjSENS_TENDONLIMITPOS:
case mjSENS_TENDONLIMITVEL:
case mjSENS_TENDONLIMITFRC:
case mjSENS_GEOMDIST:
case mjSENS_INSIDESITE:
case mjSENS_E_POTENTIAL:
case mjSENS_E_KINETIC:
case mjSENS_CLOCK:
return 1;
case mjSENS_CAMPROJECTION:
return 2;
case mjSENS_ACCELEROMETER:
case mjSENS_VELOCIMETER:
case mjSENS_GYRO:
case mjSENS_FORCE:
case mjSENS_TORQUE:
case mjSENS_MAGNETOMETER:
case mjSENS_BALLANGVEL:
case mjSENS_FRAMEPOS:
case mjSENS_FRAMEXAXIS:
case mjSENS_FRAMEYAXIS:
case mjSENS_FRAMEZAXIS:
case mjSENS_FRAMELINVEL:
case mjSENS_FRAMEANGVEL:
case mjSENS_FRAMELINACC:
case mjSENS_FRAMEANGACC:
case mjSENS_SUBTREECOM:
case mjSENS_SUBTREELINVEL:
case mjSENS_SUBTREEANGMOM:
case mjSENS_GEOMNORMAL:
return 3;
case mjSENS_GEOMFROMTO:
return 6;
case mjSENS_BALLQUAT:
case mjSENS_FRAMEQUAT:
return 4;
case mjSENS_CONTACT:
return sensor->intprm[2] * mju_condataSize(sensor->intprm[0]);
case mjSENS_USER:
return sensor->dim;
case mjSENS_PLUGIN:
return 0; // to be filled in by plugin
}
return -1;
}
// get id
int mjs_getId(mjsElement* element) {
if (!element) {
src/user/user_api.h
+2
View File
@@ -420,6 +420,8 @@ MJAPI const void* mjs_getUserValue(mjsElement* element, const char* key);
// Delete user payload.
MJAPI void mjs_deleteUserValue(mjsElement* element, const char* key);
// Return sensor dimension.
MJAPI int mjs_sensorDim(const mjsSensor* sensor);
//---------------------------------- Initialization -----------------------------------------------
src/user/user_objects.cc
+8 -38
View File
@@ -6801,15 +6801,12 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "sensor must be attached to site");
}
// set dim and datatype
// set datatype
if (type == mjSENS_TOUCH || type == mjSENS_RANGEFINDER) {
dim = 1;
datatype = mjDATATYPE_POSITIVE;
} else if (type == mjSENS_CAMPROJECTION) {
dim = 2;
datatype = mjDATATYPE_REAL;
} else {
dim = 3;
datatype = mjDATATYPE_REAL;
}
@@ -6845,7 +6842,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
if (type == mjSENS_JOINTPOS) {
needstage = mjSTAGE_POS;
@@ -6863,7 +6859,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_ACC;
break;
@@ -6876,7 +6871,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
if (type == mjSENS_TENDONPOS) {
needstage = mjSTAGE_POS;
@@ -6894,7 +6888,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
if (type == mjSENS_ACTUATORPOS) {
needstage = mjSTAGE_POS;
@@ -6919,11 +6912,9 @@ void mjCSensor::Compile(void) {
// set
if (type == mjSENS_BALLQUAT) {
dim = 4;
datatype = mjDATATYPE_QUATERNION;
needstage = mjSTAGE_POS;
} else {
dim = 3;
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_VEL;
}
@@ -6943,7 +6934,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
if (type == mjSENS_JOINTLIMITPOS) {
needstage = mjSTAGE_POS;
@@ -6968,7 +6958,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 1;
datatype = mjDATATYPE_REAL;
if (type == mjSENS_TENDONLIMITPOS) {
needstage = mjSTAGE_POS;
@@ -6994,13 +6983,6 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "sensor must be attached to (x)body, geom, site or camera");
}
// set dim
if (type == mjSENS_FRAMEQUAT) {
dim = 4;
} else {
dim = 3;
}
// set datatype
if (type == mjSENS_FRAMEQUAT) {
datatype = mjDATATYPE_QUATERNION;
@@ -7031,7 +7013,6 @@ void mjCSensor::Compile(void) {
}
// set
dim = 3;
datatype = mjDATATYPE_REAL;
if (type == mjSENS_SUBTREECOM) {
needstage = mjSTAGE_POS;
@@ -7048,7 +7029,6 @@ void mjCSensor::Compile(void) {
if (reftype != mjOBJ_SITE) {
throw mjCError(this, "sensor must be associated with a site");
}
dim = 1;
datatype = mjDATATYPE_REAL;
needstage = mjSTAGE_POS;
break;
@@ -7076,13 +7056,10 @@ void mjCSensor::Compile(void) {
// set
needstage = mjSTAGE_POS;
if (type == mjSENS_GEOMDIST) {
dim = 1;
datatype = mjDATATYPE_POSITIVE;
} else if (type == mjSENS_GEOMNORMAL) {
dim = 3;
datatype = mjDATATYPE_AXIS;
} else {
dim = 6;
datatype = mjDATATYPE_REAL;
}
break;
@@ -7116,11 +7093,6 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "subtree2 must be a child of the world");
}
// check for non-positive dim
if (dim <= 0) {
throw mjCError(this, "dim must be positive in sensor, got %d", nullptr, dim);
}
// check for dataspec correctness
int dataspec = intprm[0];
if (dataspec <= 0) {
@@ -7136,19 +7108,17 @@ void mjCSensor::Compile(void) {
"mjNCONDATA bits", nullptr, dataspec);
}
// check for dim correctness
int size = mju_condataSize(dataspec);
if (dim % size != 0) {
throw mjCError(this, "dim %d not divisible by size %d implied by data spec (intprm[0])",
nullptr, dim, size);
}
// check for reduce correctness
int reduce = intprm[1];
if (reduce < 0 || reduce > 3) {
throw mjCError(this, "unknown reduction criterion. got %d, "
"expected one of {0, 1, 2, 3}", nullptr, reduce);
}
// check for non-positive num
if (intprm[2] <= 0) {
throw mjCError(this, "num (intprm[2]) must be positive in sensor, got %d", nullptr, dim);
}
}
needstage = mjSTAGE_ACC;
@@ -7158,7 +7128,6 @@ void mjCSensor::Compile(void) {
case mjSENS_E_POTENTIAL:
case mjSENS_E_KINETIC:
case mjSENS_CLOCK:
dim = 1;
needstage = mjSTAGE_POS;
datatype = mjDATATYPE_REAL;
break;
@@ -7179,7 +7148,6 @@ void mjCSensor::Compile(void) {
break;
case mjSENS_PLUGIN:
dim = 0; // to be filled in by the plugin later
datatype = mjDATATYPE_REAL; // no noise added to plugin sensors, this attribute is unused
if (plugin_name.empty() && plugin_instance_name.empty()) {
@@ -7204,6 +7172,8 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "invalid type in sensor '%s' (id = %d)", name.c_str(), id);
}
dim = mjs_sensorDim(this);
// check cutoff for incompatible data types
if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
(datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {
src/xml/xml_native_reader.cc
+7 -8
View File
@@ -4216,20 +4216,19 @@ void mjXReader::Sensor(XMLElement* section) {
}
sensor->intprm[0] = dataspec;
// number of contacts, sensor dim
sensor->dim = 1;
ReadAttrInt(elem, "num", &sensor->dim);
if (sensor->dim <= 0) {
throw mjXError(elem, "'num' must be positive in sensor");
}
sensor->dim *= mju_condataSize(dataspec);
// reduction type (intprm[1])
sensor->intprm[1] = 0;
if (MapValue(elem, "reduce", &n, reduce_map, reduce_sz)) {
sensor->intprm[1] = n;
}
// number of contacts (intprm[2])
sensor->intprm[2] = 1;
ReadAttrInt(elem, "num", &sensor->intprm[2]);
if (sensor->intprm[2] <= 0) {
throw mjXError(elem, "'num' must be positive in sensor");
}
// sensor type
sensor->type = mjSENS_CONTACT;
}