From 893942a729123c193eb5aadaabca0a8042bd20d0 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 28 Oct 2022 05:57:05 -0700 Subject: [PATCH] Allow tendon `springlength` attribute to take two values between which the spring produces no force (a deadband). If given two non-decreasing values, `springlength` specifies a deadband for spring stiffness, inside of which the force is 0 and outside of which force behaves like a regular spring, with the setpoint corresponding to the nearest 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 test/engine/testdata/tendon_springlength.xml example model. Spring-limited spatial tendons whose lower range value is 0 are rendered as catenaries. Fixes #520. PiperOrigin-RevId: 484509706 Change-Id: I6698e94ee36168f52d501e83057559805e05172d --- doc/APIreference.rst | 2 +- doc/XMLreference.rst | 13 +++-- doc/changelog.rst | 14 ++++- include/mujoco/mjmodel.h | 2 +- include/mujoco/mjxmacro.h | 2 +- src/engine/engine_core_smooth.c | 17 ++++-- src/engine/engine_sensor.c | 14 ++++- src/engine/engine_setconst.c | 7 +-- src/engine/engine_vis_visualize.c | 23 ++++++-- src/user/user_model.cc | 3 +- src/user/user_objects.cc | 7 ++- src/user/user_objects.h | 2 +- src/xml/xml_native_reader.cc | 5 +- src/xml/xml_native_writer.cc | 10 ++-- src/xml/xml_util.cc | 2 +- test/engine/engine_core_smooth_test.cc | 33 +++++++++++- test/engine/testdata/tendon_springlength.xml | 30 +++++++++++ test/user/user_objects_test.cc | 56 +++++++++++++++++++- test/xml/xml_native_reader_test.cc | 2 +- test/xml/xml_native_writer_test.cc | 46 ++++++++++++++++ 20 files changed, 259 insertions(+), 31 deletions(-) create mode 100644 test/engine/testdata/tendon_springlength.xml 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"(