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

Use enum instead of NaNs for alternative orientation specification.

Browse Source 
PiperOrigin-RevId: 627413508
Change-Id: I9f1a682a1533e1a3fa3cac0a462f28ce9b0fa192
This commit is contained in:
Yuval Tassa
2024-04-23 09:31:27 -07:00
committed by Copybara-Service
parent f457789de1
commit cf433a4ad9
5 changed files with 46 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.h
+15 -5
View File
@@ -69,13 +69,22 @@ typedef enum _mjtLimited { // type of limit specification
} mjtLimited;
typedef enum _mjtInertiaFromGeom {
typedef enum _mjtInertiaFromGeom { // whether to infer body inertias from child geoms
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
} mjtInertiaFromGeom;
typedef enum _mjtOrientation { // type of orientation specifier
mjORIENTATION_QUAT = 0, // quaternion
mjORIENTATION_AXISANGLE, // axis and angle
mjORIENTATION_XYAXES, // x and y axes
mjORIENTATION_ZAXIS, // z axis (minimal rotation)
mjORIENTATION_EULER, // Euler angles
} mjtOrientation;
//---------------------------------- attribute structs (mjs) ---------------------------------------
typedef struct _mjElement { // element type, do not modify
@@ -140,10 +149,11 @@ typedef struct _mjSpec { // model specification
typedef struct _mjsOrientation { // alternative orientation specifiers
double axisangle[4]; // rotation axis and angle
mjtOrientation type; // active orientation specifier
double axisangle[4]; // axis and angle
double xyaxes[6]; // x and y axes
double zaxis[3]; // z axis (use minimal rotation)
double euler[3]; // euler angles
double zaxis[3]; // z axis (minimal rotation)
double euler[3]; // Euler angles
} mjsOrientation;
@@ -957,7 +967,7 @@ MJAPI void mjs_setDefault(mjElement* element, mjsDefault* def);
// Set element's enlcosing frame.
MJAPI void mjs_setFrame(mjElement* dest, mjsFrame* frame);
// Resolve alternative orientations to quat.
// Resolve alternative orientations to quat, return error if any.
MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence,
const mjsOrientation* orientation);
src/user/user_init.cc
+7 -7
View File
@@ -77,7 +77,7 @@ void mjs_defaultSpec(mjSpec& model) {
// default orientation attributes
void mjs_defaultOrientation(mjsOrientation& orient) {
orient.axisangle[0] = orient.xyaxes[0] = orient.zaxis[0] = orient.euler[0] = mjNAN;
memset(&orient, 0, sizeof(mjsOrientation));
}
@@ -89,12 +89,12 @@ void mjs_defaultBody(mjsBody& body) {
// body frame
body.pos[0] = mjNAN;
body.quat[0] = 1;
body.alt.axisangle[0] = body.alt.xyaxes[0] = body.alt.zaxis[0] = body.alt.euler[0] = mjNAN;
mjs_defaultOrientation(body.alt);
// inertial frame
body.ipos[0] = mjNAN;
body.iquat[0] = 1;
body.ialt.axisangle[0] = body.ialt.xyaxes[0] = body.ialt.zaxis[0] = body.ialt.euler[0] = mjNAN;
mjs_defaultOrientation(body.ialt);
body.fullinertia[0] = mjNAN;
}
@@ -105,7 +105,7 @@ void mjs_defaultFrame(mjsFrame& frame) {
memset(&frame, 0, sizeof(mjsFrame));
mju_zero3(frame.pos);
mjuu_setvec(frame.quat, 1, 0, 0, 0);
frame.alt.axisangle[0] = frame.alt.xyaxes[0] = frame.alt.zaxis[0] = frame.alt.euler[0] = mjNAN;
mjs_defaultOrientation(frame.alt);
}
@@ -135,7 +135,7 @@ void mjs_defaultGeom(mjsGeom& geom) {
// frame
geom.quat[0] = 1;
geom.fromto[0] = mjNAN;
geom.alt.axisangle[0] = geom.alt.xyaxes[0] = geom.alt.zaxis[0] = geom.alt.euler[0] = mjNAN;
mjs_defaultOrientation(geom.alt);
// contact-related
geom.contype = 1;
@@ -181,7 +181,7 @@ void mjs_defaultSite(mjsSite& site) {
site.quat[0] = 1;
site.size[0] = site.size[1] = site.size[2] = 0.005;
site.fromto[0] = mjNAN;
site.alt.axisangle[0] = site.alt.xyaxes[0] = site.alt.zaxis[0] = site.alt.euler[0] = mjNAN;
mjs_defaultOrientation(site.alt);
// color
site.rgba[0] = site.rgba[1] = site.rgba[2] = 0.5f;
@@ -199,7 +199,7 @@ void mjs_defaultCamera(mjsCamera& cam) {
// extrinsics
cam.quat[0] = 1;
cam.alt.axisangle[0] = cam.alt.xyaxes[0] = cam.alt.zaxis[0] = cam.alt.euler[0] = mjNAN;
mjs_defaultOrientation(cam.alt);
// intrinsics
cam.fovy = 45;
src/user/user_objects.cc
+4 -4
View File
@@ -194,7 +194,7 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
mjuu_copyvec(euler, orient.euler, 3);
// set quat using axisangle
if (mjuu_defined(axisangle[0])) {
if (orient.type == mjORIENTATION_AXISANGLE) {
// convert to radians if necessary, normalize axis
if (degree) {
axisangle[3] = axisangle[3] / 180.0 * mjPI;
@@ -212,7 +212,7 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
}
// set quat using xyaxes
if (mjuu_defined(xyaxes[0])) {
if (orient.type == mjORIENTATION_XYAXES) {
// normalize x axis
if (mjuu_normvec(xyaxes, 3)<mjEPS) {
return "xaxis too small";
@@ -239,7 +239,7 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
}
// set quat using zaxis
if (mjuu_defined(zaxis[0])) {
if (orient.type == mjORIENTATION_ZAXIS) {
if (mjuu_normvec(zaxis, 3)<mjEPS) {
return "zaxis too small";
}
@@ -248,7 +248,7 @@ const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
// handle euler
if (mjuu_defined(euler[0])) {
if (orient.type == mjORIENTATION_EULER) {
// convert to radians if necessary
if (degree) {
for (int i=0; i<3; i++) {
src/xml/xml_base.cc
+19 -7
View File
@@ -54,13 +54,25 @@ void mjXBase::SetModel(mjSpec* _model) {
// read alternative orientation specification
int mjXBase::ReadAlternative(XMLElement* elem, mjsOrientation& alt) {
string text;
int read = (int)(elem->Attribute("quat") != 0) +
(ReadAttr(elem, "axisangle", 4, alt.axisangle, text) ? 1 : 0) +
(ReadAttr(elem, "xyaxes", 6, alt.xyaxes, text) ? 1 : 0) +
(ReadAttr(elem, "zaxis", 3, alt.zaxis, text) ? 1 : 0) +
(ReadAttr(elem, "euler", 3, alt.euler, text) ? 1 : 0);
if (read > 1) {
int numspec = (int)(elem->Attribute("quat") != 0);
if (ReadAttr(elem, "axisangle", 4, alt.axisangle, text)) {
numspec++;
alt.type = mjORIENTATION_AXISANGLE;
}
if (ReadAttr(elem, "xyaxes", 6, alt.xyaxes, text)) {
numspec++;
alt.type = mjORIENTATION_XYAXES;
}
if (ReadAttr(elem, "zaxis", 3, alt.zaxis, text)) {
numspec++;
alt.type = mjORIENTATION_ZAXIS;
}
if (ReadAttr(elem, "euler", 3, alt.euler, text)) {
numspec++;
alt.type = mjORIENTATION_EULER;
}
if (numspec > 1) {
throw mjXError(elem, "multiple orientation specifiers are not allowed");
}
return read;
return numspec;
}
src/xml/xml_urdf.cc
+1
View File
@@ -635,6 +635,7 @@ void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) {
mjsOrientation alt;
mjs_defaultOrientation(alt);
if (ReadAttr(temp, "rpy", 3, alt.euler, text)) {
alt.type = mjORIENTATION_EULER;
mjs_resolveOrientation(quat, 0, "XYZ", &alt);
}
}