No longer modify control vector in-place, add mju_clip.

- Never modify `d->ctrl`:
  - `ctrl` values which are outside their respective `ctrlrange` are clamped internally, but `d->ctrl` itself is unmodified.
  - Similarly, if `mjWARN_BADCTRL` is raised (NaNs or huge values), `ctrl` is cleared internally, but `d->ctrl` itself is unmodified.
- `mjWARN_BADCTRL` is no longer raised for `d->ctrl` values that are larger in absolute value than `mjMAXVAL`, if clamping makes them smaller than that.
- Added `mju_clip()` utility function.
- Added missing warning string for `case mjWARN_BADCTRL`.

PiperOrigin-RevId: 459025950
Change-Id: Ifbf974f9c02c3d70afdaa3114018fc81dfd746d3
This commit is contained in:
Yuval Tassa
2022-07-05 01:38:01 -07:00
committed by Copybara-Service
parent 09a5efc09e
commit 6d01627c0b
7 changed files with 158 additions and 43 deletions
+21 -3
View File
@@ -524,10 +524,10 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
// muscle activation dynamics, prm = (tau_act, tau_deact)
mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
// clamp control
mjtNum ctrlclamp = mjMIN(1, mjMAX(0, ctrl));
mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
// clamp activation
mjtNum actclamp = mjMIN(1, mjMAX(0, act));
mjtNum actclamp = mju_clip(act, 0, 1);
// compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
mjtNum tau;
@@ -688,6 +688,19 @@ mjtNum mju_max(mjtNum a, mjtNum b) {
// clip x to the range [min, max]
mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) {
if (x<min) {
return min;
} else if (x>max) {
return max;
} else {
return x;
}
}
// sign function
mjtNum mju_sign(mjtNum x) {
if (x<0) {
@@ -928,8 +941,13 @@ const char* mju_warningText(int warning, int info) {
mjSNPRINTF(str, "Nan, Inf or huge value in QACC at DOF %d. The simulation is unstable.", info);
break;
case mjWARN_BADCTRL:
mjSNPRINTF(str, "Nan, Inf or huge value in CTRL at ACTUATOR %d. The simulation is unstable.",
info);
break;
default:
mjSTRNCPY(str, "Unknown warning type");
mjSNPRINTF(str, "Unknown warning type %d.", warning);
}
return str;