diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index ad60d6e3..c08aa0c3 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2296,10 +2296,11 @@ coordinates results in compiler error. See :ref:`CComposite` in the modeling gui This attribute determines the type of composite object. The remaining attributes and sub-elements are then interpreted according to the type. Default settings are also adjusted depending on the type. - The **particle** type creates a 1D, 2D or 3D grid of equally-spaced bodies. Each body has a single sphere geom and 3 - orthogonal sliding joints, allowing translation but not rotation. The geom condim and priority attributes are set to - 1 by default. This makes the spheres have frictionless contacts with all other geoms (unless the priority of some - frictional geom is higher). + The **particle** type creates a 1D, 2D or 3D grid of equally-spaced bodies. By default, each body has a single sphere + geom and 3 orthogonal sliding joints, allowing translation but not rotation. The geom condim and priority attributes + are set to 1 by default. This makes the spheres have frictionless contacts with all other geoms (unless the priority + of some frictional geom is higher). The user can replace the default sliders with multiple joints of kind="particle" + and replace the default sphere with a custom geom. The **grid** type creates a 1D or 2D grid of bodies, each having a sphere geom, a sphere site, and 3 orthogonal sliding joints by default. The :el:`pin` sub-element can be used to specify that some bodies should not have joints, @@ -2403,7 +2404,7 @@ Depending on the composite type, some joints are created automatically (e.g. the joints are optional (e.g. the stretch and twist joints in rope). This sub-element is used to specify which optional joints should be created, as well as to adjust the attributes of both automatic and optional joints. -:at:`kind`: :at-val:`[main, twist, stretch], required` +:at:`kind`: :at-val:`[main, twist, stretch, particle], required` The joint kind here is orthogonal to the joint type in the rest of MJCF. The joint kind refers to the function of the joint within the mechanism comprising the composite body, while the joint type (hinge or slide) is implied by the joint kind and composite body type. @@ -2420,6 +2421,10 @@ joints should be created, as well as to adjust the attributes of both automatic The **stretch** kind corresponds to slide joints enabling rope, loop and cloth objects to stretch. These are optional joints and are only created if this sub-element is present. This sub-element is also used to adjust the attributes of the optional stretch joints. For other composite object types this sub-element has no effect. + + The **particle** kind can only be used with the particle composite type. As opposed to all previous kinds, this kind + *replaces* the default 3 sliders with user-defined joints. User-defined joints can be repeated, for example + to create planar particles with two sliders and a hinge. :at:`solreffix`, :at:`solimpfix` These are the solref and solimp attributes used to equality-constrain the joint. Whether or not a given joint is quality-constrained depends on the joint kind and composite object type as explained above. For joints that are not @@ -2429,7 +2434,7 @@ joints should be created, as well as to adjust the attributes of both automatic .. |body/composite/joint attrib list| replace:: :at:`group`, :at:`stiffness`, :at:`damping`, :at:`armature`, :at:`limited`, :at:`range`, :at:`margin`, - :at:`solreflimit`, :at:`solimplimit`, :at:`frictionloss`, :at:`solreffriction`, :at:`solimpfriction` + :at:`solreflimit`, :at:`solimplimit`, :at:`frictionloss`, :at:`solreffriction`, :at:`solimpfriction`, :at:`type` |body/composite/joint attrib list| Same meaning as regular :ref:`joint ` attributes. diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 5fa61435..fcfbd77f 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -746,11 +746,13 @@ | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`damping` | :at:`armature` | :at:`solreffix` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`solimpfix` | :at:`limited` | :at:`range` | | +| | | | :at:`solimpfix` | :at:`type` | :at:`axis` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`margin` | :at:`solreflimit` | :at:`solimplimit` | | +| | | | :at:`limited` | :at:`range` | :at:`margin` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`frictionloss` | :at:`solreffriction` | :at:`solimpfriction` | | +| | | | :at:`solreflimit` | :at:`solimplimit` | :at:`frictionloss` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`solreffriction` | :at:`solimpfriction` | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_3|:ref:`tendon | \* | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 0e76da0a..88c84b9d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -34,6 +34,11 @@ General for dyntype :at-val:`user`, as native activation dynamics are all scalar. Added example test implementing 2nd-order activation dynamics to `engine_forward_test.cc `_. +- Improved particle :ref:`composite` type, which now permits a user-specified geometry and multiple + joints. See the two new examples: + `particle_free.xml `_ and + `particle_free2d.xml `_ + Version 2.3.0 (October 18, 2022) -------------------------------- diff --git a/model/composite/particle_free.xml b/model/composite/particle_free.xml new file mode 100644 index 00000000..d5b72ffb --- /dev/null +++ b/model/composite/particle_free.xml @@ -0,0 +1,43 @@ + + + + + + \ No newline at end of file diff --git a/model/composite/particle_free2d.xml b/model/composite/particle_free2d.xml new file mode 100644 index 00000000..c7b79152 --- /dev/null +++ b/model/composite/particle_free2d.xml @@ -0,0 +1,45 @@ + + + + + + diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index f24b9a40..aedde5c4 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -102,6 +102,23 @@ void mjCComposite::AdjustSoft(mjtNum* solref, mjtNum* solimp, int level) { +// create the array of default joint options, append new elements only for particles type +bool mjCComposite::AddDefaultJoint(char* error, int error_sz) { + for (int i=0; ipos[1] = offset[1] + spacing*(iy - 0.5*count[1]); b->pos[2] = offset[2] + spacing*(iz - 0.5*count[2]); - // add slider joints - for (int i=0; i<3; i++) { - mjCJoint* jnt = b->AddJoint(def + mjCOMPKIND_JOINT, false); - jnt->def = body->def; - jnt->type = mjJNT_SLIDE; - mjuu_setvec(jnt->pos, 0, 0, 0); - mjuu_setvec(jnt->axis, 0, 0, 0); - jnt->axis[i] = 1; + // add slider joints if none defined + if (!add[mjCOMPKIND_PARTICLE]) { + for (int i=0; i<3; i++) { + mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0], false); + jnt->def = body->def; + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[i] = 1; + } + } + + // add user-specified joints + else { + for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) { + mjCJoint* jnt = b->AddJoint(&defjnt, false); + jnt->def = body->def; + } } // add geom mjCGeom* g = b->AddGeom(def); g->def = body->def; - g->type = mjGEOM_SPHERE; } } } @@ -387,7 +415,7 @@ bool mjCComposite::MakeGrid(mjCModel* model, mjCBody* body, char* error, int err // add slider joint mjCJoint* jnt[3]; for (int i=0; i<3; i++) { - jnt[i] = b->AddJoint(def + mjCOMPKIND_JOINT); + jnt[i] = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); jnt[i]->def = body->def; mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix, iy); jnt[i]->name = txt; @@ -611,7 +639,7 @@ mjCBody* mjCComposite::AddCableBody(mjCModel* model, mjCBody* body, int ix, mjtN // add curvature joint if (!first || strcmp(initial.c_str(), "none")) { - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); jnt->def = body->def; jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL; jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping; @@ -754,7 +782,7 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i // add main joint for (int i=0; i<2; i++) { // add joint - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1); jnt->name = txt; @@ -767,7 +795,7 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i // add twist joint if (add[mjCOMPKIND_TWIST]) { // add joint - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_TWIST); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_TWIST][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1); jnt->name = txt; @@ -785,7 +813,7 @@ mjCBody* mjCComposite::AddRopeBody(mjCModel* model, mjCBody* body, int ix, int i // add stretch joint if (add[mjCOMPKIND_STRETCH]) { // add joint - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_STRETCH); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_STRETCH][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1); jnt->name = txt; @@ -1008,7 +1036,7 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body, // add main joint for (int i=0; i<2; i++) { // add joint - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_JOINT); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix1, iy1); jnt->name = txt; @@ -1026,7 +1054,7 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body, // add twist joint if (add[mjCOMPKIND_TWIST]) { - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_TWIST); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_TWIST][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJT%d_%d", prefix.c_str(), ix1, iy1); jnt->name = txt; @@ -1048,7 +1076,7 @@ mjCBody* mjCComposite::AddClothBody(mjCModel* model, mjCBody* body, // add stretch joint if (add[mjCOMPKIND_STRETCH]) { // add joint - mjCJoint* jnt = body->AddJoint(def + mjCOMPKIND_STRETCH); + mjCJoint* jnt = body->AddJoint(&defjoint[mjCOMPKIND_STRETCH][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJS%d_%d", prefix.c_str(), ix1, iy1); jnt->name = txt; @@ -1174,7 +1202,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro } // add slider joint - mjCJoint* jnt = b->AddJoint(def + mjCOMPKIND_JOINT); + mjCJoint* jnt = b->AddJoint(&defjoint[mjCOMPKIND_JOINT][0]); jnt->def = body->def; mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz); jnt->name = txt; diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 3a5cc803..af6e6807 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -15,8 +15,8 @@ #ifndef MUJOCO_SRC_USER_USER_COMPOSITE_H_ #define MUJOCO_SRC_USER_USER_COMPOSITE_H_ -#include #include +#include #include #include @@ -44,6 +44,7 @@ typedef enum _mjtCompKind { mjCOMPKIND_STRETCH, mjCOMPKIND_TENDON, mjCOMPKIND_SHEAR, + mjCOMPKIND_PARTICLE, mjNCOMPKINDS } mjtCompKind; @@ -64,6 +65,7 @@ class mjCComposite { mjCComposite(void); void SetDefault(void); + bool AddDefaultJoint(char* error = NULL, int error_sz = 0); void AdjustSoft(mjtNum* solref, mjtNum* solimp, int level); bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz); @@ -123,8 +125,9 @@ class mjCComposite { int skingroup; // skin group of the composite object // element options - bool add[mjNCOMPKINDS]; // add element - mjCDef def[mjNCOMPKINDS]; // defaults + bool add[mjNCOMPKINDS]; // add element + mjCDef def[mjNCOMPKINDS]; // default geom, site, tendon + std::unordered_map > defjoint; // default joints // computed internally int dim; // dimensionality diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c59c04f7..629fef6a 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -263,8 +263,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"<"}, {"config", "*", "2", "key", "value"}, {">"}, - {"joint", "*", "15", "kind", "group", "stiffness", "damping", "armature", - "solreffix", "solimpfix", + {"joint", "*", "17", "kind", "group", "stiffness", "damping", "armature", + "solreffix", "solimpfix", "type", "axis", "limited", "range", "margin", "solreflimit", "solimplimit", "frictionloss", "solreffriction", "solimpfriction"}, {"tendon", "*", "17", "kind", "group", "stiffness", "damping", @@ -670,10 +670,11 @@ const mjMap comp_map[mjNCOMPTYPES] = { // composite joint kind -const mjMap jkind_map[3] = { +const mjMap jkind_map[4] = { {"main", mjCOMPKIND_JOINT}, {"twist", mjCOMPKIND_TWIST}, - {"stretch", mjCOMPKIND_STRETCH} + {"stretch", mjCOMPKIND_STRETCH}, + {"particle", mjCOMPKIND_PARTICLE} }; @@ -1978,28 +1979,45 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) { while (ejnt) { // kind int kind; - MapValue(ejnt, "kind", &kind, jkind_map, 3, true); + MapValue(ejnt, "kind", &kind, jkind_map, 4, true); + + // create a new element if this kind already exists + if (comp.add[kind]) { + char error[200]; + if (!comp.AddDefaultJoint(error, 200)) { + throw mjXError(elem, error); + } + } comp.add[kind] = true; + // get element + mjCDef *el = &comp.defjoint[(mjtCompKind)kind].back(); + + // particle joint + if (MapValue(ejnt, "type", &n, joint_map, joint_sz)) { + el->joint.type = (mjtJoint)n; + } + ReadAttr(ejnt, "axis", 3, el->joint.axis, text); + // solreffix, solimpfix - ReadAttr(ejnt, "solreffix", mjNREF, comp.def[kind].equality.solref, text, false, false); - ReadAttr(ejnt, "solimpfix", mjNIMP, comp.def[kind].equality.solimp, text, false, false); + ReadAttr(ejnt, "solreffix", mjNREF, el->equality.solref, text, false, false); + ReadAttr(ejnt, "solimpfix", mjNIMP, el->equality.solimp, text, false, false); // joint attributes - MapValue(elem, "limited", &comp.def[kind].joint.limited, TFAuto_map, 3); - ReadAttrInt(ejnt, "group", &comp.def[kind].joint.group); - ReadAttr(ejnt, "solreflimit", mjNREF, comp.def[kind].joint.solref_limit, text, false, false); - ReadAttr(ejnt, "solimplimit", mjNIMP, comp.def[kind].joint.solimp_limit, text, false, false); + MapValue(elem, "limited", &el->joint.limited, TFAuto_map, 3); + ReadAttrInt(ejnt, "group", &el->joint.group); + ReadAttr(ejnt, "solreflimit", mjNREF, el->joint.solref_limit, text, false, false); + ReadAttr(ejnt, "solimplimit", mjNIMP, el->joint.solimp_limit, text, false, false); ReadAttr(ejnt, - "solreffriction", mjNREF, comp.def[kind].joint.solref_friction, text, false, false); + "solreffriction", mjNREF, el->joint.solref_friction, text, false, false); ReadAttr(ejnt, - "solimpfriction", mjNIMP, comp.def[kind].joint.solimp_friction, text, false, false); - ReadAttr(ejnt, "stiffness", 1, &comp.def[kind].joint.stiffness, text); - ReadAttr(ejnt, "range", 2, comp.def[kind].joint.range, text); - ReadAttr(ejnt, "margin", 1, &comp.def[kind].joint.margin, text); - ReadAttr(ejnt, "armature", 1, &comp.def[kind].joint.armature, text); - ReadAttr(ejnt, "damping", 1, &comp.def[kind].joint.damping, text); - ReadAttr(ejnt, "frictionloss", 1, &comp.def[kind].joint.frictionloss, text); + "solimpfriction", mjNIMP, el->joint.solimp_friction, text, false, false); + ReadAttr(ejnt, "stiffness", 1, &el->joint.stiffness, text); + ReadAttr(ejnt, "range", 2, el->joint.range, text); + ReadAttr(ejnt, "margin", 1, &el->joint.margin, text); + ReadAttr(ejnt, "armature", 1, &el->joint.armature, text); + ReadAttr(ejnt, "damping", 1, &el->joint.damping, text); + ReadAttr(ejnt, "frictionloss", 1, &el->joint.frictionloss, text); // advance ejnt = ejnt->NextSiblingElement("joint"); diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 70a7b344..d647f06a 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -20,3 +20,6 @@ target_link_libraries(user_objects_test fixture gmock) mujoco_test(user_mesh_test) target_link_libraries(user_mesh_test fixture gmock absl::str_format) + +mujoco_test(user_composite_test) +target_link_libraries(user_composite_test fixture gmock) diff --git a/test/user/user_composite_test.cc b/test/user/user_composite_test.cc new file mode 100644 index 00000000..9110b386 --- /dev/null +++ b/test/user/user_composite_test.cc @@ -0,0 +1,80 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for user/user_model.cc. + +#include +#include + +#include +#include +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::IsNull; +using ::testing::NotNull; +using ::testing::HasSubstr; +using UserCompositeTest = MujocoTest; + +// ------------- test automatic inference of nuser_xxx ------------------------- + +TEST_F(UserCompositeTest, MultipleJointsNotAllowedUnlessParticle) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("Only particles are allowed to have multiple joints")); +} + +TEST_F(UserCompositeTest, StretchAndTwistAllowed) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, NotNull()); + mj_deleteModel(m); +} + +} // namespace +} // namespace mujoco