diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 6af8b3d9..3469f367 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -498,6 +498,14 @@ void mjs_setFrame(mjElement dest, mjsFrame* frame) { +// Resolve alternative orientations. +const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, + const mjsOrientation* orientation) { + return ResolveOrientation(quat, degree, sequence, *orientation); +} + + + // get id int mjs_getId(mjElement element) { return reinterpret_cast(element)->id; diff --git a/src/user/user_api.h b/src/user/user_api.h index b78c8495..db24d853 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -915,6 +915,10 @@ MJAPI void mjs_setDefault(mjElement element, mjmDefault* def); // Set frame. MJAPI void mjs_setFrame(mjElement dest, mjsFrame* frame); +// Resolve alternative orientations to quat. +MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, + const mjsOrientation* orientation); + // Compute quat and inertia from body->fullinertia. MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inertia[3]); @@ -924,6 +928,9 @@ MJAPI const char* mjs_setFullInertia(mjsBody* body, double quat[4], double inert // Default model attributes. MJAPI void mjs_defaultSpec(mjSpec& model); +// Default orientation attributes. +MJAPI void mjs_defaultOrientation(mjsOrientation& orient); + // Default body attributes. MJAPI void mjs_defaultBody(mjsBody& body); diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 41e031e6..3130e39c 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -73,6 +73,7 @@ mjCFlexcomp::mjCFlexcomp(void) { centered = false; mjs_defaultPlugin(plugin); + mjs_defaultOrientation(alt); plugin_name = ""; plugin_instance_name = ""; plugin.name = (mjString)&plugin_name; @@ -131,7 +132,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { } // compute orientation - const char* alterr = alt.Set(quat, model->spec.degree, model->spec.euler); + const char* alterr = mjs_resolveOrientation(quat, model->spec.degree, model->spec.euler, &alt); if (alterr) { return comperr(error, alterr, error_sz); } diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 8e6b4340..29d72301 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -79,7 +79,7 @@ class mjCFlexcomp { // pose transform relative to parent body double pos[3]; // position double quat[4]; // orientation - mjCAlternative alt; // alternative orientation + mjsOrientation alt; // alternative orientation // set by user or computed internally bool rigid; // all vertices are in parent body (all pinned) diff --git a/src/user/user_init.cc b/src/user/user_init.cc index 0e85848e..b9553933 100644 --- a/src/user/user_init.cc +++ b/src/user/user_init.cc @@ -75,6 +75,13 @@ 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; +} + + + // default body attributes void mjs_defaultBody(mjsBody& body) { memset(&body, 0, sizeof(mjsBody)); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 79008f69..6b5d41ee 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -141,17 +141,22 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos -//------------------ class mjCAlternative implementation ------------------------------------------- - -// constructor -mjCAlternative::mjCAlternative() { - axisangle[0] = xyaxes[0] = zaxis[0] = euler[0] = mjNAN; -} - +//------------------ alternative orientation implementation ---------------------------------------- // compute frame orientation given alternative specifications // used for geom, site, body and camera frames -const char* mjCAlternative::Set(double* quat, bool degree, const char* sequence) { +const char* ResolveOrientation(double* quat, bool degree, const char* sequence, + const mjsOrientation& orient) { + mjtNum axisangle[4]; + mjtNum xyaxes[6]; + mjtNum zaxis[3]; + mjtNum euler[3]; + + mjuu_copyvec(axisangle, orient.axisangle, 4); + mjuu_copyvec(xyaxes, orient.xyaxes, 6); + mjuu_copyvec(zaxis, orient.zaxis, 3); + mjuu_copyvec(euler, orient.euler, 3); + // set quat using axisangle if (mjuu_defined(axisangle[0])) { // convert to radians if necessary, normalize axis @@ -754,14 +759,6 @@ void mjCBody::CopyFromSpec() { *static_cast(this) = spec; userdata_ = spec_userdata_; userdata = (mjDoubleVec)&userdata_; - mju_copy4(alt_.axisangle, alt.axisangle); - mju_copy(alt_.xyaxes, alt.xyaxes, 6); - mju_copy3(alt_.zaxis, alt.zaxis); - mju_copy3(alt_.euler, alt.euler); - mju_copy4(ialt_.axisangle, ialt.axisangle); - mju_copy(ialt_.xyaxes, ialt.xyaxes, 6); - mju_copy3(ialt_.zaxis, ialt.zaxis); - mju_copy3(ialt_.euler, ialt.euler); plugin.active = spec.plugin.active; plugin.instance = spec.plugin.instance; plugin.name = spec.plugin.name; @@ -1195,7 +1192,7 @@ void mjCBody::Compile(void) { } // check and process orientation alternatives for body - const char* err = alt_.Set(quat, model->degree, model->euler); + const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); if (err) { throw mjCError(this, "error '%s' in frame alternative", err); } @@ -1432,10 +1429,6 @@ void mjCFrame::CopyFromSpec() { *static_cast(this) = spec; mju_copy3(pos, spec.pos); mju_copy4(quat, spec.quat); - mju_copy4(alt_.axisangle, alt.axisangle); - mju_copy(alt_.xyaxes, alt.xyaxes, 6); - mju_copy3(alt_.zaxis, alt.zaxis); - mju_copy3(alt_.euler, alt.euler); } @@ -1446,7 +1439,7 @@ void mjCFrame::Compile() { } CopyFromSpec(); - const char* err = alt_.Set(quat, model->spec.degree, model->spec.euler); + const char* err = ResolveOrientation(quat, model->spec.degree, model->spec.euler, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } @@ -1746,10 +1739,6 @@ void mjCGeom::CopyFromSpec() { hfieldname = (mjString)&hfieldname_; meshname = (mjString)&meshname_; material = (mjString)&material_; - mju_copy4(alt_.axisangle, alt.axisangle); - mju_copy(alt_.xyaxes, alt.xyaxes, 6); - mju_copy3(alt_.zaxis, alt.zaxis); - mju_copy3(alt_.euler, alt.euler); plugin.active = spec.plugin.active; plugin.instance = spec.plugin.instance; plugin.name = spec.plugin.name; @@ -2181,7 +2170,7 @@ void mjCGeom::Compile(void) { // not 'fromto': try alternative else { - const char* err = alt_.Set(quat, model->degree, model->euler); + const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in geom %d", err, id); } @@ -2355,10 +2344,6 @@ void mjCSite::CopyFromSpec() { material_ = spec_material_; userdata = (mjDoubleVec)&userdata_; material = (mjString)&material_; - mju_copy4(alt_.axisangle, alt.axisangle); - mju_copy(alt_.xyaxes, alt.xyaxes, 6); - mju_copy3(alt_.zaxis, alt.zaxis); - mju_copy3(alt_.euler, alt.euler); } @@ -2430,7 +2415,7 @@ void mjCSite::Compile(void) { // alternative orientation else { - const char* err = alt_.Set(quat, model->degree, model->euler); + const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } @@ -2514,10 +2499,6 @@ void mjCCamera::CopyFromSpec() { targetbody_ = spec_targetbody_; userdata = (mjDoubleVec)&userdata_; targetbody = (mjString)&targetbody_; - mju_copy4(alt_.axisangle, alt.axisangle); - mju_copy(alt_.xyaxes, alt.xyaxes, 6); - mju_copy3(alt_.zaxis, alt.zaxis); - mju_copy3(alt_.euler, alt.euler); } @@ -2534,7 +2515,7 @@ void mjCCamera::Compile(void) { userdata_.resize(model->nuser_cam); // process orientation specifications - const char* err = alt_.Set(quat, model->degree, model->euler); + const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in camera %d", err, id); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 0cf4fbde..fa29ea16 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -30,7 +30,6 @@ // forward declarations of all mjC/X classes class mjCError; -class mjCAlternative; class mjCBase; class mjCBody; class mjCFrame; @@ -82,13 +81,10 @@ class [[nodiscard]] mjCError { // alternative specifications of frame orientation -class mjCAlternative : public mjsOrientation { - public: - mjCAlternative(); // constuctor - const char* Set(double* quat, // set frame quat - bool degree, // angle format: degree/radian - const char* sequence); // euler sequence format: "xyz" -}; +const char* ResolveOrientation(double* quat, // set frame quat + bool degree, // angle format: degree/radian + const char* sequence, // euler sequence format: "xyz" + const mjsOrientation& orient); @@ -223,10 +219,6 @@ class mjCBase : public mjCBase_ { // Describes a rigid body class mjCBody_ : public mjCBase { - public: - mjCAlternative alt_; - mjCAlternative ialt_; - protected: // variables computed by 'Compile' and 'AddXXX' int parentid; // parent index in global array @@ -340,7 +332,6 @@ class mjCBody : public mjCBody_, private mjsBody { class mjCFrame_ : public mjCBase { protected: bool compiled; // frame already compiled - mjCAlternative alt_; }; class mjCFrame : public mjCFrame_, private mjsFrame { @@ -433,7 +424,6 @@ class mjCGeom_ : public mjCBase { bool inferinertia; // true if inertia should be computed from geom protected: - mjCAlternative alt_; bool visual_; // true: geom does not collide and is unreferenced int matid; // id of geom's material mjCMesh* mesh; // geom's mesh @@ -513,8 +503,6 @@ class mjCGeom : public mjCGeom_, private mjsGeom { class mjCSite_ : public mjCBase { protected: - mjCAlternative alt_; - // variable-size data std::string material_; std::vector userdata_; @@ -565,7 +553,6 @@ class mjCCamera_ : public mjCBase { protected: mjCBody* body; // camera's body int targetbodyid; // id of target body; -1: none - mjCAlternative alt_; std::string targetbody_; std::string spec_targetbody_; std::vector userdata_; diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index 28e76beb..0d7f6381 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -20,7 +20,6 @@ #include #include "user/user_api.h" -#include "user/user_objects.h" #include "user/user_util.h" #include "xml/xml_native_reader.h" #include "xml/xml_urdf.h" @@ -633,9 +632,10 @@ void mjXURDF::Origin(XMLElement* origin_elem, double* pos, double* quat) { ReadAttr(temp, "xyz", 3, pos, text); // orientation - mjCAlternative alt; + mjsOrientation alt; + mjs_defaultOrientation(alt); if (ReadAttr(temp, "rpy", 3, alt.euler, text)) { - alt.Set(quat, 0, "XYZ"); + mjs_resolveOrientation(quat, 0, "XYZ", &alt); } } }