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 { 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 ctrl = GetCtrl(m, d, state, /*actearly=*/false);
mjtNum error = ctrl - d->actuator_length[actuator_idx_]; 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) { 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 ctrl = GetCtrl(m, d, state, m->actuator_actearly[actuator_idx_]);
mjtNum error = ctrl - d->actuator_length[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); 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; State state;
int state_idx = m->actuator_actadr[instance]; int state_idx = m->actuator_actadr[actuator_idx];
if (config_.i_gain) { if (config_.i_gain) {
state.integral = d->act[state_idx++]; state.integral = d->act[state_idx++];
} }
plugin/actuator/pid.h
+1 -1
View File
@@ -84,7 +84,7 @@ class Pid {
mjtNum integral = 0; mjtNum integral = 0;
}; };
// Reads data from d->act and returns it as a State struct. // 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 // Returns the PID setpoint, which is normally d->ctrl, but can be d->act for
// actuators with dyntype != none. // actuators with dyntype != none.
test/plugin/actuator/pid_test.cc
+8 -4
View File
@@ -322,10 +322,6 @@ TEST_F(PidTest, ITerm) {
<mujoco> <mujoco>
<extension> <extension>
<plugin plugin="mujoco.pid"> <plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="40.0"/>
<config key="kd" value="4"/>
</instance>
<instance name="pid2"> <instance name="pid2">
<config key="kp" value="40.0"/> <config key="kp" value="40.0"/>
<config key="ki" value="40"/> <config key="ki" value="40"/>
@@ -337,6 +333,14 @@ TEST_F(PidTest, ITerm) {
<config key="kd" value="4"/> <config key="kd" value="4"/>
<config key="imax" value="5"/> <config key="imax" value="5"/>
</instance> </instance>
<!-- intentionally put the plugin instances out of order, to test
any accidentaly dependency on the plugin definition order matching
the actuator order.
-->
<instance name="pid1">
<config key="kp" value="40.0"/>
<config key="kd" value="4"/>
</instance>
</plugin> </plugin>
</extension> </extension>