chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add site semantics to weld constraint. Fixes #1896

Browse Source 
PiperOrigin-RevId: 675112981
Change-Id: Ie4c01440d54116075df89f480591700762892cbd
This commit is contained in:
Taylor Howell
2024-09-16 05:44:53 -07:00
committed by Copybara-Service
parent 2da15cf137
commit 60a1921b34
13 changed files with 368 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+36 -9
View File
@@ -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<equality-connect-site1>` and :ref:`site2<equality-connect-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 <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/equality_site.xml>`__.
.. _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<equality-weld-body1>` (and optionally :ref:`anchor<equality-weld-anchor>`,
:ref:`relpose<equality-weld-relpose>`, :ref:`body2<equality-weld-body2>`). When using this specification, the
constraint is assumed to be satisfied at the configuration in which the model is defined.
- :ref:`site1<equality-weld-site1>` and :ref:`site2<equality-weld-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 <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/equality_site.xml>`__.
.. _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-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<equality-weld-site1>` description for more details.
.. _equality-weld-torquescale:
:at:`torquescale`: :at-val:`real, "1"`
doc/XMLschema.rst
+2 -2
View File
@@ -642,9 +642,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<equality-weld-name>` | :ref:`class<equality-weld-class>` | :ref:`body1<equality-weld-body1>` | :ref:`body2<equality-weld-body2>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`relpose<equality-weld-relpose>` | :ref:`anchor<equality-weld-anchor>` | :ref:`active<equality-weld-active>` | :ref:`solref<equality-weld-solref>` | |
| | | | :ref:`relpose<equality-weld-relpose>` | :ref:`anchor<equality-weld-anchor>` | :ref:`site1<equality-weld-site1>` | :ref:`site2<equality-weld-site2>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimp<equality-weld-solimp>` | :ref:`torquescale<equality-weld-torquescale>` | | | |
| | | | :ref:`active<equality-weld-active>` | :ref:`solref<equality-weld-solref>` | :ref:`solimp<equality-weld-solimp>` | :ref:`torquescale<equality-weld-torquescale>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| equality |br| |_| |L| | | .. table:: |
doc/changelog.rst
+15 -7
View File
@@ -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 <https://github.com/danfis/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-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<equality-connect>` and :ref:`weld<equality-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 <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/equality_site.xml>`__ 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<body-freejoint-align>` attribute or globally using the compiler
:ref:`alignfree<compiler-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<compiler-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<keyframe>`) will
src/engine/engine_core_constraint.c
+34 -11
View File
@@ -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
src/engine/engine_io.c
+17 -5
View File
@@ -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;
src/engine/engine_setconst.c
+26 -19
View File
@@ -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);
}
}
src/engine/engine_vis_visualize.c
+8 -3
View File
@@ -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);
}
src/user/user_objects.cc
+4 -1
View File
@@ -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");
src/xml/xml_native_reader.cc
+46 -9
View File
@@ -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;
src/xml/xml_native_writer.cc
+9 -4
View File
@@ -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:
test/engine/testdata/equality_site.xml
Vendored
+48
View File
@@ -0,0 +1,48 @@
<mujoco model="Connect and Weld defined with sites">
<option viscosity="1">
<flag contact="disable"/>
</option>
<visual>
<scale framewidth="0.05" framelength="0.4"/>
</visual>
<default>
<default class="free">
<geom type="box" size=".2" fromto=".2 .2 0 .2 .2 -2" rgba=".4 .7 .6 1"/>
</default>
<site rgba=".0 .7 0 1"/>
</default>
<worldbody>
<geom pos="0 0 -2" type="plane" size="10 10 .01"/>
<light pos="0 0 20"/>
<site name="a1" pos="-2 0 0" euler="-90 0 0"/>
<body pos="-2 1 0">
<freejoint/>
<geom class="free"/>
<site name="a2"/>
</body>
<site name="b1" euler="-90 0 0"/>
<body pos="0 1 0">
<freejoint/>
<geom class="free"/>
<site name="b2"/>
</body>
<site name="c1" pos="2 0 0" euler="-90 0 0"/>
<body pos="2 1 0">
<freejoint/>
<geom class="free"/>
<site name="c2"/>
</body>
</worldbody>
<equality>
<connect name="connect" site1="a1" site2="a2"/>
<weld name="weak weld" site1="b1" site2="b2" torquescale="0.2"/>
<weld name="weld" site1="c1" site2="c2"/>
</equality>
</mujoco>
test/engine/testdata/weld.xml
Vendored
+11
View File
@@ -47,6 +47,16 @@
<freejoint/>
<geom class="free"/>
</body>
<body name="box5" pos="5 0 0">
<geom type="box" class="static"/>
<site name="box5"/>
</body>
<body name="beam5" pos="5 0 0">
<freejoint/>
<geom class="free"/>
<site name="beam5" pos="0 -1 0" euler="45 20 5"/>
</body>
</worldbody>
<equality>
@@ -54,5 +64,6 @@
<weld name="anchor no torques" body1="box2" body2="beam2" torquescale="0" anchor="0 -2 0"/>
<weld name="relpose" body1="box3" body2="beam3" relpose="0 0 0 1 -.3 0 0"/>
<weld name="relpose+anchor" body1="box4" body2="beam4" relpose="0 0 0 1 -.3 0 0" anchor="0 0 -1"/>
<weld name="site" site1="box5" site2="beam5"/>
</equality>
</mujoco>
test/user/user_objects_test.cc
+112 -7
View File
@@ -2328,14 +2328,14 @@ TEST_F(UserObjectsTest, BadConnect) {
string xml = base.replace(pos, len, "<connect body1='1' anchor='0 0 1'/>");
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, "<connect site1='0' site2='1'/>");
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, "<connect body1='1'/>");
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, "<connect body1='1' site1='1'/>");
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"(
<mujoco>
<worldbody>
<site name="0" size="1"/>
<body name="1" pos="0 0 1">
<freejoint/>
<geom size="1"/>
<site name="1"/>
</body>
</worldbody>
<equality>
WELD
</equality>
</mujoco>
)";
int pos = base.find("WELD");
int len = 4;
// good model using body semantic
string xml = base.replace(pos, len, "<weld body1='1' anchor='0 0 1' torquescale='2'/>");
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, "<weld site1='0' site2='1' torquescale='2'/>");
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, "<weld body1='1' site1='1'/>");
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, "<weld anchor='0 0 1' site1='1'/>");
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, "<weld relpose='0 0 0 1 0 0 0' site1='1'/>");
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, "<weld body1='1' site1='1' site2='1'/>");
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, "<weld anchor='0 0 1'/>");
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, "<weld site2='1'/>");
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);
}