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

Allow tendon springlength attribute to take two values between which the spring produces no force (a deadband).

Browse Source 
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
This commit is contained in:
Yuval Tassa
2022-10-28 05:57:05 -07:00
committed by Copybara-Service
parent 3a445d0fdd
commit 893942a729
20 changed files with 259 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference.rst
+1 -1
View File
@@ -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)
doc/XMLreference.rst
+10 -3
View File
@@ -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<tendon-spatial>` tendons in mind, which can only have nonegative length. In order to
set the :at:`springlength` of a :ref:`fixed<tendon-fixed>` tendon to ``-1``, use a nearby value like ``-0.99999``.
|br| If two non-decreasing values are given, they define a `dead-band <https://en.wikipedia.org/wiki/Deadband>`_
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"`
doc/changelog.rst
+13 -1
View File
@@ -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 <https://en.wikipedia.org/wiki/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 <https://github.com/deepmind/mujoco/tree/main/test/engine/testdata/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)
--------------------------------
include/mujoco/mjmodel.h
+1 -1
View File
@@ -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)
include/mujoco/mjxmacro.h
+1 -1
View File
@@ -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) ) \
src/engine/engine_core_smooth.c
+14 -3
View File
@@ -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) {
src/engine/engine_sensor.c
+12 -2
View File
@@ -825,9 +825,19 @@ void mj_energyPos(const mjModel* m, mjData* d) {
if (!mjDISABLED(mjDSBL_PASSIVE)) {
for (int i=0; i<m->ntendon; 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;
}
}
}
src/engine/engine_setconst.c
+4 -3
View File
@@ -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; i<m->ntendon; 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];
}
}
}
src/engine/engine_vis_visualize.c
+19 -4
View File
@@ -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; i<m->ntendon; 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);
src/user/user_model.cc
+2 -1
View File
@@ -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);
src/user/user_objects.cc
+6 -1
View File
@@ -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);
}
}
src/user/user_objects.h
+1 -1
View File
@@ -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<double> userdata; // user data
float rgba[4]; // rgba when material is omitted
src/xml/xml_native_reader.cc
+4 -1
View File
@@ -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
src/xml/xml_native_writer.cc
+7 -3
View File
@@ -51,7 +51,7 @@ class TINYXML2_LIB mj_XMLPrinter : public tinyxml2::XMLPrinter {
public:
void PrintSpace( int depth ) {
for( int i=0; i<depth; ++i ) {
for (int i=0; i<depth; ++i) {
Write( " " );
}
}
@@ -524,8 +524,12 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
WriteAttr(elem, "stiffness", 1, &pten->stiffness, &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) {
src/xml/xml_util.cc
+1 -1
View File
@@ -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<typename T>
int mjXUtil::ReadAttr(XMLElement* elem, const char* attr, const int len,
T* data, string& text, bool required, bool exact) {
test/engine/engine_core_smooth_test.cc
+31 -2
View File
@@ -31,6 +31,7 @@ namespace {
using ::testing::ElementsAre;
using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::NotNull;
using CoreSmoothTest = MujocoTest;
static std::vector<mjtNum> GetVector(const mjtNum* array, int length) {
@@ -47,7 +48,7 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
</mujoco>
)";
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
test/engine/testdata/tendon_springlength.xml
Vendored
+30
View File
@@ -0,0 +1,30 @@
<mujoco>
<!-- use Runge-Kutta for better energy conservation -->
<option integrator="RK4">
<flag energy="enable"/>
</option>
<statistic center="0 0 0"/>
<worldbody>
<site name="origin" size=".01" rgba="1 0 0 1"/>
<light pos="0 0 3"/>
<body pos="1 0 .06">
<joint type="slide" axis="1 0 0"/>
<geom size=".05"/>
<site name="attachment" size=".01" pos="0 0 -.06" rgba="1 0 0 1"/>
</body>
</worldbody>
<tendon>
<spatial name="tendon" stiffness="10" springlength="0 .5">
<site site="origin"/>
<site site="attachment"/>
</spatial>
</tendon>
<sensor>
<tendonpos tendon="tendon"/>
</sensor>
</mujoco>
test/user/user_objects_test.cc
+55 -1
View File
@@ -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"(
<mujoco>
<default>
<tendon springlength=".2 .5"/>
</default>
<worldbody>
<site name="0"/>
<site name="1" pos="1 0 0"/>
</worldbody>
<tendon>
<spatial>
<site site="0"/>
<site site="1"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<site name="0"/>
<site name="1" pos="1 0 0"/>
</worldbody>
<tendon>
<spatial springlength="1 0">
<site site="0"/>
<site site="1"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> 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
test/xml/xml_native_reader_test.cc
+1 -1
View File
@@ -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"(
<mujoco>
test/xml/xml_native_writer_test.cc
+46
View File
@@ -708,6 +708,52 @@ TEST_F(XMLWriterTest, WritesSkin) {
mj_deleteModel(mtemp);
}
TEST_F(XMLWriterTest, SpringlengthOneValue) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="1"/>
<site name="2" pos="0 0 1"/>
</worldbody>
<tendon>
<spatial springlength="0.5">
<site site="1"/>
<site site="2"/>
</spatial>
</tendon>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<site name="1"/>
<site name="2" pos="0 0 1"/>
</worldbody>
<tendon>
<spatial springlength="0 0.5">
<site site="1"/>
<site site="2"/>
</spatial>
</tendon>
</mujoco>
)";
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"(