diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 156386b929..e6b55b2320 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6151,14 +6151,16 @@ applying the adhesive force. In the video above, such inactive contacts are blue An adhesion actuator's length is always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying :el:`general` attributes are set as follows: -=========== ======= =========== ======== -Attribute Setting Attribute Setting -=========== ======= =========== ======== -dyntype none dynprm 1 0 0 -gaintype fixed gainprm gain 0 0 -biastype none biasprm 0 0 0 -trntype body ctrllimited true -=========== ======= =========== ======== +============ ============= =========== ======== +Attribute Setting Attribute Setting +============ ============= =========== ======== +dyntype none/filter dynprm 1 0 0 +gaintype fixed gainprm gain 0 0 +biastype none biasprm 0 0 0 +trntype body ctrllimited true +============ ============= =========== ======== + +If :at:`timeconst` is nonzero, :at:`dyntype` is set to ``filter`` and :at:`dynprm` to ``timeconst 0 0``. This element has a subset of the common attributes and two custom attributes. @@ -6200,6 +6202,13 @@ This element has a subset of the common attributes and two custom attributes. value multiplied by the gain. This force is distributed equally between all the contacts involving geoms belonging to the target body. +.. _actuator-adhesion-timeconst: + +:at:`timeconst`: :at-val:`real, "0"` + Time constant for a first-order filter dynamics on the actuator activation. If positive, the actuator's + :at:`dyntype` is set to ``filter`` and the control signal is low-pass filtered with this time constant. If 0 + (default), no filter dynamics are applied. + .. _actuator-plugin: @@ -9713,6 +9722,8 @@ refsite, tendon, slidersite, cranksite. .. _default-adhesion-gain: +.. _default-adhesion-timeconst: + .. _default-adhesion-user: .. _default-adhesion-group: diff --git a/doc/includes/references.h b/doc/includes/references.h index 8622ba24ab..ef1e708657 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3634,7 +3634,8 @@ const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double tausmooth, double range[2], double force, double scale, double lmin, double lmax, double vmax, double fpmax, double fvmax); -const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +const char* mjs_setToAdhesion(mjsActuator* actuator, double gain, + double timeconst = 0); mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 3d5717f7ad..efd0342c2f 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1704,7 +1704,8 @@ MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], do double lmax, double vmax, double fpmax, double fvmax); // Set actuator to active adhesion; return error if any. -MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain, + double timeconst); //---------------------------------- Assets -------------------------------------------------------- diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 787dacffcd..5577e76149 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -10682,6 +10682,10 @@ name='gain', type=ValueType(name='double'), ), + FunctionParameterDecl( + name='timeconst', + type=ValueType(name='double'), + ), ), doc='Set actuator to active adhesion; return error if any.', )), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index e3bfc7ef9a..277e3a3e9b 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1296,13 +1296,13 @@ PYBIND11_MODULE(_specs, m) { py::arg("vmax") = -1, py::arg("fpmax") = -1, py::arg("fvmax") = -1); mjsActuator.def( "set_to_adhesion", - [](raw::MjsActuator* self, double gain) { - std::string err = mjs_setToAdhesion(self, gain); + [](raw::MjsActuator* self, double gain, double timeconst) { + std::string err = mjs_setToAdhesion(self, gain, timeconst); if (!err.empty()) { throw pybind11::value_error(err); } }, - py::arg("gain")); + py::arg("gain"), py::arg("timeconst") = 0); // ============================= MJSTENDONPATH =============================== // helper struct for tendon path indexing diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 46b44ee1d6..1e7380711f 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1052,14 +1052,22 @@ const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double t // Set to adhesion actuator. -const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) { +const char* mjs_setToAdhesion(mjsActuator* actuator, double gain, + double timeconst) { actuator->gainprm[0] = gain; actuator->ctrllimited = 1; actuator->gaintype = mjGAIN_FIXED; actuator->biastype = mjBIAS_NONE; + if (timeconst > 0) { + actuator->dynprm[0] = timeconst; + actuator->dyntype = mjDYN_FILTER; + } + if (gain < 0) return "adhesion gain cannot be negative"; + if (timeconst < 0) + return "adhesion timeconst cannot be negative"; if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) return "adhesion control range cannot be negative"; return ""; diff --git a/src/user/user_api.h b/src/user/user_api.h index 9bae6bdd45..fc59aad98a 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -190,7 +190,8 @@ MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], do double lmax, double vmax, double fpmax, double fvmax); // Set actuator to adhesion, return error on failure. -MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain, + double timeconst); //---------------------------------- Add assets ---------------------------------------------------- diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index ebbfb24f88..bd1e0efc61 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -204,7 +204,7 @@ std::vector MJCF[nMJCF] = { "timeconst", "range", "force", "scale", "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "?", "forcelimited", "ctrlrange", "forcerange", - "gain", "user", "group", "nsample", "interp", "delay"}, + "gain", "timeconst", "user", "group", "nsample", "interp", "delay"}, {">"}, {"extension", "*"}, @@ -432,7 +432,8 @@ std::vector MJCF[nMJCF] = { "timeconst", "tausmooth", "range", "force", "scale", "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay", - "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"}, + "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain", + "timeconst"}, {"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "joint", "jointinparent", @@ -2481,9 +2482,11 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { // adhesion else if (type == "adhesion") { double gain = actuator->gainprm[0]; + double timeconst = 0; ReadAttr(elem, "gain", 1, &gain, text); ReadAttr(elem, "ctrlrange", 2, actuator->ctrlrange, text); - err = mjs_setToAdhesion(actuator, gain); + ReadAttr(elem, "timeconst", 1, &timeconst, text); + err = mjs_setToAdhesion(actuator, gain, timeconst); } else if (type == "plugin") { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 42cc750734..cb72f70782 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3002,6 +3002,82 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { mj_deleteModel(model); } +// adhesion actuator with timeconst sets dyntype to filter +TEST_F(ActuatorParseTest, AdhesionTimeconst) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // expect dyntype to be filter + EXPECT_EQ(model->actuator_dyntype[0], mjDYN_FILTER); + // expect dynprm[0] to be the timeconst value + EXPECT_EQ(model->actuator_dynprm[0], 0.03); + // expect gain was set correctly + EXPECT_EQ(model->actuator_gainprm[0], 100); + mj_deleteModel(model); +} + +// adhesion actuator without timeconst has no dynamics +TEST_F(ActuatorParseTest, AdhesionNoTimeconst) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // expect dyntype to remain none + EXPECT_EQ(model->actuator_dyntype[0], mjDYN_NONE); + // expect gain was set correctly + EXPECT_EQ(model->actuator_gainprm[0], 50); + mj_deleteModel(model); +} + +// adhesion actuator rejects negative timeconst +TEST_F(ActuatorParseTest, AdhesionNegativeTimeconst) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); +} + TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) { static constexpr char xml[] = R"( diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 3aa71a8cf3..8221ebd68f 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -9808,8 +9808,8 @@ int mjs_setName_wrapper(MjsElement& element, const String& name) { return mjs_setName(element.get(), name.as().data()); } -std::string mjs_setToAdhesion_wrapper(MjsActuator& actuator, double gain) { - return std::string(mjs_setToAdhesion(actuator.get(), gain)); +std::string mjs_setToAdhesion_wrapper(MjsActuator& actuator, double gain, double timeconst) { + return std::string(mjs_setToAdhesion(actuator.get(), gain, timeconst)); } std::string mjs_setToCylinder_wrapper(MjsActuator& actuator, double timeconst, double bias, double area, double diameter) {