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

Fix a bug with PID actuators, when plugin definition order differs from actuator order.

Browse Source 
PiperOrigin-RevId: 587729879
Change-Id: If71c7e374f36ef08c5147e7316066d6f5365925f
This commit is contained in:
Nimrod Gileadi
2023-12-04 08:30:41 -08:00
committed by Copybara-Service
parent c0b7b9873d
commit 236dcb4ffd
3 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/actuator/pid.cc
+4 -4
View File
@@ -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++];
}