diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 0d438a3a..dc0d4f32 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -1665,7 +1665,7 @@ mjModel mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) mjtNum* tendon_damping; // damping coefficient (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) - mjtNum* tendon_lengthspring; // tendon length in qpos_spring (ntendon x 1) + mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) mjtNum* tendon_length0; // tendon length in qpos0 (ntendon x 1) mjtNum* tendon_invweight0; // inv. weight in qpos0 (ntendon x 1) mjtNum* tendon_user; // user data (ntendon x nuser_tendon) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 68dfc8bc..d2b7a205 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2788,9 +2788,16 @@ the obstacle geom. :at:`rgba`: :at-val:`real(4), "0.5 0.5 0.5 1"` Color and transparency of the tendon. When this value is different from the internal default, it overrides the corresponding material properties. -:at:`springlength`: :at-val:`real, "-1"` - The length of the tendon corresponding to the spring resting position. If this value is negative, the tendon resting - length is determined from the model reference configuration in qpos0. +:at:`springlength`: :at-val:`real(2), "-1 -1"` + Spring resting position, can take either one or two values. If one value is given, it corresponds to the length of + the tendon at rest. If it is ``-1``, the tendon resting length is determined from the model reference configuration + in ``mjModel.qpos0``. |br| Note that the default value of ``-1``, which invokes the automatic length computation, + was designed with :ref:`spatial` tendons in mind, which can only have nonegative length. In order to + set the :at:`springlength` of a :ref:`fixed` tendon to ``-1``, use a nearby value like ``-0.99999``. + |br| If two non-decreasing values are given, they define a `dead-band `_ + range. If the tendon length is between the two values, the force is 0. If it is outside this range, the force behaves + like a regular spring, with the rest-point corresponding to the nearest :at:`springlength` value. A deadband can be + used to define tendons whose limits are enforced by springs rather than constraints. :at:`stiffness`: :at-val:`real, "0"` Stiffness coefficient. A positive value generates a spring force (linear in position) acting along the tendon. :at:`damping`: :at-val:`real, "0"` diff --git a/doc/changelog.rst b/doc/changelog.rst index c31f9333..401b655c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,8 +9,20 @@ Upcoming version (not yet released) General ^^^^^^^ -- Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions. +- The tendon :at:`springlength` attribute can now take two values. Given two non-decreasing values, `springlength` + specifies a `deadband `_ range for spring stiffness. If the tendon length is + between the two values, the force is 0. If length is outside this range, the force behaves like a regular spring, with + the spring resting length corresponding to the nearest :at:`springlength` value. This can be used to create tendons + whose limits are enforced by springs rather than constraints, which are cheaper and easier to analyse. See + `tendon_springlength.xml `_ + example model. + + .. attention:: + This is a minor breaking API change. ``mjModel.tendon_lengthspring`` now has size ``ntendon x 2`` rather than + ``ntendon x 1``. + - Removed the requirement that stateless actuators come before stateful actuators. +- Added :ref:`mju_fill`, :ref:`mju_symmetrize` and :ref:`mju_eye` utility functions. Version 2.3.0 (October 18, 2022) -------------------------------- diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 6db1abe6..8e5a40be 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -832,7 +832,7 @@ struct mjModel_ { mjtNum* tendon_stiffness; // stiffness coefficient (ntendon x 1) mjtNum* tendon_damping; // damping coefficient (ntendon x 1) mjtNum* tendon_frictionloss; // loss due to friction (ntendon x 1) - mjtNum* tendon_lengthspring; // tendon length in qpos_spring (ntendon x 1) + mjtNum* tendon_lengthspring; // spring resting length range (ntendon x 2) mjtNum* tendon_length0; // tendon length in qpos0 (ntendon x 1) mjtNum* tendon_invweight0; // inv. weight in qpos0 (ntendon x 1) mjtNum* tendon_user; // user data (ntendon x nuser_tendon) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 00c8589d..a667aea7 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -341,7 +341,7 @@ X( mjtNum, tendon_stiffness, ntendon, 1 ) \ X( mjtNum, tendon_damping, ntendon, 1 ) \ X( mjtNum, tendon_frictionloss, ntendon, 1 ) \ - X( mjtNum, tendon_lengthspring, ntendon, 1 ) \ + X( mjtNum, tendon_lengthspring, ntendon, 2 ) \ X( mjtNum, tendon_length0, ntendon, 1 ) \ X( mjtNum, tendon_invweight0, ntendon, 1 ) \ X( mjtNum, tendon_user, ntendon, MJ_M(nuser_tendon) ) \ diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 1cb6b7e7..e5040fe7 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1383,9 +1383,20 @@ void mj_passive(const mjModel* m, mjData* d) { stiffness = m->tendon_stiffness[i]; damping = m->tendon_damping[i]; - // compute spring-damper linear force along tendon - frc = -stiffness * (d->ten_length[i] - m->tendon_lengthspring[i]) - -damping * d->ten_velocity[i]; + // compute spring force along tendon + mjtNum length = d->ten_length[i]; + mjtNum lower = m->tendon_lengthspring[2*i]; + mjtNum upper = m->tendon_lengthspring[2*i+1]; + if (length > upper) { + frc = stiffness * (upper - length); + } else if (length < lower) { + frc = stiffness * (lower - length); + } else { + frc = 0; + } + + // compute damper linear force along tendon + frc -= damping * d->ten_velocity[i]; // transform to joint torque, add to qfrc_passive: dense or sparse if (issparse) { diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index feb3b3fe..c2b29734 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -825,9 +825,19 @@ void mj_energyPos(const mjModel* m, mjData* d) { if (!mjDISABLED(mjDSBL_PASSIVE)) { for (int i=0; intendon; i++) { stiffness = m->tendon_stiffness[i]; + mjtNum length = d->ten_length[i]; + mjtNum displacement = 0; - d->energy[0] += 0.5*stiffness*(d->ten_length[i] - m->tendon_lengthspring[i])* - (d->ten_length[i] - m->tendon_lengthspring[i]); + // compute spring displacement + mjtNum lower = m->tendon_lengthspring[2*i]; + mjtNum upper = m->tendon_lengthspring[2*i+1]; + if (length > upper) { + displacement = upper - length; + } else if (length < lower) { + displacement = lower - length; + } + + d->energy[0] += 0.5*stiffness*displacement*displacement; } } } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 882fc92b..92514410 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -368,10 +368,11 @@ static void setSpring(mjModel* m, mjData* d) { mj_tendon(m, d); mj_transmission(m, d); - // copy if model spring length is negative + // copy if model spring length is -1 for (int i=0; intendon; i++) { - if (m->tendon_lengthspring[i]<0) { - m->tendon_lengthspring[i] = d->ten_length[i]; + if (m->tendon_lengthspring[2*i] == -1 && m->tendon_lengthspring[2*i+1] == -1) { + // explicit springlength unused, set equal to ten_length + m->tendon_lengthspring[2*i] = m->tendon_lengthspring[2*i+1] = d->ten_length[i]; } } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 975fd9d5..a22ace9e 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1350,14 +1350,24 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if (vopt->flags[mjVIS_TENDON] && (category & catmask)) { for (int i=0; intendon; i++) { if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) { + // stiff tendon has a deadband spring + int limitedspring = + m->tendon_stiffness[i] > 0 && // positive stiffness + m->tendon_lengthspring[2*i] == 0 && // range lower-bound is 0 + m->tendon_lengthspring[2*i+1] > 0; // range upper-bound is positive + + // non-stiff tendon has a length constraint + int limitedconstraint = + m->tendon_stiffness[i] == 0 && // zero stiffness + m->tendon_limited[i] == 1 && // limited length range + m->tendon_range[2*i] == 0; // range lower-bound is 0 + // conditions for drawing a catenary int draw_catenary = !mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero m->tendon_num[i] == 2 && // only two sites on the tendon - m->tendon_limited[i] == 1 && // limited length range - m->tendon_range[2*i] == 0 && // range lower-bound is 0 - m->tendon_stiffness[i] == 0 && // no stiffness + (limitedspring || limitedconstraint) && // either spring or constraint length limits m->tendon_damping[i] == 0 && // no damping m->tendon_frictionloss[i] == 0; // no frictionloss @@ -1400,7 +1410,12 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mju_copy3(x1, d->wrap_xpos + 3*d->ten_wrapadr[i] + 3); // length of the tendon - mjtNum length = m->tendon_range[2*i+1]; + mjtNum length; + if (limitedconstraint) { + length = m->tendon_range[2*i+1]; + } else { + length = m->tendon_lengthspring[2*i+1]; + } // points along catenary path int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index a63bae95..f82c298b 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1801,7 +1801,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_stiffness[i] = (mjtNum)pte->stiffness; m->tendon_damping[i] = (mjtNum)pte->damping; m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; - m->tendon_lengthspring[i] = (mjtNum)pte->springlength; + m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; + m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1]; copyvec(m->tendon_user+nuser_tendon*i, pte->userdata.data(), nuser_tendon); copyvec(m->tendon_rgba+4*i, pte->rgba, 4); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 22313ec3..ef964a3e 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3041,7 +3041,7 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { stiffness = 0; damping = 0; frictionloss = 0; - springlength = -1; + springlength[0] = springlength[1] = -1; rgba[0] = rgba[1] = rgba[2] = 0.5f; rgba[3] = 1.0f; userdata.clear(); @@ -3273,6 +3273,11 @@ void mjCTendon::Compile(void) { if (range[0]>=range[1] && limited) { throw mjCError(this, "invalid limits in tendon '%s (id = %d)'", name.c_str(), id); } + + // check springlength + if (springlength[0] > springlength[1]) { + throw mjCError(this, "invalid springlength in tendon '%s (id = %d)'", name.c_str(), id); + } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 3d2c6b03..cfa430fb 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -802,7 +802,7 @@ class mjCTendon : public mjCBase { double stiffness; // stiffness coefficient double damping; // damping coefficient double frictionloss; // friction loss - double springlength; // spring length; -1: use qpos_spring + double springlength[2]; // spring resting length; {-1, -1}: use qpos_spring std::vector userdata; // user data float rgba[4]; // rgba when material is omitted diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index e7bb5c11..be686dac 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1588,7 +1588,10 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) { ReadAttr(elem, "stiffness", 1, &pten->stiffness, text); ReadAttr(elem, "damping", 1, &pten->damping, text); ReadAttr(elem, "frictionloss", 1, &pten->frictionloss, text); - ReadAttr(elem, "springlength", 1, &pten->springlength, text); + // read springlength, either one or two values; if one, copy to second value + if (ReadAttr(elem, "springlength", 2, pten->springlength, text, false, false) == 1) { + pten->springlength[1] = pten->springlength[0]; + } ReadAttr(elem, "rgba", 4, pten->rgba, text); // read userdata diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 0bb0e327..fb73c88d 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -51,7 +51,7 @@ class TINYXML2_LIB mj_XMLPrinter : public tinyxml2::XMLPrinter { public: void PrintSpace( int depth ) { - for( int i=0; istiffness, &def->tendon.stiffness); WriteAttr(elem, "damping", 1, &pten->damping, &def->tendon.damping); WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->tendon.frictionloss); - WriteAttr(elem, "springlength", 1, &pten->springlength, &def->tendon.springlength); - + if (pten->springlength[0] != pten->springlength[1] || + def->tendon.springlength[0] != def->tendon.springlength[1]) { + WriteAttr(elem, "springlength", 2, pten->springlength, def->tendon.springlength); + } else { + WriteAttr(elem, "springlength", 1, pten->springlength, def->tendon.springlength); + } // spatial only if (!fixed) { if (pten->material!=def->tendon.material) { diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 7263764d..b2ea7e07 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -565,7 +565,7 @@ string mjXUtil::FindValue(const mjMap* map, int mapsz, int value) { // read attribute "attr" of element "elem" // "len" is the number of floats or doubles to be read // the content is returned in "text", the numeric data in "data" -// return true if attribute found, false if not found and not required +// return number of elements found template int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len, T* data, string& text, bool required, bool exact) { diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 9c14a4c1..b59256a7 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -31,6 +31,7 @@ namespace { using ::testing::ElementsAre; using ::testing::Pointwise; using ::testing::DoubleNear; +using ::testing::NotNull; using CoreSmoothTest = MujocoTest; static std::vector GetVector(const mjtNum* array, int length) { @@ -47,7 +48,7 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { )"; mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, testing::NotNull()); + ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); mj_resetDataDebug(model, data, 'd'); @@ -206,7 +207,7 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) { constexpr char kRefsitePath[] = "engine/testdata/refsite.xml"; const std::string xml_path = GetTestDataFilePath(kRefsitePath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - ASSERT_THAT(model, ::testing::NotNull()); + ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); // set pose target in ctrl (3 positions, 3 rotations) @@ -410,5 +411,33 @@ TEST_F(AdhesionTest, ExpectedAdhesionForce) { mj_deleteModel(model); } + +// ------------------------------ tendons -------------------------------------- + +using TendonTest = MujocoTest; + +// check tendon spring deadband using example model +TEST_F(TendonTest, SpringrangeDeadband) { + const std::string xml_path = + GetTestDataFilePath("engine/testdata/tendon_springlength.xml"); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + + // initial state outside deadband: spring is active + mj_forward(model, data); + mjtNum expected_force = model->tendon_stiffness[0] * + (model->tendon_lengthspring[1] - data->ten_length[0]); + EXPECT_EQ(expected_force, data->qfrc_passive[0]); + + // put body inside deadband: spring is inactive + data->qpos[0] = -1; + mj_forward(model, data); + EXPECT_EQ(0, data->qfrc_passive[0]); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco diff --git a/test/engine/testdata/tendon_springlength.xml b/test/engine/testdata/tendon_springlength.xml new file mode 100644 index 00000000..1dacb1e9 --- /dev/null +++ b/test/engine/testdata/tendon_springlength.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 28dcf607..c0c08b25 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -679,7 +679,7 @@ TEST_F(UserDataTest, NSensorTooSmall) { EXPECT_THAT(error.data(), HasSubstr("nuser_sensor")); } -// ------------- test for auto parsing of *limited fields ------------- +// ------------- test for auto parsing of *limited fields ---------------------- using LimitedTest = MujocoTest; @@ -791,5 +791,59 @@ TEST_F(LimitedTest, ErrorIfForceLimitedMissingOnActuator) { EXPECT_THAT(error.data(), HasSubstr("actuator")); } +// ------------- tests for tendon springrange ---------------------------------- + +using SpringrangeTest = MujocoTest; + +TEST_F(SpringrangeTest, DefaultsPropagate) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->tendon_lengthspring[0], .2); + EXPECT_EQ(model->tendon_lengthspring[1], .5); + mj_deleteModel(model); +} + +TEST_F(SpringrangeTest, InvalidRange) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("invalid springlength in tendon")); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index a1d9bca3..2f08e31b 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -877,7 +877,7 @@ TEST_F(ActuatorParseTest, DampersDontRequireRange) { mj_deleteModel(model); } -// make sure range requirement is not enforced at parse time +// adhesion actuators inherit from general defaults TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 288740e8..5f7f36a6 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -708,6 +708,52 @@ TEST_F(XMLWriterTest, WritesSkin) { mj_deleteModel(mtemp); } +TEST_F(XMLWriterTest, SpringlengthOneValue) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("springlength=\"0.5\"")); + mj_deleteModel(model); +} + +TEST_F(XMLWriterTest, SpringlengthTwoValues) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("springlength=\"0 0.5\"")); + mj_deleteModel(model); +} + // check that no precision is lost when saving XMLs with FullFloatPrecision TEST_F(XMLWriterTest, SetPrecision) { static constexpr char xml[] = R"(