diff --git a/plugin/actuator/pid.cc b/plugin/actuator/pid.cc index 54260fe0..62fba647 100644 --- a/plugin/actuator/pid.cc +++ b/plugin/actuator/pid.cc @@ -160,7 +160,7 @@ mjtNum Pid::GetCtrl(const mjModel* m, const mjData* d, const State& state, } void Pid::ActDot(const mjModel* m, mjData* d, int instance) const { - State state = GetState(m, d, instance); + State state = GetState(m, d, actuator_idx_); mjtNum ctrl = GetCtrl(m, d, state, /*actearly=*/false); mjtNum error = ctrl - d->actuator_length[actuator_idx_]; @@ -180,7 +180,7 @@ void Pid::ActDot(const mjModel* m, mjData* d, int instance) const { } void Pid::Compute(const mjModel* m, mjData* d, int instance) { - State state = GetState(m, d, instance); + State state = GetState(m, d, actuator_idx_); mjtNum ctrl = GetCtrl(m, d, state, m->actuator_actearly[actuator_idx_]); mjtNum error = ctrl - d->actuator_length[actuator_idx_]; @@ -217,9 +217,9 @@ int Pid::ActDim(const mjModel* m, int instance, int actuator_id) { return (i_gain ? 1 : 0) + (HasSlew(m, instance) ? 1 : 0); } -Pid::State Pid::GetState(const mjModel* m, mjData* d, int instance) const { +Pid::State Pid::GetState(const mjModel* m, mjData* d, int actuator_idx) const { State state; - int state_idx = m->actuator_actadr[instance]; + int state_idx = m->actuator_actadr[actuator_idx]; if (config_.i_gain) { state.integral = d->act[state_idx++]; } diff --git a/plugin/actuator/pid.h b/plugin/actuator/pid.h index 1e2c5f24..185e07f3 100644 --- a/plugin/actuator/pid.h +++ b/plugin/actuator/pid.h @@ -84,7 +84,7 @@ class Pid { mjtNum integral = 0; }; // Reads data from d->act and returns it as a State struct. - State GetState(const mjModel* m, mjData* d, int instance) const; + State GetState(const mjModel* m, mjData* d, int actuator_idx) const; // Returns the PID setpoint, which is normally d->ctrl, but can be d->act for // actuators with dyntype != none. diff --git a/test/plugin/actuator/pid_test.cc b/test/plugin/actuator/pid_test.cc index 5e13c439..06938d2f 100644 --- a/test/plugin/actuator/pid_test.cc +++ b/test/plugin/actuator/pid_test.cc @@ -322,10 +322,6 @@ TEST_F(PidTest, ITerm) { - - - - @@ -337,6 +333,14 @@ TEST_F(PidTest, ITerm) { + + + + +