Add functions for setting an mjsActuator to a specific shortcut type.
PiperOrigin-RevId: 769049331 Change-Id: If5bb04966bd29a57569a2c17d712b9ae3d33324d
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
dd18dd2024
commit
f75772587a
@@ -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);
|
||||
|
||||
@@ -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.
|
||||
|
||||
Reference in New Issue
Block a user