From eb9568a48b4f6af20d92a6d85648b98345e420cd Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 27 Nov 2023 03:31:25 -0800 Subject: [PATCH] Add `frame` element to MJCF. PiperOrigin-RevId: 585598244 Change-Id: I4c06be3dd586dab5a4458f05faaf1e7f2b615458 --- doc/XMLreference.rst | 92 +++++++++++++++++++++++++++++- doc/changelog.rst | 3 +- doc/modeling.rst | 11 +++- src/user/user_model.cc | 4 +- src/user/user_objects.cc | 92 +++++++++++++++++++++++++++++- src/user/user_objects.h | 29 ++++++++++ src/user/user_util.cc | 11 +++- src/xml/xml_native_reader.cc | 29 ++++++++-- src/xml/xml_native_reader.h | 3 +- src/xml/xml_util.cc | 4 +- test/engine/testdata/frame.xml | 43 ++++++++++++++ test/user/user_objects_test.cc | 74 ++++++++++++++++++++++++ test/xml/xml_native_reader_test.cc | 33 +++++++++++ 13 files changed, 411 insertions(+), 17 deletions(-) create mode 100644 test/engine/testdata/frame.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 34a72e1c..50307135 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -103,13 +103,72 @@ In the remainder of this chapter we describe all valid MJCF elements and their a multiple contexts, in which case their meaning depends on the parent element. This is why we always show the parent as a prefix in the documentation below. +.. _meta-element: + +Meta elements +~~~~~~~~~~~~~ + +These elements are not strictly part of the low-level MJCF format definition, but rather instruct the compiler to +perform some operation on the model. A general property of meta-elements is that they disappear from the model upon +saving the XML. There are currently four meta-elements in MJCF: + +- :ref:`include` and :ref:`frame`, which are outside of the schema. +- :ref:`composite` and :ref:`flexcomp` which are part of the schema, but serve to + procedurally generate other MJCF elements. + +.. _frame: + +**frame** (R) +^^^^^^^^^^^^^ + +The frame meta-element is a pure coordinate transformation that can wrap any group of elements in the kinematic tree +(under :ref:`worldbody`). After compilation, frame elements disappear and their transformation is accumulated +in their direct children. The attributes of the frame meta-element are documented :ref:`below`. + +.. collapse:: Usage example of frame + + Loading this model and saving it: + + .. code-block:: xml + + + + + + + + + + + ... + + + + + + Results in this model: + + .. code-block:: xml + + + + + + + ... + + + + + Note that in the saved model, the frame elements have disappeared but their transformation was accumulated with those + of their child elements. .. _include: **include** (*) -~~~~~~~~~~~~~~~ +^^^^^^^^^^^^^^^ -This element does not strictly speaking belong to MJCF. Instead it is a meta-element, used to assemble multiple XML +This element does not strictly belong to MJCF. Instead it is a meta-element, used to assemble multiple XML files in a single document object model (DOM) before parsing. The included file must be a valid XML file with a unique top-level element. This top-level element is removed by the parser, and the elements below it are inserted at the location of the :el:`include` element. At least one element must be inserted as a result of this procedure. The @@ -3913,6 +3972,35 @@ Associate this flexcomp with an :ref:`engine plugin`. Either :at:`plug :at:`instance`: :at-val:`string, optional` Instance name, used for explicit plugin instantiation. + +.. _body-frame: + +:el-prefix:`body/` |-| **frame** (*) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Frames specify a coordinate transformation which is applied to all child elements. They disappear during compilation +and the transformation they encode is accumulated in their direct children. See :ref:`frame` for examples. + +.. _frame-pos: + +:at:`pos`: :at-val:`real(3), "0 0 0"` + The 3D position of the frame, in the parent coordinate system. + +.. _frame-quat: + +.. _frame-axisangle: + +.. _frame-xyaxes: + +.. _frame-zaxis: + +.. _frame-euler: + +:at:`quat`, :at:`axisangle`, :at:`xyaxes`, :at:`zaxis`, :at:`euler` + See :ref:`COrientation`. + + + .. _contact: **contact** (*) diff --git a/doc/changelog.rst b/doc/changelog.rst index 8a190164..75c4d6f7 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,10 +7,11 @@ Upcoming version (not yet released) General ^^^^^^^ - - Improved convergence of Signed Distance Function (SDF) collisions by using line search and a new objective function for the optimization. This allows to decrease the number of initial points needed for finding the contacts and is more robust for very small or large geom sizes. +- Added :ref:`frame` to MJCF, a :ref:`meta-element` which defines a pure coordinate transformation + on its direct children, without requiring a :ref:`body`. Plugins ^^^^^^^ diff --git a/doc/modeling.rst b/doc/modeling.rst index 7abdb53e..c43b9a04 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -204,16 +204,21 @@ cameras and lights. A related attribute is :ref:`compiler/angle`. It specifies whether angles in the MJCF file are expressed in degrees or radians (after compilation, angles are always expressed in radians). +Positions are specified using + +:at:`pos`: :at-val:`real(3), "0 0 0"` + Position relative to parent. + .. _COrientation: Frame orientations -~~~~~~~~~~~~~~~~~~ +^^^^^^^^^^^^^^^^^^ Several model elements have right-handed spatial frames associated with them. These are all the elements defined in the kinematic tree except for joints. A spatial frame is defined by its position and orientation. Specifying 3D positions is straightforward, but specifying 3D orientations can be challenging. This is why MJCF provides several alternative -mechanisms. No matter which mechanism the user chooses, the frame orientation is always represented as a unit quaternion -after compilation. Recall that a 3D rotation by angle :math:`a` around axis given by the unit vector :math:`(x, y, z)` +mechanisms. No matter which mechanism the user chooses, the frame orientation is always converted internally to a unit +quaternion. Recall that a 3D rotation by angle :math:`a` around axis given by the unit vector :math:`(x, y, z)` corresponds to the quaternion :math:`(\cos(a/2), \: \sin(a/2) \cdot (x, y, z))`. Also recall that every 3D orientation can be uniquely specified by a single 3D rotation by some angle around some axis. diff --git a/src/user/user_model.cc b/src/user/user_model.cc index c3b533ed..7729d9aa 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1506,7 +1506,9 @@ void mjCModel::CopyTree(mjModel* m) { if (rotfound || !IsNullPose(m->jnt_pos+3*jid, NULL) || ((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) && - ((pj->locaxis[0]!=0) + (pj->locaxis[1]!=0) + (pj->locaxis[2]!=0))>1)) { + ((mju_abs(pj->locaxis[0])>mjEPS) + + (mju_abs(pj->locaxis[1])>mjEPS) + + (mju_abs(pj->locaxis[2])>mjEPS)) > 1)) { m->body_simple[i] = 0; } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 383e4d37..4530b661 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -491,6 +491,7 @@ mjCBase::mjCBase() { xmlpos[0] = xmlpos[1] = -1; model = 0; def = 0; + frame = nullptr; // plugin variables is_plugin = false; @@ -534,6 +535,15 @@ std::string mjCBase::GetAssetContentType(std::string_view resource_name, } +void mjCBase::SetFrame(mjCFrame* _frame) { + if (!_frame) { + return; + } + frame = _frame; + frame->Compile(); +} + + //------------------ class mjCBody implementation -------------------------------------------------- // constructor @@ -574,6 +584,7 @@ mjCBody::mjCBody(mjCModel* _model) { // clear object lists bodies.clear(); geoms.clear(); + frames.clear(); joints.clear(); sites.clear(); cameras.clear(); @@ -587,6 +598,7 @@ mjCBody::~mjCBody() { // delete objects allocated here for (int i=0; ipos, frame->quat); + } + // compute local frame rel. to parent body if (id>0) { model->bodies[parentid]->MakeLocal(locpos, locquat, pos, quat); @@ -1076,6 +1103,39 @@ void mjCBody::Compile(void) { +//------------------ class mjCFrame implementation ------------------------------------------------- + +// initialize frame +mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) { + compiled = false; + model = _model; + frame = _frame ? _frame : NULL; + mju_zero3(pos); + mjuu_setvec(quat, 1, 0, 0, 0); +} + +void mjCFrame::Compile() { + if (compiled) { + return; + } + + const char* err = alt.Set(quat, 0, model->degree, model->euler); + if (err) { + throw mjCError(this, "orientation specification error '%s' in site %d", err, id); + } + + // compile parents and accumulate result + if (frame) { + frame->Compile(); + mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + } + + mjuu_normvec(quat, 4); + compiled = true; +} + + + //------------------ class mjCJoint implementation ------------------------------------------------- // initialize default joint @@ -1197,6 +1257,13 @@ int mjCJoint::Compile(void) { } } + // frame + if (frame) { + double mat[9]; + mjuu_quat2mat(mat, frame->quat); + mjuu_mulvecmat(axis, axis, mat); + } + // FREE or BALL: set axis to (0,0,1) if (type==mjJNT_FREE || type==mjJNT_BALL) { axis[0] = axis[1] = 0; @@ -1223,6 +1290,9 @@ int mjCJoint::Compile(void) { if (type!=mjJNT_FREE) { double qunit[4] = {1, 0, 0, 0}; double qloc[4]; + if (frame) { + mjuu_frameaccum(pos, qunit, frame->pos, frame->quat); + } body->MakeLocal(locpos, qloc, pos, qunit); } else { mjuu_zerovec(locpos, 3); @@ -1842,6 +1912,11 @@ void mjCGeom::Compile(void) { throw mjCError(this, "plugin '%s' does not support sign distance fields", plugin->name); } } + + // frame + if (frame) { + mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + } } @@ -1952,6 +2027,11 @@ void mjCSite::Compile(void) { } } + // frame + if (frame) { + mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + } + // normalize quaternion mjuu_normvec(quat, 4); @@ -2017,6 +2097,11 @@ void mjCCamera::Compile(void) { throw mjCError(this, "orientation specification error '%s' in camera %d", err, id); } + // frame + if (frame) { + mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + } + // normalize quaternion mjuu_normvec(quat, 4); @@ -2121,6 +2206,11 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) { void mjCLight::Compile(void) { double locquat[4], quat[4]= {1, 0, 0, 0}; + // frame + if (frame) { + mjuu_frameaccum(pos, quat, frame->pos, frame->quat); + } + // normalize direction, make sure it is not zero if (mjuu_normvec(dir, 3) bodies; // child bodies std::vector geoms; // geoms attached to this body + std::vector frames; // frames attached to this body std::vector joints; // joints allowing motion relative to parent std::vector sites; // sites attached to this body std::vector cameras; // cameras attached to this body @@ -286,6 +293,28 @@ class mjCBody : public mjCBase { +//------------------------- class mjCFrame --------------------------------------------------------- +// Describes a coordinate transformation relative to its parent + +class mjCFrame : public mjCBase { + friend class mjCBase; + friend class mjCBody; + friend class mjCModel; + + public: + double pos[3]; // frame position + double quat[4]; // frame orientation + mjCAlternative alt; // alternative orientation specification + + private: + bool compiled; // frame already compiled + + mjCFrame(mjCModel* = 0, mjCFrame* = 0); // constructor + void Compile(void); // compiler +}; + + + //------------------------- class mjCJoint --------------------------------------------------------- // Describes a motion degree of freedom of a body relative to its parent diff --git a/src/user/user_util.cc b/src/user/user_util.cc index c8a145f2..4c75ccce 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -191,9 +191,14 @@ void mjuu_mulquat(double* res, const double* qa, const double* qb) { // multiply vector by 3-by-3 matrix void mjuu_mulvecmat(double* res, const double* vec, const double* mat) { - res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2]; - res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2]; - res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]; + double tmp[3] = { + mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2], + mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2], + mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2] + }; + res[0] = tmp[0]; + res[1] = tmp[1]; + res[2] = tmp[2]; } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d2ed4b99..4121b2b9 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -872,7 +872,7 @@ void mjXReader::Parse(XMLElement* root) { for (XMLElement* section = root->FirstChildElement("worldbody"); section; section = section->NextSiblingElement("worldbody")) { - Body(section, model->GetWorld()); + Body(section, model->GetWorld(), nullptr); } for (XMLElement* section = root->FirstChildElement("contact"); section; @@ -2949,7 +2949,7 @@ void mjXReader::Asset(XMLElement* section) { // body/world section parser; recursive -void mjXReader::Body(XMLElement* section, mjCBody* pbody) { +void mjXReader::Body(XMLElement* section, mjCBody* pbody, mjCFrame* frame) { string text, name; XMLElement* elem; int n; @@ -2960,7 +2960,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { } // no attributes allowed in world body - if (pbody->id==0 && section->FirstAttribute()) { + if (pbody->id==0 && section->FirstAttribute() && !frame) { throw mjXError(section, "World body cannot have attributes"); } @@ -3000,6 +3000,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // create joint and parse mjCJoint* pjoint = pbody->AddJoint(def); OneJoint(elem, pjoint); + pjoint->SetFrame(frame); } // freejoint sub-element @@ -3011,6 +3012,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // create free joint without defaults mjCJoint* pjoint = pbody->AddJoint(NULL, true); + pjoint->SetFrame(frame); // save defaults after creation, to make sure writing is ok pjoint->def = def; @@ -3025,6 +3027,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // create geom and parse mjCGeom* pgeom = pbody->AddGeom(def); OneGeom(elem, pgeom); + pgeom->SetFrame(frame); // discard visual if (!pgeom->contype && !pgeom->conaffinity && model->discardvisual) { @@ -3038,6 +3041,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // create site and parse mjCSite* psite = pbody->AddSite(def); OneSite(elem, psite); + psite->SetFrame(frame); } // camera sub-element @@ -3045,6 +3049,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // create camera and parse mjCCamera* pcam = pbody->AddCamera(def); OneCamera(elem, pcam); + pcam->SetFrame(frame); } // light sub-element @@ -3052,6 +3057,7 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // create light and parse mjCLight* plight = pbody->AddLight(def); OneLight(elem, plight); + plight->SetFrame(frame); } // plugin sub-element @@ -3071,6 +3077,18 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { OneFlexcomp(elem, pbody); } + // frame sub-element + else if (name=="frame") { + mjCFrame* pframe = pbody->AddFrame(frame); + GetXMLPos(elem, pframe); + + ReadAttr(elem, "pos", 3, pframe->pos, text); + ReadQuat(elem, "quat", pframe->quat, text); + ReadAlternative(elem, pframe->alt); + + Body(elem, pbody, pframe); + } + // body sub-element else if (name=="body") { // read childdef @@ -3102,8 +3120,11 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) { // read userdata ReadVector(elem, "user", pchild->userdata, text); + // add frame + pchild->SetFrame(frame); + // make recursive call - Body(elem, pchild); + Body(elem, pchild, nullptr); } // no match diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index f1170dcf..45f9671e 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -42,7 +42,8 @@ class mjXReader : public mjXBase { void Visual(tinyxml2::XMLElement* section); // visual section void Statistic(tinyxml2::XMLElement* section); // statistic section void Asset(tinyxml2::XMLElement* section); // asset section - void Body(tinyxml2::XMLElement* section, mjCBody* pbody); // body/world section + void Body(tinyxml2::XMLElement* section, mjCBody* pbody, + mjCFrame* pframe); // body/world section void Contact(tinyxml2::XMLElement* section); // contact section void Deformable(tinyxml2::XMLElement* section); // deformable section void Equality(tinyxml2::XMLElement* section); // equality section diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index c3abb1f6..0d1a7f75 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -372,7 +372,9 @@ bool mjXSchema::NameMatch(XMLElement* elem, int level) { return true; } - return false; + if (level>=1 && !strcmp(elem->Value(), "frame")) { + return true; + } } // regular check diff --git a/test/engine/testdata/frame.xml b/test/engine/testdata/frame.xml new file mode 100644 index 00000000..4d2b6db1 --- /dev/null +++ b/test/engine/testdata/frame.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index d11e4f4b..63c795be 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -1398,5 +1398,79 @@ TEST_F(SpringrangeTest, InvalidRange) { EXPECT_THAT(error.data(), HasSubstr("invalid springlength in tendon")); } +// ------------- test frame ---------------------------------------------------- +TEST_F(MujocoTest, Frame) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, testing::NotNull()) << error.data(); + EXPECT_EQ(m->nbody, 4); + + // geom quat transformed to euler = 0 0 50 + EXPECT_NEAR(m->geom_quat[0], mju_cos(25. * mjPI / 180.), 1e-3); + EXPECT_NEAR(m->geom_quat[1], 0, 0); + EXPECT_NEAR(m->geom_quat[2], 0, 0); + EXPECT_NEAR(m->geom_quat[3], mju_sin(25. * mjPI / 180.), 1e-3); + + // geom transformed to frame 0 1 0, 0 0 1, 1 0 0 + EXPECT_NEAR(m->geom_quat[4], .5, 1e-6); + EXPECT_NEAR(m->geom_quat[5], .5, 1e-6); + EXPECT_NEAR(m->geom_quat[6], .5, 1e-6); + EXPECT_NEAR(m->geom_quat[7], .5, 1e-6); + + // geom pos transformed from 0 1 0 to 0 2 0 + EXPECT_EQ(m->geom_pos[6], 0); + EXPECT_EQ(m->geom_pos[7], 2); + EXPECT_EQ(m->geom_pos[8], 0); + + // body pos transformed from 1 0 0 to 1 1 0 + EXPECT_EQ(m->body_pos[6], 1); + EXPECT_EQ(m->body_pos[7], 1); + EXPECT_EQ(m->body_pos[8], 0); + + // nested geom pos not transformed + EXPECT_EQ(m->geom_pos[ 9], 0); + EXPECT_EQ(m->geom_pos[10], 0); + EXPECT_EQ(m->geom_pos[11], 1); + + // joint axis transformed to 0 -1 0 + EXPECT_NEAR(m->jnt_axis[0], 0, 1e-6); + EXPECT_NEAR(m->jnt_axis[1], -1, 1e-6); + EXPECT_NEAR(m->jnt_axis[2], 0, 1e-6); + + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index e57c6c6d..a74fabf0 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -407,6 +407,39 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) { } } +// ---------------------- test frame parsing --------------------------------- +TEST_F(XMLReaderTest, ParseFrame) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, testing::NotNull()) << error.data(); + mj_deleteModel(m); +} + // ---------------------- test camera parsing --------------------------------- TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) {