From a1036b86d3c94887e523dd68c5a05251ed18573b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 29 Aug 2024 07:09:52 -0700 Subject: [PATCH] Allow connect constraints to be specified using sites. PiperOrigin-RevId: 668925419 Change-Id: I10cb71e6706e617c830cd90d6d0836b94a5a8cfa --- doc/XMLreference.rst | 42 +++++++--- doc/XMLschema.rst | 4 +- doc/changelog.rst | 6 +- doc/includes/references.h | 3 + include/mujoco/mjmodel.h | 1 + include/mujoco/mjspec.h | 1 + include/mujoco/mjvisualize.h | 1 + include/mujoco/mjxmacro.h | 1 + introspect/structs.py | 19 +++++ src/engine/engine_core_constraint.c | 22 ++++-- src/engine/engine_setconst.c | 20 +++-- src/engine/engine_vis_visualize.c | 17 +++-- src/user/user_model.cc | 1 + src/user/user_objects.cc | 47 ++++++------ src/xml/xml_native_reader.cc | 41 ++++++++-- src/xml/xml_native_writer.cc | 11 ++- test/engine/testdata/connect.xml | 52 +++++++++++++ test/user/user_objects_test.cc | 110 +++++++++++++++++++++++---- unity/Runtime/Bindings/MjBindings.cs | 2 + 19 files changed, 325 insertions(+), 76 deletions(-) create mode 100644 test/engine/testdata/connect.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index bf4ec37d..120684d8 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4347,9 +4347,15 @@ all equality constraint types, thus we document them only once, under the :ref:` :el-prefix:`equality/` |-| **connect** (*) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates an equality constraint that connects two bodies at a point. The point is not necessarily within the -geoms volumes of either body. This constraint can be used to define ball joints outside the kinematic tree. +This element creates an equality constraint that connects two bodies at a point. The constraint effectively defines a +ball joint outside the kinematic tree. Connect constraints can be specified in one of two ways +- Using :ref:`body1` and :ref:`anchor` (both required) and + optionally :ref:`body2`. When using this specification, the constraint is assumed to be + satisfied in the configuration in which the model is defined. +- :ref:`site1` and :ref:`site2` (both required). When using this + specification, the two sites will be pulled together by the constraint, regardless of their position in the default + configuration. .. _equality-connect-name: @@ -4377,8 +4383,9 @@ geoms volumes of either body. This constraint can be used to define ball joints .. _equality-connect-body1: -:at:`body1`: :at-val:`string, required` - Name of the first body participating in the constraint. +:at:`body1`: :at-val:`string, optional` + Name of the first body participating in the constraint. Either this attribute and :at:`anchor` must be specified, or + :at:`site1` and :at:`site2` must be specified. .. _equality-connect-body2: @@ -4388,15 +4395,26 @@ geoms volumes of either body. This constraint can be used to define ball joints .. _equality-connect-anchor: -:at:`anchor`: :at-val:`real(3), required` - Coordinates of the 3D anchor point where the two bodies are connected. In the compiled mjModel the anchor is stored - twice, relative to the local frame of each body. At runtime this yields two global points computed by forward - kinematics; the constraint solver pushes these points towards each other. In the MJCF model however only one point is - given. We assume that the equality constraint is exactly satisfied in the configuration in which the model is defined - (this applies to all other constraint types as well). The compiler uses the single anchor specified in the MJCF model - to compute the two body-relative anchor points in mjModel. Specified relative to the local coordinate frame of - the *first* body. +:at:`anchor`: :at-val:`real(3), optional` + Coordinates of the 3D anchor point where the two bodies are connected, in the local coordinate frame of :at:`body1`. + The constraint is assumed to be satisfied in the configuration in which the model is defined, which lets the compiler + compute the associated anchor point for :at:`body2`. +.. _equality-connect-site1: + +:at:`site1`: :at-val:`string, optional` + Name of a site belonging to the first body participating in the constraint. When specified, :at:`site2` must also be + specified. The (:at:`site1`, :at:`site2`) specification is a more flexible alternative to the (:at:`body1`, + :at:`anchor`) specification, and is different in two ways. First, the sites are not required to overlap at the + default configuration; if they do not overlap then the sites will "snap together" at the beginning of the + simulation. Second, changing the site positions in ``mjModel.site_pos`` at runtime will correctly change the position + of the constraint (i.e. the content of ``mjModel.eq_data`` has no effect when this semantic is used). + +.. _equality-connect-site2: + +:at:`site2`: :at-val:`string, optional` + Name of a site belonging to the second body participating in the constraint. When specified, :at:`site1` must also be + specified. See the :ref:`site1` description for more details. .. _equality-weld: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 2cb92df3..098363ac 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -631,7 +631,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`class` | :ref:`body1` | :ref:`body2` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`anchor` | :ref:`active` | :ref:`solref` | :ref:`solimp` | | +| | | | :ref:`anchor` | :ref:`site1` | :ref:`site2` | :ref:`active` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`solref` | :ref:`solimp` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| equality |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 3fafab30..439c42ec 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -16,6 +16,10 @@ General the :ref:`mjOption` struct. This is because the new convex collision detection pipeline (see below) does not use the MPR algorithm. The semantics of these options remain identical. +- Added a new way of defining :ref:`connect` equality constraints, using two sites rather than bodies. + The new semantic is useful when the assumption that the constraint is satisfied in the base configuration does not + hold. In this case the sites will "snap together" at the beginning of the simulation. Additionally, changing the site + positions in ``mjModel.site_pos`` at runtime can be used to modify the constraint. - Added the :ref:`nativeccd` flag. When this flag is enabled, general convex collision detection is handled natively, as opposed to using `libccd `__. This feature is in early stages of testing. @@ -24,7 +28,7 @@ General - When :ref:`attaching` sub-models, :ref:`keyframes` will now be correctly merged into the parent model, but only on the first attachment. - Added the :ref:`mjtSameFrame` enum which contains the possible frame alignments of bodies and their children. These - alignments are used as shortcuts in :ref:`mj_kinematics`. + alignments are used for computation shortcuts in :ref:`mj_kinematics`. MJX ^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index 4ecd4b74..8f55f852 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1269,6 +1269,7 @@ struct mjModel_ { int* eq_type; // constraint type (mjtEq) (neq x 1) int* eq_obj1id; // id of object 1 (neq x 1) int* eq_obj2id; // id of object 2 (neq x 1) + int* eq_objtype; // type of both objects (mjtObj) (neq x 1) mjtByte* eq_active0; // initial enable/disable constraint state (neq x 1) mjtNum* eq_solref; // constraint solver reference (neq x mjNREF) mjtNum* eq_solimp; // constraint solver impedance (neq x mjNIMP) @@ -2090,6 +2091,7 @@ typedef struct mjsEquality_ { // equality specification mjtByte active; // is equality initially active mjString* name1; // name of object 1 mjString* name2; // name of object 2 + mjtObj objtype; // type of both objects mjtNum solref[mjNREF]; // solver reference mjtNum solimp[mjNIMP]; // solver impedance mjString* info; // message appended to errors @@ -2991,6 +2993,7 @@ struct mjvSceneState_ { int* eq_type; int* eq_obj1id; int* eq_obj2id; + int* eq_objtype; mjtNum* eq_data; int* tendon_num; diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 3c536103..cdb6b7bb 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -985,6 +985,7 @@ struct mjModel_ { int* eq_type; // constraint type (mjtEq) (neq x 1) int* eq_obj1id; // id of object 1 (neq x 1) int* eq_obj2id; // id of object 2 (neq x 1) + int* eq_objtype; // type of both objects (mjtObj) (neq x 1) mjtByte* eq_active0; // initial enable/disable constraint state (neq x 1) mjtNum* eq_solref; // constraint solver reference (neq x mjNREF) mjtNum* eq_solimp; // constraint solver impedance (neq x mjNIMP) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 7d76a46e..292c8b54 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -578,6 +578,7 @@ typedef struct mjsEquality_ { // equality specification mjtByte active; // is equality initially active mjString* name1; // name of object 1 mjString* name2; // name of object 2 + mjtObj objtype; // type of both objects mjtNum solref[mjNREF]; // solver reference mjtNum solimp[mjNIMP]; // solver impedance mjString* info; // message appended to errors diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index 36ae5596..c5261c6a 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -578,6 +578,7 @@ struct mjvSceneState_ { int* eq_type; int* eq_obj1id; int* eq_obj2id; + int* eq_objtype; mjtNum* eq_data; int* tendon_num; diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 37dfeb4a..0aa22805 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -438,6 +438,7 @@ XMJV( int, eq_type, neq, 1 ) \ XMJV( int, eq_obj1id, neq, 1 ) \ XMJV( int, eq_obj2id, neq, 1 ) \ + XMJV( int, eq_objtype, neq, 1 ) \ X ( mjtByte, eq_active0, neq, 1 ) \ X ( mjtNum, eq_solref, neq, mjNREF ) \ X ( mjtNum, eq_solimp, neq, mjNIMP ) \ diff --git a/introspect/structs.py b/introspect/structs.py index fe789863..be3e6ad7 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -3088,6 +3088,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='id of object 2 (neq x 1)', ), + StructFieldDecl( + name='eq_objtype', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='type of both objects (mjtObj) (neq x 1)', + ), StructFieldDecl( name='eq_active0', type=PointerType( @@ -7217,6 +7224,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='', ), + StructFieldDecl( + name='eq_objtype', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), StructFieldDecl( name='eq_data', type=PointerType( @@ -10305,6 +10319,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name of object 2', ), + StructFieldDecl( + name='objtype', + type=ValueType(name='mjtObj'), + doc='type of both objects', + ), StructFieldDecl( name='solref', type=ArrayType( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b07cc0d2..74f60634 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -505,21 +505,33 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { size = 0; NV = 0; NV2 = 0; + int body_id[2]; // process according to type switch ((mjtEq) m->eq_type[i]) { case mjEQ_CONNECT: // connect bodies with ball joint - // find global points - for (int j=0; j < 2; j++) { - mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j); - mju_addTo3(pos[j], d->xpos + 3*id[j]); + // find global points, body semantic + if (m->eq_objtype[i] == mjOBJ_BODY) { + for (int j=0; j < 2; j++) { + mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j); + mju_addTo3(pos[j], d->xpos + 3*id[j]); + body_id[j] = id[j]; + } + } + + // find global points, site semantic + else { + for (int j=0; j < 2; j++) { + mju_copy3(pos[j], d->site_xpos + 3*id[j]); + body_id[j] = m->site_bodyid[id[j]]; + } } // compute position error mju_sub3(cpos, pos[0], pos[1]); // compute Jacobian difference (opposite of contact: 0 - 1) - NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0], + NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0], jac[1], jac[0], jacdif, NULL, NULL, NULL); // copy difference into jac[0] diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 92d47ae2..8bb19739 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -286,12 +286,22 @@ static void set0(mjModel* m, mjData* d) { // connect constraint if (m->eq_type[i] == mjEQ_CONNECT) { - // pos = anchor position in global frame - mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0); + switch ((mjtObj) m->eq_objtype[i]) { + case mjOBJ_BODY: + // pos = anchor position in global frame + mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0); - // data[3-5] = anchor position in body2 local frame - mju_subFrom3(pos, d->xpos+3*id2); - mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos); + // data[3-5] = anchor position in body2 local frame + mju_subFrom3(pos, d->xpos+3*id2); + mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos); + break; + case mjOBJ_SITE: + // site-based connect, eq_data is unused + mju_zero(m->eq_data+mjNEQDATA*i, mjNEQDATA); + break; + default: + mjERROR("invalid objtype in connect constraint %d", i); + } } // weld constraint diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 1210e696..8155225e 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -2021,13 +2021,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) { // connect or weld for (int i=0; i < m->neq; i++) { - if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) { + int is_weld = m->eq_type[i] == mjEQ_WELD; + int is_connect = m->eq_type[i] == mjEQ_CONNECT; + if (d->eq_active[i] && (is_connect || is_weld)) { // compute endpoints in global coordinates int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; - mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD)); - mju_addTo3(vec, d->xpos+3*j); - mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT)); - mju_addTo3(end, d->xpos+3*k); + if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) { + mju_copy3(vec, d->site_xpos+3*j); + mju_copy3(end, d->site_xpos+3*k); + } else { + mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*is_weld); + mju_addTo3(vec, d->xpos+3*j); + mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*is_connect); + mju_addTo3(end, d->xpos+3*k); + } // construct geom sz[0] = scl * m->vis.scale.constraint; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 646ecfe7..40f0d9bd 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2727,6 +2727,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->eq_type[i] = peq->type; m->eq_obj1id[i] = peq->obj1id; m->eq_obj2id[i] = peq->obj2id; + m->eq_objtype[i] = peq->objtype; m->eq_active0[i] = peq->active; mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 2fe975b4..956f7a03 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4793,65 +4793,64 @@ void mjCEquality::CopyFromSpec() { void mjCEquality::ResolveReferences(const mjCModel* m) { - mjtObj objtype; + mjtObj object_type; mjCBase *px1, *px2; mjtJoint jt1, jt2; // determine object type - if (type==mjEQ_CONNECT || type==mjEQ_WELD) { - objtype = mjOBJ_BODY; + if (type==mjEQ_WELD) { + object_type = mjOBJ_BODY; + } else if (type==mjEQ_CONNECT) { + if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) { + throw mjCError(this, "connect constraint supports only sites and bodies"); + } + object_type = objtype; } else if (type==mjEQ_JOINT) { - objtype = mjOBJ_JOINT; + object_type = mjOBJ_JOINT; } else if (type==mjEQ_TENDON) { - objtype = mjOBJ_TENDON; + object_type = mjOBJ_TENDON; } else if (type==mjEQ_FLEX) { - objtype = mjOBJ_FLEX; + object_type = mjOBJ_FLEX; } else { throw mjCError(this, "invalid type in equality constraint"); } // find object 1, get id - px1 = m->FindObject(objtype, name1_); + px1 = m->FindObject(object_type, name1_); if (!px1) { - throw mjCError(this, "unknown element '%s' in equality constraint %d", name1_.c_str(), id); + throw mjCError(this, "unknown element '%s' in equality constraint", name1_.c_str()); } obj1id = px1->id; // find object 2, get id if (!name2_.empty()) { - px2 = m->FindObject(objtype, name2_); + px2 = m->FindObject(object_type, name2_); if (!px2) { throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id); } obj2id = px2->id; - } - - // object 2 unspecified: set to -1 - else { - if (objtype==mjOBJ_GEOM) { - throw mjCError(this, "both geom are required in equality constraint"); - } else { - obj2id = -1; - px2 = 0; - } + } else { + // object 2 unspecified: set to -1 + obj2id = -1; + px2 = nullptr; } // set missing body = world - if (objtype==mjOBJ_BODY && obj2id==-1) { + if (object_type == mjOBJ_BODY && obj2id == -1) { obj2id = 0; } // make sure the two objects are different - if (obj1id==obj2id) { + if (obj1id == obj2id) { throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1_.c_str(), id); } // make sure joints are scalar - if (type==mjEQ_JOINT) { + if (type == mjEQ_JOINT) { jt1 = ((mjCJoint*)px1)->type; jt2 = (px2 ? ((mjCJoint*)px2)->type : mjJNT_HINGE); - if ((jt1!=mjJNT_HINGE && jt1!=mjJNT_SLIDE) || - (jt2!=mjJNT_HINGE && jt2!=mjJNT_SLIDE)) { + if ((jt1 != mjJNT_HINGE && jt1 != mjJNT_SLIDE) || + (jt2 != mjJNT_HINGE && jt2 != mjJNT_SLIDE)) { throw mjCError(this, "only HINGE and SLIDE joint allowed in constraint"); } } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index a6097ceb..99ee8cf7 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -363,8 +363,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"equality", "*", "0"}, {"<"}, - {"connect", "*", "8", "name", "class", "body1", "body2", "anchor", - "active", "solref", "solimp"}, + {"connect", "*", "10", "name", "class", "body1", "body2", "anchor", + "site1", "site2", "active", "solref", "solimp"}, {"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor", "active", "solref", "solimp", "torquescale"}, {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef", @@ -1896,10 +1896,39 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { } switch (equality->type) { - case mjEQ_CONNECT: - ReadAttrTxt(elem, "body1", name1, true); - ReadAttrTxt(elem, "body2", name2); - ReadAttr(elem, "anchor", 3, equality->data, text, true); + case mjEQ_CONNECT: { + auto maybe_site1 = ReadAttrStr(elem, "site1"); + auto maybe_site2 = ReadAttrStr(elem, "site2"); + auto maybe_body1 = ReadAttrStr(elem, "body1"); + auto maybe_body2 = ReadAttrStr(elem, "body2"); + bool has_anchor = ReadAttr(elem, "anchor", 3, equality->data, text); + + bool maybe_site = maybe_site1.has_value() || maybe_site2.has_value(); + bool maybe_body = maybe_body1.has_value() || maybe_body2.has_value() || has_anchor; + + if (maybe_site && maybe_body) { + throw mjXError(elem, "body and site semantics cannot be mixed"); + } + + bool site_semantic = maybe_site1.has_value() && maybe_site2.has_value(); + bool body_semantic = maybe_body1.has_value() && has_anchor; + if (site_semantic == body_semantic) { + throw mjXError(elem, "either both body1 and anchor must be defined," + " or both site1 and site2 must be defined"); + } + + if (body_semantic) { + name1 = maybe_body1.value(); + if (maybe_body2.has_value()) { + name2 = maybe_body2.value(); + } + equality->objtype = mjOBJ_BODY; + } else { + name1 = maybe_site1.value(); + name2 = maybe_site2.value(); + equality->objtype = mjOBJ_SITE; + } + } break; case mjEQ_WELD: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 52ee844c..d572fba4 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -620,9 +620,14 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe switch (equality->type) { case mjEQ_CONNECT: - WriteAttrTxt(elem, "body1", mjs_getString(equality->name1)); - WriteAttrTxt(elem, "body2", mjs_getString(equality->name2)); - WriteAttr(elem, "anchor", 3, equality->data); + if (equality->objtype == mjOBJ_BODY) { + WriteAttrTxt(elem, "body1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "body2", mjs_getString(equality->name2)); + WriteAttr(elem, "anchor", 3, equality->data); + } else { + WriteAttrTxt(elem, "site1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "site2", mjs_getString(equality->name2)); + } break; case mjEQ_WELD: diff --git a/test/engine/testdata/connect.xml b/test/engine/testdata/connect.xml new file mode 100644 index 00000000..012784d1 --- /dev/null +++ b/test/engine/testdata/connect.xml @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 6cd6fabf..653a1100 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -38,6 +38,7 @@ std::vector AsVector(const mjtNum* array, int n) { return std::vector(array, array + n); } +using std::string; using ::testing::ElementsAre; using ::testing::HasSubstr; using ::testing::IsNull; @@ -392,7 +393,7 @@ using KeyframeTest = MujocoTest; constexpr char kKeyframePath[] = "user/testdata/keyframe.xml"; TEST_F(KeyframeTest, CheckValues) { - const std::string xml_path = GetTestDataFilePath(kKeyframePath); + const string xml_path = GetTestDataFilePath(kKeyframePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); ASSERT_THAT(model, NotNull()); EXPECT_EQ(model->nkey, 7); @@ -410,7 +411,7 @@ TEST_F(KeyframeTest, CheckValues) { } TEST_F(KeyframeTest, ResetDataKeyframe) { - const std::string xml_path = GetTestDataFilePath(kKeyframePath); + const string xml_path = GetTestDataFilePath(kKeyframePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); @@ -442,7 +443,7 @@ TEST_F(KeyframeTest, ResetDataKeyframe) { } TEST_F(KeyframeTest, ResetDataKeyframeAcceptsNegativeKeyframe) { - const std::string xml_path = GetTestDataFilePath(kKeyframePath); + const string xml_path = GetTestDataFilePath(kKeyframePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); @@ -650,7 +651,7 @@ static constexpr int kSphereBodyId = 1, kCylinderBodyId = 2, kCapsuleBodyId = 3, kCapsuleGeomId = 2; TEST_F(MjCGeomTest, CapsuleMass) { - const std::string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); + const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); // Mass of capsule should equal mass of cylinder + mass of sphere. mjtNum sphere_cylinder_mass = @@ -661,7 +662,7 @@ TEST_F(MjCGeomTest, CapsuleMass) { } TEST_F(MjCGeomTest, CapsuleInertiaZ) { - const std::string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); + const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); // z-inertia of capsule should equal sphere + cylinder z-inertia. mjtNum sphere_cylinder_z_inertia = @@ -673,7 +674,7 @@ TEST_F(MjCGeomTest, CapsuleInertiaZ) { } TEST_F(MjCGeomTest, CapsuleInertiaX) { - const std::string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); + const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); // The CoM of a solid hemisphere is 3/8*radius away from from the disk. @@ -1168,7 +1169,7 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) { using MjCHFieldTest = MujocoTest; TEST_F(MjCHFieldTest, PngMap) { - const std::string xml_path = + const string xml_path = GetTestDataFilePath("user/testdata/hfield_png.xml"); std::array error; mjModel* model = @@ -1853,7 +1854,7 @@ constexpr char kKeyAutoLimits[] = "user/testdata/auto_limits.xml"; // check joint limit values when automatically inferred based on range TEST_F(LimitedTest, JointLimited) { - const std::string path = GetTestDataFilePath(kKeyAutoLimits); + const string path = GetTestDataFilePath(kKeyAutoLimits); std::array err; mjModel* model = mj_loadXML(path.c_str(), nullptr, err.data(), err.size()); ASSERT_THAT(model, NotNull()) << err.data(); @@ -2046,8 +2047,10 @@ TEST_F(SpringrangeTest, InvalidRange) { EXPECT_THAT(error.data(), HasSubstr("line 9")); } +using UserObjectsTest = MujocoTest; + // ------------- test frame ---------------------------------------------------- -TEST_F(MujocoTest, Frame) { +TEST_F(UserObjectsTest, Frame) { static constexpr char xml[] = R"( @@ -2141,7 +2144,7 @@ TEST_F(MujocoTest, Frame) { mj_deleteData(d); } -TEST_F(MujocoTest, FrameTransformsLight) { +TEST_F(UserObjectsTest, FrameTransformsLight) { static constexpr char xml[] = R"( @@ -2153,7 +2156,7 @@ TEST_F(MujocoTest, FrameTransformsLight) { )"; std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(m, testing::NotNull()) << error.data(); + EXPECT_THAT(m, NotNull()) << error.data(); EXPECT_EQ(m->nlight, 1); constexpr mjtNum eps = 1e-14; @@ -2170,7 +2173,7 @@ TEST_F(MujocoTest, FrameTransformsLight) { // ------------- test bvh ------------------------------------------------------ -TEST_F(MujocoTest, RobustBVH) { +TEST_F(UserObjectsTest, RobustBVH) { static constexpr char xml1[] = R"( @@ -2203,10 +2206,10 @@ TEST_F(MujocoTest, RobustBVH) { std::array error; mjModel* m1 = LoadModelFromString(xml1, error.data(), error.size()); - EXPECT_THAT(m1, testing::NotNull()) << error.data(); + EXPECT_THAT(m1, NotNull()) << error.data(); mjModel* m2 = LoadModelFromString(xml2, error.data(), error.size()); - EXPECT_THAT(m2, testing::NotNull()) << error.data(); + EXPECT_THAT(m2, NotNull()) << error.data(); EXPECT_EQ(m1->nbvh, m2->nbvh); for (int i = 0; i < m1->nbvh; i++) { @@ -2217,5 +2220,84 @@ TEST_F(MujocoTest, RobustBVH) { mj_deleteModel(m2); } +// ------------- test equality compilation ------------------------------------- + +TEST_F(UserObjectsTest, BadConnect) { + string base = R"( + + + + + + + + + + + CONNECT + + + )"; + int pos = base.find("CONNECT"); + int len = 7; + + // good model using body semantic + string xml = base.replace(pos, len, ""); + char error[1024]; + mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + EXPECT_THAT(m, NotNull()) << error; + EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 1, 0, 0, 2)); + mj_deleteModel(m); + + // good model using site semantic + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + EXPECT_THAT(m, NotNull()) << error; + EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0)); + mj_deleteModel(m); + + char error_missing[] = "either both body1 and anchor must be defined," + " or both site1 and site2 must be defined\nElement 'connect', line 12"; + + // bad model (missing anchor) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + EXPECT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_missing)); + + char error_mixed[] = "body and site semantics cannot be mixed" + "\nElement 'connect', line 12"; + + // bad model (mixing body and site) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + EXPECT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_mixed)); + + // load spec with no constraints + xml = base.erase(pos, len); + mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error)); + EXPECT_THAT(s, NotNull()) << error; + + // add a connect but don't set objtype + mjsEquality* equality = mjs_addEquality(s, nullptr); + equality->type = mjEQ_CONNECT; + mjs_setString(equality->name1, "0"); + mjs_setString(equality->name2, "1"); + + // expect compilation to fail + m = mj_compile(s, nullptr); + EXPECT_THAT(m, IsNull()); + EXPECT_THAT(mjs_getError(s), + HasSubstr("connect constraint supports only sites and bodies")); + + // set objtype, expect compilation to succeed + equality->objtype = mjOBJ_SITE; + m = mj_compile(s, nullptr); + EXPECT_THAT(m, NotNull()) << mjs_getError(s); + mj_deleteModel(m); + mj_deleteSpec(s); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 02747279..176f0307 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5500,6 +5500,7 @@ public unsafe struct mjModel_ { public int* eq_type; public int* eq_obj1id; public int* eq_obj2id; + public int* eq_objtype; public byte* eq_active0; public double* eq_solref; public double* eq_solimp; @@ -6300,6 +6301,7 @@ public unsafe struct model { public int* eq_type; public int* eq_obj1id; public int* eq_obj2id; + public int* eq_objtype; public double* eq_data; public int* tendon_num; public int* tendon_matid;