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

Add the pid actuator: setpoint inputs, integral action, slew rate limiting.

Browse Source 
<pid kp kv|dampratio [ki imax] [slewmax]> is a PID controller with real position and velocity setpoint inputs on a single force output, plus an optional feedforward input. With a zero velocity setpoint it reproduces <position> bit-exactly; the input signature is any subset of [pos, vel, ff], selected with input="..." and recorded as mjtCtrlInput bits in
actuator_ctrlspec; absent setpoint inputs are fixed at zero, so the control vector contains no inert entries.

kp and kv are single-sourced in the affine bias parameters (biasprm[1,2]) with no gainprm mirror: every consumer of the position-servo shape
(dampratio conversion, inheritrange, qDeriv) reads one location, which is what makes the bit-exact <position> parity possible. Controller state uses dyntype 'pid' with slot-gated activations in the order [slew, integral], following the dcmotor slot idiom: slewmax (dynprm[1]) rate limits the effective position setpoint through an activation holding it;
ki (gainprm[0]) integrates the position error -- wrapped on rotational transmissions -- with anti-windup clamping of the integrand at imax (dynprm[0]). Both features require the pos input. Servo input unpacking is shared with the dcmotor controller (unpackServoInputs); per-input ranges are exposed as posrange/velrange/ffrange.

This subsumes the functionality of the mujoco.pid plugin with proper activation state: correct under all integrators, visible to keyframes, act sensors and reset. Migration: kp/ki/kd map to kp/ki/kv, plugin imax is in force units (divide by ki), slewmax carries over; the single ctrl becomes input="pos".

PiperOrigin-RevId: 957588898
Change-Id: Id2786836ca6e76f58e5b5cc8323fc23be0a53784
This commit is contained in:
Yuval Tassa
2026-08-01 04:28:10 -07:00
committed by Copybara-Service
parent 7bc1aa9b05
commit 279df98cd0
33 changed files with 1504 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/user/user_api_test.cc
+36 -1
View File
@@ -18,7 +18,7 @@
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <filesystem>
#include <filesystem> // NOLINT
#include <functional>
#include <map>
#include <memory>
@@ -513,6 +513,41 @@ TEST_F(MujocoTest, SetToOrientation) {
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToPID) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
// stateless PID with kv, default input signature
double kv = 3.0;
const char* err = mjs_setToPID(actuator, 5.0, &kv, nullptr, nullptr, nullptr,
nullptr, 0, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gaintype, mjGAIN_PID);
EXPECT_EQ(actuator->biastype, mjBIAS_AFFINE);
EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
EXPECT_EQ(actuator->biasprm[1], -5.0);
EXPECT_EQ(actuator->biasprm[2], -3.0);
EXPECT_EQ(actuator->gainprm[0], 0.0);
// integral action with anti-windup, pos-only signature
double ki = 0.5, imax = 2.0, dampratio = 1.0;
err = mjs_setToPID(actuator, 5.0, nullptr, &dampratio, &ki, &imax, nullptr, 0,
mjINPUT_POS);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->dyntype, mjDYN_PID);
EXPECT_EQ(actuator->gainprm[0], 0.5);
EXPECT_EQ(actuator->dynprm[0], 2.0);
EXPECT_EQ(actuator->biasprm[2], 1.0);
EXPECT_EQ(actuator->ctrlspec, mjINPUT_POS);
// kv and dampratio are mutually exclusive
err = mjs_setToPID(actuator, 5.0, &kv, &dampratio, nullptr, nullptr, nullptr,
0, 0);
EXPECT_STREQ(err, "kv and dampratio cannot both be defined");
mj_deleteSpec(spec);
}
static constexpr char xml_plugin_1[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>