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

Explicitly specify actdim on PID plugin actuators, to allow them to be used with keyframes.

Browse Source 
Support actdim attribute on plugin actuators.

Remove the actuator_actdim callback. This callback leads to crashes when loading keyframes for models that have a stateful plugin.

PiperOrigin-RevId: 650649505
Change-Id: I40d1994d02ae4e20ce631b2702a984766cf2628e
This commit is contained in:
Nimrod Gileadi
2024-07-09 09:07:44 -07:00
committed by Copybara-Service
parent e0d33eb1d0
commit 393895bb29
16 changed files with 201 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_plugin_test.cc
+11 -23
View File
@@ -149,12 +149,6 @@ class TestActuator : public BaseTestPlugin {
} else {
multiplier = kDefaultMultiplier;
}
s = mj_getPluginConfig(m, instance, "actdim");
if (*s) {
std::stringstream(s) >> actdim_;
} else {
actdim_ = 0;
}
for (int i = 0; i < m->nu; ++i) {
if (m->actuator_plugin[i] == instance) {
actuators.push_back(&d->actuator_force[i]);
@@ -187,7 +181,13 @@ class TestActuator : public BaseTestPlugin {
if (m->actuator_plugin[i] != instance_) {
continue;
}
mju_fill(d->act_dot + m->actuator_actadr[i], kActDotValue, actdim_);
// If the user specified an actdim, fill the act_dot values that belong
// to the plugin.
int actnum = m->actuator_actnum[i];
if (m->actuator_dyntype[i] != mjDYN_NONE) {
actnum--;
}
mju_fill(d->act_dot + m->actuator_actadr[i], kActDotValue, actnum);
}
}
@@ -195,7 +195,6 @@ class TestActuator : public BaseTestPlugin {
mjtNum multiplier;
std::vector<mjtNum*> actuators;
int instance_;
int actdim_;
void WriteActuatorForce() {
for (mjtNum* actuator_force : actuators) {
@@ -263,7 +262,7 @@ int RegisterActuatorPlugin() {
plugin.name = "mujoco.test.actuator";
const char* attributes[] = {"stride", "multiplier", "actdim"};
const char* attributes[] = {"stride", "multiplier"};
plugin.nattribute = sizeof(attributes) / sizeof(*attributes);
plugin.attributes = attributes;
@@ -296,16 +295,6 @@ int RegisterActuatorPlugin() {
auto actuator = reinterpret_cast<TestActuator*>(d->plugin_data[instance]);
actuator->Advance();
};
plugin.actuator_actdim =
+[](const mjModel* m, int instance, int actuator_id) {
const char* actdim_str = mj_getPluginConfig(m, instance, "actdim");
if (actdim_str) {
int actdim = 0;
std::stringstream(actdim_str) >> actdim;
return actdim;
}
return 0;
};
plugin.actuator_act_dot = +[](const mjModel* m, mjData* d, int instance) {
auto actuator = reinterpret_cast<TestActuator*>(d->plugin_data[instance]);
actuator->ActDot(m, d);
@@ -383,7 +372,6 @@ constexpr char xml[] = R"(
<instance name="actuator2">
<config key="stride" value="2"/>
<config key="multiplier" value="0.125"/>
<config key="actdim" value="3"/>
</instance>
</plugin>
<plugin plugin="mujoco.test.passive"/>
@@ -416,8 +404,8 @@ constexpr char xml[] = R"(
<config key="stride" value="4"/>
<config key="multiplier" value="0.03125"/>
</plugin>
<plugin joint="h2" instance="actuator2"/>
<plugin joint="h2" dyntype="filter" dynprm="0.9" instance="actuator2"/>
<plugin joint="h2" actdim="3" instance="actuator2"/>
<plugin joint="h2" actdim="4" dyntype="filter" dynprm="0.9" instance="actuator2"/>
</actuator>
</mujoco>
)";
@@ -640,7 +628,7 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
// Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1
// from setting dyntype="filter" on one of the plugin actuators
EXPECT_EQ(m->na, 7);
ASSERT_EQ(m->na, 7);
EXPECT_EQ(m->actuator_actnum[0], 0);
EXPECT_EQ(m->actuator_actnum[1], 3);
EXPECT_EQ(m->actuator_actnum[2], 4);
test/plugin/actuator/pid_test.cc
+119 -15
View File
@@ -14,12 +14,14 @@
// Tests for the PID controller plugin
#include <string>
#include <string_view>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/cleanup/cleanup.h>
#include <absl/strings/str_replace.h>
#include <absl/strings/string_view.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
@@ -109,7 +111,7 @@ TEST_F(PidTest, PGainWithFilterExact) {
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1"
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" actdim="1"
dyntype="filterexact" dynprm="0.1" actearly="true"/>
<general joint="j2" gainprm="4.0 0 0" biastype="affine" biasprm="0 -4.0 0"
dyntype="filterexact" dynprm="0.1" actearly="true"/>
@@ -169,7 +171,7 @@ TEST_F(PidTest, SlewMaxRate) {
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" actdim="1" />
<position joint="j2" kp="4.0" />
</actuator>
</mujoco>
@@ -235,10 +237,10 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) {
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" actdim="1"/>
<!-- make an integrated velocity controller using the PID plugin -->
<plugin joint="j2" plugin="mujoco.pid" instance="pid2"
dyntype="integrator" dynprm="1 0 0" actearly="true"/>
dyntype="integrator" dynprm="1 0 0" actearly="true" actdim="1"/>
</actuator>
</mujoco>
)";
@@ -250,9 +252,9 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) {
// having a slew rate means that there should be one extra state variable
// for the plugin.
EXPECT_EQ(m->actuator_actnum[0], 1);
ASSERT_EQ(m->actuator_actnum[0], 1);
// The integrated-velocity controller should have one activation variable too.
EXPECT_EQ(m->actuator_actnum[1], 1);
ASSERT_EQ(m->actuator_actnum[1], 1);
mjData* d = mj_makeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
@@ -299,7 +301,9 @@ TEST_F(PidTest, SlewMaxRateUsesFirstCtrl) {
</body>
</worldbody>
<actuator><plugin joint="j1" plugin="mujoco.pid" instance="pid1" /></actuator>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" actdim="1"/>
</actuator>
</mujoco>
)";
@@ -363,8 +367,8 @@ TEST_F(PidTest, ITerm) {
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid2" />
<plugin joint="j3" plugin="mujoco.pid" instance="pid3" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid2" actdim="1" />
<plugin joint="j3" plugin="mujoco.pid" instance="pid3" actdim="1" />
</actuator>
</mujoco>
)";
@@ -413,7 +417,7 @@ TEST_F(PidTest, FiniteDifferencing) {
</worldbody>
<actuator>
<plugin joint="j" plugin="mujoco.pid" instance="pid" />
<plugin joint="j" plugin="mujoco.pid" instance="pid" actdim="2" />
</actuator>
</mujoco>
)";
@@ -425,7 +429,7 @@ TEST_F(PidTest, FiniteDifferencing) {
// actuators with an I term and max slew rate should have 2 activation
// variables.
EXPECT_EQ(m->actuator_actnum[0], 2);
ASSERT_EQ(m->actuator_actnum[0], 2);
mjData* d = mj_makeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
@@ -471,7 +475,7 @@ TEST_F(PidTest, CtrlClamp) {
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid"
ctrlrange="0.25 0.75" />
ctrlrange="0.25 0.75" actdim="1" />
</actuator>
</mujoco>
)";
@@ -534,7 +538,7 @@ TEST_F(PidTest, CopyData) {
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid2" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid2" actdim="2"/>
</actuator>
</mujoco>
)";
@@ -588,8 +592,8 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) {
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid" />
<plugin joint="j1" plugin="mujoco.pid" instance="pid" actdim="1" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid" actdim="1" />
</actuator>
</mujoco>
)";
@@ -682,5 +686,105 @@ TEST_F(PidTest, InvalidSlew) {
EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
EXPECT_THAT(std::string_view(error), HasSubstr("slewmax"));
}
TEST_F(PidTest, WrongActdim) {
// XML where PLACEHOLDER is going to be replaced with various things
constexpr absl::string_view kBaseXml = R"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="4.0"/>
<config key="slewmax" value="0.75"/>
</instance>
</plugin>
</extension>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" PLACEHOLDER />
</actuator>
</mujoco>
)";
char error[1024] = {0};
{
std::string no_actdim =
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", ""}});
mjModel* m = LoadModelFromString(no_actdim, error, sizeof(error));
EXPECT_THAT(m, IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
}
{
std::string big_actdim =
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", "actdim=\"2\""}});
mjModel* m = LoadModelFromString(big_actdim, error, sizeof(error));
EXPECT_THAT(m, IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
}
{
std::string dyntype_integrator = absl::StrReplaceAll(
kBaseXml, {{"PLACEHOLDER", "dyntype=\"integrator\" actdim=\"1\""}});
mjModel* m = LoadModelFromString(dyntype_integrator, error, sizeof(error));
EXPECT_THAT(m, IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"2\""));
}
}
// Regression test: loading models with PID plugin and keyframes used to crash.
TEST_F(PidTest, Keyframe) {
constexpr absl::string_view kModelXml = R"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="4.0"/>
<config key="slewmax" value="0.75"/>
</instance>
</plugin>
</extension>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" actdim="1" />
</actuator>
<keyframe>
<key name="home" qpos="0" act="1" />
</keyframe>
</mujoco>
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// having a slew rate means that there should be one extra state variable
// for the plugin.
EXPECT_EQ(m->actuator_actnum[0], 1);
EXPECT_EQ(m->na, 1);
ASSERT_EQ(m->nkey, 1);
EXPECT_EQ(m->key_act[0], 1.0);
mjData* d = mj_makeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
mj_resetDataKeyframe(m, d, 0);
EXPECT_EQ(d->act[0], 1.0);
}
} // namespace
} // namespace mujoco