diff --git a/doc/includes/references.h b/doc/includes/references.h index 3f725ad9..1d539d56 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3438,6 +3438,19 @@ mjsTuple* mjs_addTuple(mjSpec* s); mjsKey* mjs_addKey(mjSpec* s); mjsPlugin* mjs_addPlugin(mjSpec* s); mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); +const char* mjs_setToMotor(mjsActuator* actuator); +const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); +const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); +const char* mjs_setToVelocity(mjsActuator* actuator, double kv); +const char* mjs_setToDamper(mjsActuator* actuator, double kv); +const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, + double bias, double area, double diameter); +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); 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 59283756..bc9242a2 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1501,6 +1501,38 @@ MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); +//---------------------------------- Set actuator parameters --------------------------------------- + +// Set actuator to motor, return error if any. +MJAPI const char* mjs_setToMotor(mjsActuator* actuator); + +// Set actuator to position, return error if any. +MJAPI const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to integrated velocity, return error if any. +MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to velocity servo, return error if any. +MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv); + +// Set actuator to activate damper, return error if any. +MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv); + +// Set actuator to hydraulic or pneumatic cylinder, return error if any. +MJAPI const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, + double bias, double area, double diameter); + +// Set actuator to muscle, return error if any.a +MJAPI 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); + +// Set actuator to active adhesion, return error if any. +MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); + + //---------------------------------- Assets -------------------------------------------------------- // Add mesh. diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 1c4877b9..680122a1 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -9551,6 +9551,266 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Add default.', )), + ('mjs_setToMotor', + FunctionDecl( + name='mjs_setToMotor', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + ), + doc='Set actuator to motor, return error if any.', + )), + ('mjs_setToPosition', + FunctionDecl( + name='mjs_setToPosition', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kp', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='kv', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='dampratio', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='inheritrange', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to position, return error if any.', + )), + ('mjs_setToIntVelocity', + FunctionDecl( + name='mjs_setToIntVelocity', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kp', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='kv', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='dampratio', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(1,), + ), + ), + FunctionParameterDecl( + name='inheritrange', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to integrated velocity, return error if any.', + )), + ('mjs_setToVelocity', + FunctionDecl( + name='mjs_setToVelocity', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kv', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to velocity servo, return error if any.', + )), + ('mjs_setToDamper', + FunctionDecl( + name='mjs_setToDamper', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='kv', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to activate damper, return error if any.', + )), + ('mjs_setToCylinder', + FunctionDecl( + name='mjs_setToCylinder', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='bias', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='area', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='diameter', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to hydraulic or pneumatic cylinder, return error if any.', # pylint: disable=line-too-long + )), + ('mjs_setToMuscle', + FunctionDecl( + name='mjs_setToMuscle', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='timeconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='tausmooth', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='range', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='force', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='scale', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='lmin', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='lmax', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='vmax', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='fpmax', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='fvmax', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to muscle, return error if any.a', + )), + ('mjs_setToAdhesion', + FunctionDecl( + name='mjs_setToAdhesion', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='gain', + type=ValueType(name='double'), + ), + ), + doc='Set actuator to active adhesion, return error if any.', + )), ('mjs_addMesh', FunctionDecl( name='mjs_addMesh', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 51846d2a..b2d081e6 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1010,6 +1010,95 @@ PYBIND11_MODULE(_specs, m) { [](raw::MjsActuator& self, raw::MjsDefault& default_) -> void { mjs_setDefault(self.element, &default_); }); + mjsActuator.def("set_to_motor", [](raw::MjsActuator* self) { + std::string err = mjs_setToMotor(self); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }); + mjsActuator.def( + "set_to_position", + [](raw::MjsActuator* self, double kp, double kv, double dampratio, + double timeconst, bool inheritrange) { + std::string err = mjs_setToPosition( + self, kp, kv == -1 ? nullptr : &kv, + dampratio == -1 ? nullptr : &dampratio, + timeconst == -1 ? nullptr : &timeconst, inheritrange); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1, + py::arg("timeconst") = -1, py::arg("inheritrange") = false); + mjsActuator.def( + "set_to_intvelocity", + [](raw::MjsActuator* self, double kp, double kv, double dampratio, + double timeconst, bool inheritrange) { + std::string err = mjs_setToIntVelocity( + self, kp, kv == -1 ? nullptr : &kv, + dampratio == -1 ? nullptr : &dampratio, + timeconst == -1 ? nullptr : &timeconst, inheritrange); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kp"), py::arg("kv") = -1, py::arg("dampratio") = -1, + py::arg("timeconst") = -1, py::arg("inheritrange") = false); + mjsActuator.def( + "set_to_velocity", + [](raw::MjsActuator* self, double kv) { + std::string err = mjs_setToVelocity(self, kv); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kv")); + mjsActuator.def( + "set_to_damper", + [](raw::MjsActuator* self, double kv) { + std::string err = mjs_setToDamper(self, kv); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("kv")); + mjsActuator.def( + "set_to_cylinder", + [](raw::MjsActuator* self, double timeconst, double bias, double area, + double diameter) { + std::string err = + mjs_setToCylinder(self, timeconst, bias, area, diameter); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("timeconst"), py::arg("bias"), py::arg("area"), + py::arg("diameter") = -1); + mjsActuator.def( + "set_to_muscle", + [](raw::MjsActuator* self, double timeconst[2], double tausmooth, + double range[2], double force, double scale, double lmin, double lmax, + double vmax, double fpmax, double fvmax) { + std::string err = + mjs_setToMuscle(self, timeconst, tausmooth, range, force, scale, + lmin, lmax, vmax, fpmax, fvmax); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("timeconst") = -1, py::arg("tausmooth"), + py::arg("range") = std::array{-1, -1}, py::arg("force") = -1, + py::arg("scale") = -1, py::arg("lmin") = -1, py::arg("lmax") = -1, + 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); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("gain")); // ============================= MJSTENDON =================================== mjsTendon.def("delete", diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 41a2043c..bf712505 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1232,5 +1232,61 @@ class SpecsTest(absltest.TestCase): self.assertGreater(spec3._address, 0) self.assertLen({spec1._address, spec2._address, spec3._address}, 3) + def test_actuator_shortname(self): + spec = mujoco.MjSpec() + actuator = spec.add_actuator( + gainprm=np.zeros((10, 1)), + dyntype=mujoco.mjtDyn.mjDYN_FILTER, + gaintype=mujoco.mjtGain.mjGAIN_AFFINE, + biastype=mujoco.mjtBias.mjBIAS_AFFINE, + ) + actuator.set_to_motor() + self.assertEqual(actuator.gainprm[0], 1) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + + actuator.set_to_position(kp=2.0, kv=3.0, timeconst=4.0, inheritrange=True) + self.assertEqual(actuator.gainprm[0], 2) + self.assertEqual(actuator.biasprm[1], -2) + self.assertEqual(actuator.biasprm[2], -3) + self.assertEqual(actuator.dynprm[0], 4) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_FILTEREXACT) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE) + self.assertEqual(actuator.inheritrange, True) + + actuator.set_to_intvelocity( + kp=2.0, kv=3.0, timeconst=4.0, inheritrange=True + ) + self.assertEqual(actuator.gainprm[0], 2) + self.assertEqual(actuator.biasprm[1], -2) + self.assertEqual(actuator.biasprm[2], -3) + self.assertEqual(actuator.dynprm[0], 4) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_INTEGRATOR) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE) + self.assertEqual(actuator.inheritrange, True) + + actuator.set_to_velocity(kv=5.0) + self.assertEqual(actuator.gainprm[0], 5) + self.assertEqual(actuator.biasprm[2], -5) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_AFFINE) + + actuator.set_to_damper(kv=6.0) + self.assertEqual(actuator.gainprm[0], 0) + self.assertEqual(actuator.gainprm[2], -6) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_AFFINE) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + + actuator.set_to_adhesion(gain=7.0) + self.assertEqual(actuator.gainprm[0], 7) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_NONE) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + if __name__ == '__main__': absltest.main() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index c25049b1..eea4aa93 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -695,6 +695,188 @@ mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* p +// set actuator to motor +const char* mjs_setToMotor(mjsActuator* actuator) { + // unit gain + actuator->gainprm[0] = 1; + + // implied parameters + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_NONE; + return ""; +} + + + +// set to position actuator +const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange) { + actuator->gainprm[0] = kp; + actuator->biasprm[1] = -kp; + + // set biasprm[2]; negative: regular damping, positive: dampratio + if (dampratio && kv) { + return "kv and dampratio cannot both be defined"; + } + + if (kv) { + if (*kv < 0) return "kv cannot be negative"; + actuator->biasprm[2] = -(*kv); + } + if (dampratio) { + if (*dampratio < 0) return "dampratio cannot be negative"; + actuator->biasprm[2] = *dampratio; + } + if (timeconst) { + if (*timeconst < 0) return "timeconst cannot be negative"; + actuator->dynprm[0] = *timeconst; + actuator->dyntype = *timeconst == 0 ? mjDYN_NONE : mjDYN_FILTEREXACT; + } + actuator->inheritrange = inheritrange; + + if (inheritrange > 0) { + if (actuator->ctrlrange[0] || actuator->ctrlrange[1]) { + return "ctrlrange and inheritrange cannot both be defined"; + } + } + + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; + return ""; +} + + + +// Set to integrated velocity actuator. +const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange) { + mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange); + actuator->dyntype = mjDYN_INTEGRATOR; + actuator->actlimited = 1; + + if (inheritrange > 0) { + if (actuator->actrange[0] || actuator->actrange[1]) { + return "actrange and inheritrange cannot both be defined"; + } + } + return ""; +} + + + +// Set to velocity actuator. +const char* mjs_setToVelocity(mjsActuator* actuator, double kv) { + mjuu_zerovec(actuator->biasprm, mjNBIAS); + actuator->gainprm[0] = kv; + actuator->biasprm[2] = -kv; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; + return ""; +} + + + +// Set to damper actuator. +const char* mjs_setToDamper(mjsActuator* actuator, double kv) { + mjuu_zerovec(actuator->gainprm, mjNGAIN); + actuator->gainprm[2] = -kv; + actuator->ctrllimited = 1; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_AFFINE; + actuator->biastype = mjBIAS_NONE; + + if (kv < 0) { + return "damping coefficient cannot be negative"; + } + if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) { + return "damper control range cannot be negative"; + } + return ""; +} + + + +// Set to cylinder actuator. +const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, double bias, + double area, double diameter) { + actuator->dynprm[0] = timeconst; + actuator->biasprm[0] = bias; + actuator->gainprm[0] = area; + if (diameter >= 0) { + actuator->gainprm[0] = mjPI / 4 * diameter*diameter; + } + actuator->dyntype = mjDYN_FILTER; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; + return ""; +} + + + +// Set to muscle actuator. +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) { + // set muscle defaults if same as global defaults + if (actuator->dynprm[0] == 1) actuator->dynprm[0] = 0.01; // tau act + if (actuator->dynprm[1] == 0) actuator->dynprm[1] = 0.04; // tau deact + if (actuator->gainprm[0] == 1) actuator->gainprm[0] = 0.75; // range[0] + if (actuator->gainprm[1] == 0) actuator->gainprm[1] = 1.05; // range[1] + if (actuator->gainprm[2] == 0) actuator->gainprm[2] = -1; // force + if (actuator->gainprm[3] == 0) actuator->gainprm[3] = 200; // scale + if (actuator->gainprm[4] == 0) actuator->gainprm[4] = 0.5; // lmin + if (actuator->gainprm[5] == 0) actuator->gainprm[5] = 1.6; // lmax + if (actuator->gainprm[6] == 0) actuator->gainprm[6] = 1.5; // vmax + if (actuator->gainprm[7] == 0) actuator->gainprm[7] = 1.3; // fpmax + if (actuator->gainprm[8] == 0) actuator->gainprm[8] = 1.2; // fvmax + + if (tausmooth < 0) + return "muscle tausmooth cannot be negative"; + + actuator->dynprm[2] = tausmooth; + if (timeconst[0] >= 0) actuator->dynprm[0] = timeconst[0]; + if (timeconst[1] >= 0) actuator->dynprm[1] = timeconst[1]; + if (range[0] >= 0) actuator->gainprm[0] = range[0]; + if (range[1] >= 0) actuator->gainprm[1] = range[1]; + if (force >= 0) actuator->gainprm[2] = force; + if (scale >= 0) actuator->gainprm[3] = scale; + if (lmin >= 0) actuator->gainprm[4] = lmin; + if (lmax >= 0) actuator->gainprm[5] = lmax; + if (vmax >= 0) actuator->gainprm[6] = vmax; + if (fpmax >= 0) actuator->gainprm[7] = fpmax; + if (fvmax >= 0) actuator->gainprm[8] = fvmax; + + // biasprm = gainprm + for (int n=0; n < 9; n++) { + actuator->biasprm[n] = actuator->gainprm[n]; + } + + actuator->dyntype = mjDYN_MUSCLE; + actuator->gaintype = mjGAIN_MUSCLE; + actuator->biastype = mjBIAS_MUSCLE; + return ""; +} + + + +// Set to adhesion actuator. +const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) { + actuator->gainprm[0] = gain; + actuator->ctrllimited = 1; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_NONE; + + if (gain < 0) + return "adhesion gain cannot be negative"; + if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) + return "adhesion control range cannot be negative"; + return ""; +} + + + // get spec from body mjSpec* mjs_getSpec(mjsElement* element) { return &(static_cast(element)->model->spec); diff --git a/src/user/user_api.h b/src/user/user_api.h index 04cdf13e..37825a76 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -166,6 +166,37 @@ MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); +//---------------------------------- Set actuator parameters --------------------------------------- + +// Set actuator to motor, return error on failure. +MJAPI const char* mjs_setToMotor(mjsActuator* actuator); + +// Set actuator to position, return error on failure. +MJAPI const char* mjs_setToPosition(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to integrated velocity, return error on failure. +MJAPI const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1], + double dampratio[1], double timeconst[1], double inheritrange); + +// Set actuator to velocity, return error on failure. +MJAPI const char* mjs_setToVelocity(mjsActuator* actuator, double kv); + +// Set actuator to damper, return error on failure. +MJAPI const char* mjs_setToDamper(mjsActuator* actuator, double kv); + +// Set actuator to cylinder actuator, return error on failure. +MJAPI const char* mjs_setToCylinder(mjsActuator* actuator, double timeconst, + double bias, double area, double diameter); + +// Set actuator to muscle, return error on failure. +MJAPI 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); + +// Set actuator to adhesion, return error on failure. +MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); + //---------------------------------- Add assets ---------------------------------------------------- // Add mesh. diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c5ad9c8f..e8a7dc13 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2210,6 +2210,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { type = elem->Value(); // explicit attributes + string err; if (type == "general") { // explicit attributes int n; @@ -2233,183 +2234,98 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { // direct drive motor else if (type == "motor") { - // unit gain - actuator->gainprm[0] = 1; - - // implied parameters - actuator->dyntype = mjDYN_NONE; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_NONE; + err = mjs_setToMotor(actuator); } // position or integrated velocity servo else if (type == "position" || type == "intvelocity") { - // explicit attributes - ReadAttr(elem, "kp", 1, actuator->gainprm, text); - actuator->biasprm[1] = -actuator->gainprm[0]; + double kp = actuator->gainprm[0]; + ReadAttr(elem, "kp", 1, &kp, text); // read kv - double kv = -1; // -1: undefined - if (ReadAttr(elem, "kv", 1, &kv, text)) { - if (kv < 0) throw mjXError(elem, "kv cannot be negative"); + double kv_data; + double *kv = &kv_data; + if (!ReadAttr(elem, "kv", 1, kv, text)) { + kv = nullptr; } // read dampratio - double dampratio = -1; // -1: undefined - if (ReadAttr(elem, "dampratio", 1, &dampratio, text)) { - if (dampratio < 0) throw mjXError(elem, "dampratio cannot be negative"); + double dampratio_data; + double *dampratio = &dampratio_data; + if (!ReadAttr(elem, "dampratio", 1, dampratio, text)) { + dampratio = nullptr; } - // set biasprm[2]; negative: regular damping, positive: dampratio - if (dampratio > 0 && kv > 0) { - throw mjXError(elem, "kv and dampratio cannot both be defined"); - } - if (kv > 0) actuator->biasprm[2] = -kv; - if (dampratio > 0) actuator->biasprm[2] = dampratio; - // read timeconst, set dyntype - if (ReadAttr(elem, "timeconst", 1, actuator->dynprm, text)) { - if (actuator->dynprm[0] < 0) - throw mjXError(elem, "timeconst cannot be negative"); - actuator->dyntype = actuator->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE; + double timeconst_data; + double *timeconst = &timeconst_data; + if (!ReadAttr(elem, "timeconst", 1, timeconst, text)) { + timeconst = nullptr; } // handle inheritrange - ReadAttr(elem, "inheritrange", 1, &actuator->inheritrange, text); - if (actuator->inheritrange > 0) { - if (type == "position") { - if (actuator->ctrlrange[0] || actuator->ctrlrange[1]) { - throw mjXError(elem, "ctrlrange and inheritrange cannot both be defined"); - } - } else { - if (actuator->actrange[0] || actuator->actrange[1]) { - throw mjXError(elem, "actrange and inheritrange cannot both be defined"); - } - } - } + double inheritrange = actuator->inheritrange; + ReadAttr(elem, "inheritrange", 1, &inheritrange, text); - // implied parameters - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_AFFINE; - - if (type == "intvelocity") { - actuator->dyntype = mjDYN_INTEGRATOR; - actuator->actlimited = 1; + if (type == "position") { + err = mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange); + } else { + err = mjs_setToIntVelocity(actuator, kp, kv, dampratio, timeconst, inheritrange); } } // velocity servo else if (type == "velocity") { - // clear bias - mjuu_zerovec(actuator->biasprm, mjNBIAS); - - // explicit attributes - ReadAttr(elem, "kv", 1, actuator->gainprm, text); - actuator->biasprm[2] = -actuator->gainprm[0]; - - // implied parameters - actuator->dyntype = mjDYN_NONE; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_AFFINE; + double kv = actuator->gainprm[0]; + ReadAttr(elem, "kv", 1, &kv, text); + err = mjs_setToVelocity(actuator, kv); } // damper else if (type == "damper") { - // clear gain - mjuu_zerovec(actuator->gainprm, mjNGAIN); - - // explicit attributes - ReadAttr(elem, "kv", 1, actuator->gainprm+2, text); - if (actuator->gainprm[2] < 0) - throw mjXError(elem, "damping coefficient cannot be negative"); - actuator->gainprm[2] = -actuator->gainprm[2]; - - // require nonnegative range - if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) { - throw mjXError(elem, "damper control range cannot be negative"); - } - - // implied parameters - actuator->ctrllimited = 1; - actuator->dyntype = mjDYN_NONE; - actuator->gaintype = mjGAIN_AFFINE; - actuator->biastype = mjBIAS_NONE; + double kv = 0; + ReadAttr(elem, "kv", 1, &kv, text); + err = mjs_setToDamper(actuator, kv); } // cylinder else if (type == "cylinder") { - // explicit attributes - ReadAttr(elem, "timeconst", 1, actuator->dynprm, text); - ReadAttr(elem, "bias", 3, actuator->biasprm, text); - ReadAttr(elem, "area", 1, actuator->gainprm, text); - double diameter; - if (ReadAttr(elem, "diameter", 1, &diameter, text)) { - actuator->gainprm[0] = mjPI / 4 * diameter*diameter; - } - - // implied parameters - actuator->dyntype = mjDYN_FILTER; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_AFFINE; + double timeconst = actuator->dynprm[0]; + double bias = actuator->biasprm[0]; + double area = actuator->gainprm[0]; + double diameter = -1; + ReadAttr(elem, "timeconst", 1, &timeconst, text); + ReadAttr(elem, "bias", 3, &bias, text); + ReadAttr(elem, "area", 1, &area, text); + ReadAttr(elem, "diameter", 1, &diameter, text); + err = mjs_setToCylinder(actuator, timeconst, bias, area, diameter); } // muscle else if (type == "muscle") { - // set muscle defaults if same as global defaults - if (actuator->dynprm[0] == 1)actuator->dynprm[0] = 0.01; // tau act - if (actuator->dynprm[1] == 0)actuator->dynprm[1] = 0.04; // tau deact - if (actuator->gainprm[0] == 1)actuator->gainprm[0] = 0.75; // range[0] - if (actuator->gainprm[1] == 0)actuator->gainprm[1] = 1.05; // range[1] - if (actuator->gainprm[2] == 0)actuator->gainprm[2] = -1; // force - if (actuator->gainprm[3] == 0)actuator->gainprm[3] = 200; // scale - if (actuator->gainprm[4] == 0)actuator->gainprm[4] = 0.5; // lmin - if (actuator->gainprm[5] == 0)actuator->gainprm[5] = 1.6; // lmax - if (actuator->gainprm[6] == 0)actuator->gainprm[6] = 1.5; // vmax - if (actuator->gainprm[7] == 0)actuator->gainprm[7] = 1.3; // fpmax - if (actuator->gainprm[8] == 0)actuator->gainprm[8] = 1.2; // fvmax - - // explicit attributes - ReadAttr(elem, "timeconst", 2, actuator->dynprm, text); - ReadAttr(elem, "tausmooth", 1, actuator->dynprm+2, text); - if (actuator->dynprm[2] < 0) - throw mjXError(elem, "muscle tausmooth cannot be negative"); - ReadAttr(elem, "range", 2, actuator->gainprm, text); - ReadAttr(elem, "force", 1, actuator->gainprm+2, text); - ReadAttr(elem, "scale", 1, actuator->gainprm+3, text); - ReadAttr(elem, "lmin", 1, actuator->gainprm+4, text); - ReadAttr(elem, "lmax", 1, actuator->gainprm+5, text); - ReadAttr(elem, "vmax", 1, actuator->gainprm+6, text); - ReadAttr(elem, "fpmax", 1, actuator->gainprm+7, text); - ReadAttr(elem, "fvmax", 1, actuator->gainprm+8, text); - - // biasprm = gainprm - for (int n=0; n < 9; n++) { - actuator->biasprm[n] = actuator->gainprm[n]; - } - - // implied parameters - actuator->dyntype = mjDYN_MUSCLE; - actuator->gaintype = mjGAIN_MUSCLE; - actuator->biastype = mjBIAS_MUSCLE; + double tausmooth = actuator->dynprm[2]; + double force = -1, scale = -1, lmin = -1, lmax = -1, vmax = -1, fpmax = -1, fvmax = -1; + double range[2] = {-1, -1}, timeconst[2] = {-1, -1}; + ReadAttr(elem, "timeconst", 2, timeconst, text); + ReadAttr(elem, "tausmooth", 1, &tausmooth, text); + ReadAttr(elem, "range", 2, range, text); + ReadAttr(elem, "force", 1, &force, text); + ReadAttr(elem, "scale", 1, &scale, text); + ReadAttr(elem, "lmin", 1, &lmin, text); + ReadAttr(elem, "lmax", 1, &lmax, text); + ReadAttr(elem, "vmax", 1, &vmax, text); + ReadAttr(elem, "fpmax", 1, &fpmax, text); + ReadAttr(elem, "fvmax", 1, &fvmax, text); + err = mjs_setToMuscle(actuator, timeconst, tausmooth, range, force, scale, + lmin, lmax, vmax, fpmax, fvmax); } // adhesion else if (type == "adhesion") { - // explicit attributes - ReadAttr(elem, "gain", 1, actuator->gainprm, text); - if (actuator->gainprm[0] < 0) - throw mjXError(elem, "adhesion gain cannot be negative"); - - // require nonnegative range + double gain = actuator->gainprm[0]; + ReadAttr(elem, "gain", 1, &gain, text); ReadAttr(elem, "ctrlrange", 2, actuator->ctrlrange, text); - if (actuator->ctrlrange[0] < 0 || actuator->ctrlrange[1] < 0) { - throw mjXError(elem, "adhesion control range cannot be negative"); - } - - // implied parameters - actuator->ctrllimited = 1; - actuator->gaintype = mjGAIN_FIXED; - actuator->biastype = mjBIAS_NONE; + err = mjs_setToAdhesion(actuator, gain); } else if (type == "plugin") { @@ -2429,6 +2345,11 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { throw mjXError(elem, "unrecognized actuator type: %s", type.c_str()); } + // throw error if any of the above failed + if (!err.empty()) { + throw mjXError(elem, err.c_str()); + } + // read userdata std::vector userdata; if (ReadVector(elem, "user", userdata, text)) {