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

Allow connect constraints to be specified using sites.

Browse Source 
PiperOrigin-RevId: 668925419
Change-Id: I10cb71e6706e617c830cd90d6d0836b94a5a8cfa
This commit is contained in:
Yuval Tassa
2024-08-29 07:09:52 -07:00
committed by Copybara-Service
parent 646455406c
commit a1036b86d3
19 changed files with 325 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+30 -12
View File
@@ -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<equality-connect-body1>` and :ref:`anchor<equality-connect-anchor>` (both required) and
optionally :ref:`body2<equality-connect-body2>`. When using this specification, the constraint is assumed to be
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.
.. _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<equality-connect-site1>` description for more details.
.. _equality-weld:
doc/XMLschema.rst
+3 -1
View File
@@ -631,7 +631,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<equality-connect-name>` | :ref:`class<equality-connect-class>` | :ref:`body1<equality-connect-body1>` | :ref:`body2<equality-connect-body2>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`anchor<equality-connect-anchor>` | :ref:`active<equality-connect-active>` | :ref:`solref<equality-connect-solref>` | :ref:`solimp<equality-connect-solimp>` | |
| | | | :ref:`anchor<equality-connect-anchor>` | :ref:`site1<equality-connect-site1>` | :ref:`site2<equality-connect-site2>` | :ref:`active<equality-connect-active>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solref<equality-connect-solref>` | :ref:`solimp<equality-connect-solimp>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| equality |br| |_| |L| | | .. table:: |
doc/changelog.rst
+5 -1
View File
@@ -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-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<option-flag-nativeccd>` flag. When this flag is enabled, general convex collision
detection is handled natively, as opposed to using `libccd <https://github.com/danfis/libccd>`__. This feature is in
early stages of testing.
@@ -24,7 +28,7 @@ General
- When :ref:`attaching<meAttachment>` sub-models, :ref:`keyframes<keyframe>` 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
^^^
doc/includes/references.h
+3
View File
@@ -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;
include/mujoco/mjmodel.h
+1
View File
@@ -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)
include/mujoco/mjspec.h
+1
View File
@@ -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
include/mujoco/mjvisualize.h
+1
View File
@@ -578,6 +578,7 @@ struct mjvSceneState_ {
int* eq_type;
int* eq_obj1id;
int* eq_obj2id;
int* eq_objtype;
mjtNum* eq_data;
int* tendon_num;
include/mujoco/mjxmacro.h
+1
View File
@@ -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 ) \
introspect/structs.py
+19
View File
@@ -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(
src/engine/engine_core_constraint.c
+17 -5
View File
@@ -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]
src/engine/engine_setconst.c
+15 -5
View File
@@ -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
src/engine/engine_vis_visualize.c
+12 -5
View File
@@ -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;
src/user/user_model.cc
+1
View File
@@ -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);
src/user/user_objects.cc
+23 -24
View File
@@ -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");
}
}
src/xml/xml_native_reader.cc
+35 -6
View File
@@ -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:
src/xml/xml_native_writer.cc
+8 -3
View File
@@ -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:
test/engine/testdata/connect.xml
Vendored
+52
View File
@@ -0,0 +1,52 @@
<mujoco>
<option>
<flag contact="disable"/>
</option>
<default>
<default class="static">
<geom size=".5 .1 .5" rgba=".5 .7 .5 .3"/>
</default>
<default class="free">
<geom type="box" size=".2" fromto="0 0 0 0 -1 0" rgba=".4 .7 .6 .3"/>
</default>
</default>
<worldbody>
<geom pos="0 0 -2" type="plane" size="10 10 .01"/>
<light pos="0 0 20"/>
<body name="box1" pos="-3 0 0">
<geom type="box" class="static"/>
</body>
<body name="beam1" pos="-3 0 0">
<freejoint/>
<geom class="free"/>
</body>
<body name="box2" pos="-1 0 0">
<geom type="box" class="static"/>
</body>
<body name="beam2" pos="-1 0 0">
<freejoint/>
<geom class="free"/>
</body>
<body name="box3" pos="1 0 0">
<geom type="box" class="static"/>
<site name="box3" pos=".5 -.1 .5" type="box" size=".05 .05 .05"/>
</body>
<body name="beam3" pos="1 0 0">
<freejoint/>
<geom class="free"/>
<site name="beam3" pos=".2 -.1 .2" type="box" size=".05 .05 .05"/>
</body>
</worldbody>
<equality>
<connect body1="beam1" anchor="0 0 0"/>
<connect body1="box2" body2="beam2" anchor=".5 .1 .5"/>
<connect site1="box3" site2="beam3"/>
</equality>
</mujoco>
test/user/user_objects_test.cc
+96 -14
View File
@@ -38,6 +38,7 @@ std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
return std::vector<mjtNum>(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<char, 1024> 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<char, 1024> 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"(
<mujoco>
<worldbody>
@@ -2141,7 +2144,7 @@ TEST_F(MujocoTest, Frame) {
mj_deleteData(d);
}
TEST_F(MujocoTest, FrameTransformsLight) {
TEST_F(UserObjectsTest, FrameTransformsLight) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -2153,7 +2156,7 @@ TEST_F(MujocoTest, FrameTransformsLight) {
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
@@ -2203,10 +2206,10 @@ TEST_F(MujocoTest, RobustBVH) {
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<site name="0" size="1"/>
<body name="1" pos="0 0 1">
<freejoint/>
<geom size="1"/>
<site name="1"/>
</body>
</worldbody>
<equality>
CONNECT
</equality>
</mujoco>
)";
int pos = base.find("CONNECT");
int len = 7;
// good model using body semantic
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;
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;
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, "<connect body1='1'/>");
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, "<connect body1='1' site1='1'/>");
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
unity/Runtime/Bindings/MjBindings.cs
+2
View File
@@ -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;