diff --git a/src/user/user_api.h b/src/user/user_api.h index 261a882e..1ab01b47 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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); diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 4fc27110..73ab11b4 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -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; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 99c2ec10..309e7c79 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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)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; } diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 4c1b624b..30739848 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -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); } }