From 60a1921b3489182b22b4024c037fb28eeacf54cd Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 16 Sep 2024 05:44:53 -0700 Subject: [PATCH] Add site semantics to weld constraint. Fixes #1896 PiperOrigin-RevId: 675112981 Change-Id: Ie4c01440d54116075df89f480591700762892cbd --- doc/XMLreference.rst | 45 ++++++++-- doc/XMLschema.rst | 4 +- doc/changelog.rst | 22 +++-- src/engine/engine_core_constraint.c | 45 +++++++--- src/engine/engine_io.c | 22 +++-- src/engine/engine_setconst.c | 45 ++++++---- src/engine/engine_vis_visualize.c | 11 ++- src/user/user_objects.cc | 5 +- src/xml/xml_native_reader.cc | 55 ++++++++++-- src/xml/xml_native_writer.cc | 13 ++- test/engine/testdata/equality_site.xml | 48 ++++++++++ test/engine/testdata/weld.xml | 11 +++ test/user/user_objects_test.cc | 119 +++++++++++++++++++++++-- 13 files changed, 368 insertions(+), 77 deletions(-) create mode 100644 test/engine/testdata/equality_site.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 0491cb47..6c0ffcf2 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4372,7 +4372,8 @@ ball joint outside the kinematic tree. Connect constraints can be specified in o 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. + configuration. An example of this specification is shown in + `this model `__. .. _equality-connect-name: @@ -4421,11 +4422,11 @@ ball joint outside the kinematic tree. Connect constraints can be specified in o :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). + specified. The (:at:`site1`, :at:`site2`) specification is a more flexible alternative to the body-based + 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: @@ -4442,7 +4443,15 @@ This element creates a weld equality constraint. It attaches two bodies to each freedom between them (softly of course, like all other constraints in MuJoCo). The two bodies are not required to be close to each other. The relative body position and orientation being enforced by the constraint solver is the one in which the model was defined. Note that two bodies can also be welded together rigidly, by defining one body as a child -of the other body, without any joint elements in the child body. +of the other body, without any joint elements in the child body. Weld constraints can be specified in one of two ways: + +- Using :ref:`body1` (and optionally :ref:`anchor`, + :ref:`relpose`, :ref:`body2`). When using this specification, the + constraint is assumed to be satisfied at the configuration in which the model is defined. +- :ref:`site1` and :ref:`site2` (both required). When using this + specification, the frames of the two sites will be aligned by the constraint, regardless of their position in the + default configuration. An example of this specification is shown in + `this model `__. .. _equality-weld-name: @@ -4459,8 +4468,9 @@ of the other body, without any joint elements in the child body. .. _equality-weld-body1: -:at:`body1`: :at-val:`string, required` - Name of the first body. +:at:`body1`: :at-val:`string, optional` + Name of the first body participating in the constraint. Either this attribute and must be specified or :at:`site1` + and :at:`site2` must be specified. .. _equality-weld-body2: @@ -4483,6 +4493,23 @@ of the other body, without any joint elements in the child body. this parameter is the same as for connect constraints, except that is relative to body2. If :at:`relpose` is specified, body1 will use the pose to compute its anchor point. +.. _equality-weld-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 body-based + 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 position and orientation in ``mjModel.site_pos`` and ``mjModel.site_quat`` at runtime will + correctly change the position and orientation of the constraint (i.e. the content of ``mjModel.eq_data`` has no + effect when this semantic is used, with the exception of :ref:`torquescale`). + +.. _equality-weld-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-torquescale: :at:`torquescale`: :at-val:`real, "1"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 475431fc..80b54614 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -642,9 +642,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`class` | :ref:`body1` | :ref:`body2` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`relpose` | :ref:`anchor` | :ref:`active` | :ref:`solref` | | +| | | | :ref:`relpose` | :ref:`anchor` | :ref:`site1` | :ref:`site2` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`solimp` | :ref:`torquescale` | | | | +| | | | :ref:`active` | :ref:`solref` | :ref:`solimp` | :ref:`torquescale` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| equality |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 483f55e2..89e95d49 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,7 +2,7 @@ Changelog ========= -Version 3.2.3 (Sep 12, 2024) +Version 3.2.3 (Sep 16, 2024) ---------------------------- General @@ -24,15 +24,23 @@ General detection is handled with a new native code path, rather than `libccd `__. This feature is in early stages of testing, but users who've experienced issues related to collsion detection are welcome to experiment with it and report any issues. -5. 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. + +.. youtube:: kcM_oauk3ZA + :align: right + :width: 240px + +5. Added a new way of defining :ref:`connect` and :ref:`weld` equality constraints, + using two sites. 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 (``mjModel.site_pos``) and orientations + ( ``mjModel.site_quat``) at runtime will correctly modify the constraint definition. This + `example model `__ using + the new semantic is shown in the video on the right. 6. Introduced **free joint alignment**, an optimization that applies to bodies with a free joint and no child bodies (simple free-floating bodies): automatically aligning the body frame with the inertial frame. This feature can be toggled individually using the :ref:`freejoint/align` attribute or globally using the compiler - :ref:`alignfree` attribute. The alignment diagonalizes the related 6x6 inertia sub-matrix, leading - to both faster simulation and more stable simulation of free bodies. + :ref:`alignfree` attribute. The alignment diagonalizes the related 6x6 inertia sub-matrix, + leading to both faster simulation and more stable simulation of free bodies. While this optimization is a strict improvement, it changes the semantics of the joint's degrees-of-freedom. Therefore, ``qpos`` and ``qvel`` values saved in older versions (for example, in :ref:`keyframes`) will diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index ab583a6e..27845307 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -542,11 +542,22 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_WELD: // fix relative position and orientation - // find global points - for (int j=0; j < 2; j++) { - mjtNum* anchor = data + 3*(1-j); - mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor); - 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++) { + mjtNum* anchor = data + 3*(1-j); + mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor); + 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 @@ -556,7 +567,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjtNum torquescale = data[10]; // compute error Jacobian (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, jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv); @@ -564,11 +575,23 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy(jac[0], jacdif, 3*NV); mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV); - // compute orientation error: neg(q1) * q0 * relpose (axis components only) - mjtNum* relpose = data+6; - mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose - mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1) - mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*relpose + // orientation, body semantic + if (m->eq_objtype[i] == mjOBJ_BODY) { + // compute orientation error: neg(q1) * q0 * relpose (axis components only) + mjtNum* relpose = data+6; + mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose + mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1) + } + + // orientation, site semantic + else { + mjtNum quat_site1[4]; + mju_mulQuat(quat, d->xquat+4*body_id[0], m->site_quat+4*id[0]); + mju_mulQuat(quat_site1, d->xquat+4*body_id[1], m->site_quat+4*id[1]); + mju_negQuat(quat1, quat_site1); + } + + mju_mulQuat(quat2, quat1, quat); mju_scl3(cpos+3, quat2+1, torquescale); // scale axis components by torquescale // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index b737300e..157a4455 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -2239,6 +2239,7 @@ const char* mj_validateReferences(const mjModel* m) { for (int i=0; i < m->neq; i++) { int obj1id = m->eq_obj1id[i]; int obj2id = m->eq_obj2id[i]; + int objtype = m->eq_objtype[i]; switch ((mjtEq) m->eq_type[i]) { case mjEQ_JOINT: if (obj1id >= m->njnt || obj1id < 0) { @@ -2262,11 +2263,22 @@ const char* mj_validateReferences(const mjModel* m) { case mjEQ_WELD: case mjEQ_CONNECT: - if (obj1id >= m->nbody || obj1id < 0) { - return "Invalid model: eq_obj1id out of bounds."; - } - if (obj2id >= m->nbody || obj2id < 0) { - return "Invalid model: eq_obj2id out of bounds."; + if (objtype == mjOBJ_BODY) { + if (obj1id >= m->nbody || obj1id < 0) { + return "Invalid model: eq_obj1id out of bounds."; + } + if (obj2id >= m->nbody || obj2id < 0) { + return "Invalid model: eq_obj2id out of bounds."; + } + } else if (objtype == mjOBJ_SITE) { + if (obj1id >= m->nsite || obj1id < 0) { + return "Invalid model: eq_obj1id out of bounds."; + } + if (obj2id >= m->nsite || obj2id < 0) { + return "Invalid model: eq_obj2id out of bounds."; + } + } else { + return "Invalid model: eq_objtype is not body or site."; } break; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 5815fea8..64d35ff8 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -307,26 +307,33 @@ static void set0(mjModel* m, mjData* d) { // weld constraint else if (m->eq_type[i] == mjEQ_WELD) { - // skip if user has set any quaternion data - if (m->eq_data[mjNEQDATA*i+6] || - m->eq_data[mjNEQDATA*i+7] || - m->eq_data[mjNEQDATA*i+8] || - m->eq_data[mjNEQDATA*i+9]) { - // normalize quaternion just in case - mju_normalize4(m->eq_data+mjNEQDATA*i+6); - continue; + switch ((mjtObj) m->eq_objtype[i]) { + case mjOBJ_BODY: { + // skip if user has set any quaternion data + if (!mju_isZero(m->eq_data + mjNEQDATA*i + 6, 4)) { + // normalize quaternion just in case + mju_normalize4(m->eq_data+mjNEQDATA*i+6); + continue; + } + + // anchor position is in body2 local frame + mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0); + + // data[3-5] = anchor position in body1 local frame + mju_subFrom3(pos, d->xpos+3*id1); + mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos); + + // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame + mju_negQuat(quat, d->xquat+4*id1); + mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2); + break; + } + case mjOBJ_SITE: { + break; + } + default: + mjERROR("invalid objtype in weld constraint %d", i); } - - // anchor position is in body2 local frame - mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0); - - // data[3-5] = anchor position in body1 local frame - mju_subFrom3(pos, d->xpos+3*id1); - mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos); - - // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame - mju_negQuat(quat, d->xquat+4*id1); - mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index a09e76cb..94ffaaf5 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -2026,29 +2026,34 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int is_connect = m->eq_type[i] == mjEQ_CONNECT; if (d->eq_active[i] && (is_connect || is_weld)) { // compute endpoints in global coordinates + mjtNum *xmat_j, *xmat_k; int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; - if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) { + if (m->eq_objtype[i] == mjOBJ_SITE) { mju_copy3(vec, d->site_xpos+3*j); mju_copy3(end, d->site_xpos+3*k); + xmat_j = d->site_xmat+9*j; + xmat_k = d->site_xmat+9*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); + xmat_j = d->xmat+9*j; + xmat_k = d->xmat+9*k; } // construct geom sz[0] = scl * m->vis.scale.constraint; START - mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect); + mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, xmat_j, m->vis.rgba.connect); if (vopt->label == mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } FINISH START - mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint); + mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, xmat_k, m->vis.rgba.constraint); if (vopt->label == mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 6fdb782d..441e0c12 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4920,7 +4920,10 @@ void mjCEquality::ResolveReferences(const mjCModel* m) { // determine object type if (type==mjEQ_WELD) { - object_type = mjOBJ_BODY; + if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) { + throw mjCError(this, "weld constraint supports only sites and bodies"); + } + object_type = objtype; } else if (type==mjEQ_CONNECT) { if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) { throw mjCError(this, "connect constraint supports only sites and bodies"); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index a5833cdb..d3544d2d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -365,8 +365,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"<"}, {"connect", "*", "10", "name", "class", "body1", "body2", "anchor", "site1", "site2", "active", "solref", "solimp"}, - {"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor", - "active", "solref", "solimp", "torquescale"}, + {"weld", "*", "12", "name", "class", "body1", "body2", "relpose", "anchor", + "site1", "site2", "active", "solref", "solimp", "torquescale"}, {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef", "active", "solref", "solimp"}, {"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef", @@ -1934,13 +1934,50 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { } break; - case mjEQ_WELD: - ReadAttrTxt(elem, "body1", name1, true); - ReadAttrTxt(elem, "body2", name2); - ReadAttr(elem, "relpose", 7, equality->data+3, text); - ReadAttr(elem, "torquescale", 1, equality->data+10, text); - if (!ReadAttr(elem, "anchor", 3, equality->data, text)) { - mjuu_zerovec(equality->data, 3); + case mjEQ_WELD: { + 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 has_relpose = ReadAttr(elem, "relpose", 7, equality->data+3, 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 || + has_relpose; + + 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(); + + if (site_semantic == body_semantic) { + throw mjXError( + elem, + "either body1 must be defined and optionally {body2, anchor, relpose}," + " or 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; + if (!has_anchor) { + mjuu_zerovec(equality->data, 3); + } + } else { + name1 = maybe_site1.value(); + name2 = maybe_site2.value(); + equality->objtype = mjOBJ_SITE; + } + + ReadAttr(elem, "torquescale", 1, equality->data+10, text); } break; diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 7de1a05b..023d10e6 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -631,11 +631,16 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe break; case mjEQ_WELD: - 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); + WriteAttr(elem, "relpose", 7, equality->data+3); + } else { + WriteAttrTxt(elem, "site1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "site2", mjs_getString(equality->name2)); + } WriteAttr(elem, "torquescale", 1, equality->data+10); - WriteAttr(elem, "relpose", 7, equality->data+3); break; case mjEQ_JOINT: diff --git a/test/engine/testdata/equality_site.xml b/test/engine/testdata/equality_site.xml new file mode 100644 index 00000000..7efdb36b --- /dev/null +++ b/test/engine/testdata/equality_site.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/weld.xml b/test/engine/testdata/weld.xml index 0d2860c3..5d1b1f69 100644 --- a/test/engine/testdata/weld.xml +++ b/test/engine/testdata/weld.xml @@ -47,6 +47,16 @@ + + + + + + + + + + @@ -54,5 +64,6 @@ + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 7d86394e..30576c3b 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -2328,14 +2328,14 @@ TEST_F(UserObjectsTest, BadConnect) { string xml = base.replace(pos, len, ""); char error[1024]; mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - EXPECT_THAT(m, NotNull()) << error; + ASSERT_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; + ASSERT_THAT(m, NotNull()) << error; EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0)); mj_deleteModel(m); @@ -2345,7 +2345,7 @@ TEST_F(UserObjectsTest, BadConnect) { // bad model (missing anchor) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - EXPECT_THAT(m, IsNull()); + ASSERT_THAT(m, IsNull()); EXPECT_THAT(error, HasSubstr(error_missing)); char error_mixed[] = "body and site semantics cannot be mixed" @@ -2354,13 +2354,13 @@ TEST_F(UserObjectsTest, BadConnect) { // bad model (mixing body and site) xml = base.replace(pos, len, ""); m = LoadModelFromString(xml.c_str(), error, sizeof(error)); - EXPECT_THAT(m, IsNull()); + ASSERT_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; + ASSERT_THAT(s, NotNull()) << error; // add a connect but don't set objtype mjsEquality* equality = mjs_addEquality(s, nullptr); @@ -2370,14 +2370,119 @@ TEST_F(UserObjectsTest, BadConnect) { // expect compilation to fail m = mj_compile(s, nullptr); - EXPECT_THAT(m, IsNull()); + ASSERT_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); + ASSERT_THAT(m, NotNull()) << mjs_getError(s); + mj_deleteModel(m); + mj_deleteSpec(s); +} + +TEST_F(UserObjectsTest, BadWeld) { + string base = R"( + + + + + + + + + + + WELD + + + )"; + int pos = base.find("WELD"); + int len = 4; + + // good model using body semantic + string xml = base.replace(pos, len, ""); + char error[1024]; + mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + EXPECT_THAT(AsVector(m->eq_data, 10), + ElementsAre(0, 0, 1, 0, 0, 0, 1, 0, 0, 0)); + mj_deleteModel(m); + + // good model using site semantic + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + EXPECT_THAT(AsVector(m->eq_data, 10), + ElementsAre(0, 1, 0, 0, 0, 0, 0, 0, 0, 0)); + mj_deleteModel(m); + + char error_mixed[] = + "body and site semantics cannot be mixed" + "\nElement 'weld', line 12"; + + // bad model (mixing body and site) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_mixed)); + + // bad model (mixing site and anchor) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_mixed)); + + // bad model (mixing site and relpose) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_mixed)); + + // bad model (body and site semantics are valid, but both are specified) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_mixed)); + + char error_underspecified[] = + "either body1 must be defined and optionally {body2, anchor, " + "relpose}, or site1 and site2 must be defined\nElement 'weld', line 12"; + + // bad model (underspecified body semantics) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_underspecified)); + + // bad model (underspecified site semantics) + xml = base.replace(pos, len, ""); + m = LoadModelFromString(xml.c_str(), error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(error, HasSubstr(error_underspecified)); + + // load spec with no constraints + xml = base.erase(pos, len); + mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(s, NotNull()) << error; + + // add a weld but don't set objtype + mjsEquality* equality = mjs_addEquality(s, nullptr); + equality->type = mjEQ_WELD; + mjs_setString(equality->name1, "0"); + mjs_setString(equality->name2, "1"); + + // expect compilation to fail + m = mj_compile(s, nullptr); + ASSERT_THAT(m, IsNull()); + EXPECT_THAT(mjs_getError(s), + HasSubstr("weld constraint supports only sites and bodies")); + + // set objtype, expect compilation to succeed + equality->objtype = mjOBJ_SITE; + m = mj_compile(s, nullptr); + ASSERT_THAT(m, NotNull()) << mjs_getError(s); mj_deleteModel(m); mj_deleteSpec(s); }