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

Add functions for setting an mjsActuator to a specific shortcut type.

Browse Source 
PiperOrigin-RevId: 769049331
Change-Id: If5bb04966bd29a57569a2c17d712b9ae3d33324d
This commit is contained in:
Alessio Quaglino
2025-06-09 02:15:48 -07:00
committed by Copybara-Service
parent dd18dd2024
commit f75772587a
8 changed files with 723 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+13
View File
@@ -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);
include/mujoco/mujoco.h
+32
View File
@@ -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.
python/mujoco/introspect/functions.py
+260
View File
@@ -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',
python/mujoco/specs.cc
+89
View File
@@ -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<double, 2>{-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",
python/mujoco/specs_test.py
+56
View File
@@ -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()
src/user/user_api.cc
+182
View File
@@ -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<mjCBase*>(element)->model->spec);
src/user/user_api.h
+31
View File
@@ -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.
src/xml/xml_native_reader.cc
+60 -139
View File
@@ -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<double> userdata;
if (ReadVector(elem, "user", userdata, text)) {