Add the pid actuator: setpoint inputs, integral action, slew rate limiting.
<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:
committed by
Copybara-Service
parent
7bc1aa9b05
commit
279df98cd0
@@ -266,7 +266,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
||||
('mjDYN_FILTEREXACT', 3),
|
||||
('mjDYN_MUSCLE', 4),
|
||||
('mjDYN_DCMOTOR', 5),
|
||||
('mjDYN_USER', 6),
|
||||
('mjDYN_PID', 6),
|
||||
('mjDYN_USER', 7),
|
||||
]),
|
||||
)),
|
||||
('mjtGain',
|
||||
@@ -279,7 +280,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
||||
('mjGAIN_MUSCLE', 2),
|
||||
('mjGAIN_DCMOTOR', 3),
|
||||
('mjGAIN_SO3', 4),
|
||||
('mjGAIN_USER', 5),
|
||||
('mjGAIN_PID', 5),
|
||||
('mjGAIN_USER', 6),
|
||||
]),
|
||||
)),
|
||||
('mjtBias',
|
||||
@@ -304,6 +306,16 @@ ENUMS: Mapping[str, EnumDecl] = dict([
|
||||
('mjCHART_QUAT', 2),
|
||||
]),
|
||||
)),
|
||||
('mjtCtrlInput',
|
||||
EnumDecl(
|
||||
name='mjtCtrlInput',
|
||||
declname='enum mjtCtrlInput',
|
||||
values=dict([
|
||||
('mjINPUT_POS', 1),
|
||||
('mjINPUT_VEL', 2),
|
||||
('mjINPUT_FF', 4),
|
||||
]),
|
||||
)),
|
||||
('mjtObj',
|
||||
EnumDecl(
|
||||
name='mjtObj',
|
||||
|
||||
@@ -11035,6 +11035,69 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
),
|
||||
doc='Set actuator to orientation servo.',
|
||||
)),
|
||||
('mjs_setToPID',
|
||||
FunctionDecl(
|
||||
name='mjs_setToPID',
|
||||
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='ki',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(1,),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='imax',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(1,),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='slewmax',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(1,),
|
||||
),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='inheritrange',
|
||||
type=ValueType(name='double'),
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='ctrlspec',
|
||||
type=ValueType(name='int'),
|
||||
),
|
||||
),
|
||||
doc='Set actuator to PID controller.',
|
||||
)),
|
||||
('mjs_setToDamper',
|
||||
FunctionDecl(
|
||||
name='mjs_setToDamper',
|
||||
|
||||
@@ -9647,6 +9647,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
||||
type=ValueType(name='int'),
|
||||
doc='input signature, scoped by gaintype; 0: type default',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='velrange',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(2,),
|
||||
),
|
||||
doc='range of the velocity-setpoint input (pid)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='ffrange',
|
||||
type=ArrayType(
|
||||
inner_type=ValueType(name='double'),
|
||||
extents=(2,),
|
||||
),
|
||||
doc='range of the feedforward input (pid)',
|
||||
),
|
||||
StructFieldDecl(
|
||||
name='actearly',
|
||||
type=ValueType(name='mjtBool'),
|
||||
|
||||
Reference in New Issue
Block a user