Use enum instead of NaNs for alternative orientation specification.

PiperOrigin-RevId: 627413508
Change-Id: I9f1a682a1533e1a3fa3cac0a462f28ce9b0fa192

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 zaxis[3];                 // z axis (minimal rotation)
-  double euler[3];                 // euler angles
+  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

@@ -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

@@ -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

@@ -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) +
+  int numspec = (int)(elem->Attribute("quat") != 0);
-             (ReadAttr(elem, "axisangle", 4, alt.axisangle, text) ? 1 : 0) +
+  if (ReadAttr(elem, "axisangle", 4, alt.axisangle, text)) {
-             (ReadAttr(elem, "xyaxes", 6, alt.xyaxes, text) ? 1 : 0) +
+    numspec++;
-             (ReadAttr(elem, "zaxis", 3, alt.zaxis, text) ? 1 : 0) +
+    alt.type = mjORIENTATION_AXISANGLE;
-             (ReadAttr(elem, "euler", 3, alt.euler, text) ? 1 : 0);
+  }
-  if (read > 1) {
+  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

@@ -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);
     }
   }