diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 16ab04d7..8ad6c587 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -835,7 +835,7 @@ has any effect. The settings here are global and apply to the entire model. .. _compiler-saveinertial: :at:`saveinertial`: :at-val:`[false, true], "false"` - If set to "true", the compiler will save explicit :ref:`inertial ` clauses for all bodies. + If set to "true", the compiler will save explicit :ref:`inertial ` clauses for all bodies. .. _compiler-lengthrange: @@ -4706,6 +4706,13 @@ length X, as in the clip on the right of `this example model joint damping which is integrated implicitly by the Euler method, tendon damping is not integrated implicitly, thus joint damping should be used if possible. +.. TODO(tassa): Update here once the feature is implemented. + +.. _tendon-spatial-armature: + +:at:`armature`: :at-val:`real, "0"` + Inertia associated with tendon. This feature is not yet implemented. + .. _tendon-spatial-user: :at:`user`: :at-val:`real(nuser_tendon), "0 0 ..."` @@ -4815,6 +4822,8 @@ as above. .. _tendon-fixed-damping: +.. _tendon-fixed-armature: + .. _tendon-fixed-user: .. |tendon/fixed attrib list| replace:: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 5af5fca5..56119cd7 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -627,7 +627,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`material` | :ref:`margin` | :ref:`stiffness` | :ref:`damping` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`rgba` | :ref:`user` | | | | +| | | | :ref:`armature` | :ref:`rgba` | :ref:`user` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| spatial |br| |_2| |L| | | .. table:: | @@ -661,7 +661,7 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`solimpfriction` | :ref:`frictionloss` | :ref:`springlength` | :ref:`margin` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`stiffness` | :ref:`damping` | :ref:`user` | | | +| | | | :ref:`stiffness` | :ref:`damping` | :ref:`armature` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_2| fixed |br| |_2| |L| | | .. table:: | diff --git a/doc/includes/references.h b/doc/includes/references.h index 069b895a..846f0c75 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1339,6 +1339,7 @@ struct mjModel_ { mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1) mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) mjtNum* tendon_damping; // damping coefficient (ntendon x 1) + mjtNum* tendon_armature; // inertia associated with tendon velocity (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) mjtNum* tendon_length0; // tendon length in qpos0 (ntendon x 1) @@ -2174,13 +2175,14 @@ typedef struct mjsTendon_ { // tendon specification mjsElement* element; // element type mjString* name; // name - // stiffness, damping, friction + // stiffness, damping, friction, armature double stiffness; // stiffness coefficient double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring double damping; // damping coefficient double frictionloss; // friction loss mjtNum solref_friction[mjNREF]; // solver reference: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction + double armature; // inertia associated with tendon velocity // length range int limited; // does tendon have limits (mjtLimited) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 632f6ddd..1699ca0c 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -1040,6 +1040,7 @@ struct mjModel_ { mjtNum* tendon_margin; // min distance for limit detection (ntendon x 1) mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) mjtNum* tendon_damping; // damping coefficient (ntendon x 1) + mjtNum* tendon_armature; // inertia associated with tendon velocity (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) mjtNum* tendon_length0; // tendon length in qpos0 (ntendon x 1) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 65eb55a4..42b8c03f 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -617,13 +617,14 @@ typedef struct mjsTendon_ { // tendon specification mjsElement* element; // element type mjString* name; // name - // stiffness, damping, friction + // stiffness, damping, friction, armature double stiffness; // stiffness coefficient double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring double damping; // damping coefficient double frictionloss; // friction loss mjtNum solref_friction[mjNREF]; // solver reference: tendon friction mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction + double armature; // inertia associated with tendon velocity // length range int limited; // does tendon have limits (mjtLimited) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 34a6dee4..0aaf1af6 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -484,6 +484,7 @@ X ( mjtNum, tendon_margin, ntendon, 1 ) \ XMJV( mjtNum, tendon_stiffness, ntendon, 1 ) \ XMJV( mjtNum, tendon_damping, ntendon, 1 ) \ + X ( mjtNum, tendon_armature, ntendon, 1 ) \ XMJV( mjtNum, tendon_frictionloss, ntendon, 1 ) \ XMJV( mjtNum, tendon_lengthspring, ntendon, 2 ) \ X ( mjtNum, tendon_length0, ntendon, 1 ) \ diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 515d15f4..9052043c 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -771,6 +771,7 @@ class Model(PyTreeNode): tendon_margin: min distance for limit detection (ntendon,) tendon_stiffness: stiffness coefficient (ntendon,) tendon_damping: damping coefficient (ntendon,) + tendon_armature: inertia associated with tendon velocity (ntendon,) tendon_frictionloss: loss due to friction (ntendon,) tendon_lengthspring: spring resting length range (ntendon, 2) tendon_length0: tendon length in qpos0 (ntendon,) @@ -1113,6 +1114,7 @@ class Model(PyTreeNode): tendon_margin: jax.Array tendon_stiffness: jax.Array tendon_damping: jax.Array + tendon_armature: jax.Array tendon_frictionloss: jax.Array tendon_lengthspring: jax.Array tendon_length0: jax.Array diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 73007b79..59b21fb8 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -3687,6 +3687,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='damping coefficient', array_extent=('ntendon',), ), + StructFieldDecl( + name='tendon_armature', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='inertia associated with tendon velocity', + array_extent=('ntendon',), + ), StructFieldDecl( name='tendon_frictionloss', type=PointerType( @@ -11324,6 +11332,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='solver impedance: tendon friction', ), + StructFieldDecl( + name='armature', + type=ValueType(name='double'), + doc='inertia associated with tendon velocity', + ), StructFieldDecl( name='limited', type=ValueType(name='int'), diff --git a/src/user/user_model.cc b/src/user/user_model.cc index e5650c17..e49c6112 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3390,6 +3390,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_margin[i] = (mjtNum)pte->margin; m->tendon_stiffness[i] = (mjtNum)pte->stiffness; m->tendon_damping[i] = (mjtNum)pte->damping; + m->tendon_armature[i] = (mjtNum)pte->armature; m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1]; @@ -4931,6 +4932,7 @@ bool mjCModel::CopyBack(const mjModel* m) { tendons_[i]->margin = (double)m->tendon_margin[i]; tendons_[i]->stiffness = (double)m->tendon_stiffness[i]; tendons_[i]->damping = (double)m->tendon_damping[i]; + tendons_[i]->armature = (double)m->tendon_armature[i]; tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i]; if (nuser_tendon) { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 631bb2f8..f52f8cf6 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -5616,6 +5616,12 @@ void mjCTendon::Compile(void) { // spatial path else { + if (armature < 0) { + throw mjCError(this, + "tendon '%s' (id = %d): tendon armature cannot be negative", + name.c_str(), id); + } + switch (path[i]->type) { case mjWRAP_PULLEY: // pulley should not follow other pulley @@ -5657,6 +5663,12 @@ void mjCTendon::Compile(void) { name.c_str(), id, i); } + if (armature > 0) { + throw mjCError(this, + "tendon '%s' (id = %d): geom wrapping not supported by tendon armature", + name.c_str(), id); + } + // mark geoms as non visual model->Geoms()[path[i]->obj->id]->SetNotVisual(); break; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 695ceeac..f3afd4a2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -363,18 +363,18 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"tendon", "*", "0"}, {"<"}, - {"spatial", "*", "18", "name", "class", "group", "limited", "range", + {"spatial", "*", "19", "name", "class", "group", "limited", "range", "solreflimit", "solimplimit", "solreffriction", "solimpfriction", "frictionloss", "springlength", "width", "material", - "margin", "stiffness", "damping", "rgba", "user"}, + "margin", "stiffness", "damping", "armature", "rgba", "user"}, {"<"}, {"site", "*", "1", "site"}, {"geom", "*", "2", "geom", "sidesite"}, {"pulley", "*", "1", "divisor"}, {">"}, - {"fixed", "*", "15", "name", "class", "group", "limited", "range", + {"fixed", "*", "16", "name", "class", "group", "limited", "range", "solreflimit", "solimplimit", "solreffriction", "solimpfriction", - "frictionloss", "springlength", "margin", "stiffness", "damping", "user"}, + "frictionloss", "springlength", "margin", "stiffness", "damping", "armature", "user"}, {"<"}, {"joint", "*", "2", "joint", "coef"}, {">"}, @@ -2061,6 +2061,7 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) { ReadAttr(elem, "margin", 1, &tendon->margin, text); ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text); ReadAttr(elem, "damping", 1, &tendon->damping, text); + ReadAttr(elem, "armature", 1, &tendon->armature, text); ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text); // read springlength, either one or two values; if one, copy to second value if (ReadAttr(elem, "springlength", 2, tendon->springlength, text, false, false) == 1) { @@ -3800,7 +3801,7 @@ void mjXReader::Tendon(XMLElement* section) { def = mjs_getSpecDefault(spec); } - // create equality constraint and parse + // create tendon and parse mjsTendon* tendon = mjs_addTendon(spec, def); OneTendon(elem, tendon); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 639ae081..5f2d44cf 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -729,6 +729,7 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin); WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness); WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping); + WriteAttr(elem, "armature", 1, &tendon->armature, &def->Tendon().armature); WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss); if (tendon->springlength[0] != tendon->springlength[1] || def->Tendon().springlength[0] != def->Tendon().springlength[1]) { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 7b74af4c..05a6c61c 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -15,6 +15,7 @@ // Tests for xml/xml_native_reader.cc. #include +#include #include #include #include @@ -1148,6 +1149,88 @@ TEST_F(XMLReaderTest, ParsePolycoef) { mj_deleteModel(m); } +TEST_F(XMLReaderTest, TendonArmature) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, NotNull()) << error.data(); + EXPECT_EQ(m->ntendon, 3); + EXPECT_FLOAT_EQ(m->tendon_armature[0], 1.5); + EXPECT_FLOAT_EQ(m->tendon_armature[1], 2.5); + EXPECT_FLOAT_EQ(m->tendon_armature[2], 0); + mj_deleteModel(m); +} + +TEST_F(XMLReaderTest, TendonArmatureNegative) { + 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("tendon armature cannot be negative")); +} + +TEST_F(XMLReaderTest, TendonArmatureGeomWrap) { + 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("geom wrapping not supported")); +} + // ------------------------ test frame parsing --------------------------------- TEST_F(XMLReaderTest, ParseFrame) { static constexpr char xml[] = R"( @@ -1862,7 +1945,7 @@ TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) { EXPECT_THAT(error.data(), HasSubstr("line 6")); } -TEST_F(XMLReaderTest, CameraPricipalRequiresSensorsize) { +TEST_F(XMLReaderTest, CameraPrincipalRequiresSensorsize) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 3fb3f812..f7d6ac29 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5578,6 +5578,7 @@ public unsafe struct mjModel_ { public double* tendon_margin; public double* tendon_stiffness; public double* tendon_damping; + public double* tendon_armature; public double* tendon_frictionloss; public double* tendon_lengthspring; public double* tendon_length0;