diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 75f7965d..08af2d39 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -164,6 +164,7 @@ jobs: if: ${{ runner.os != 'Windows' }} working-directory: build run: mkdir -p ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && + cp lib/libactuator.* ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && cp lib/libelasticity.* ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && cp lib/libsensor.* ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && cp lib/libsdf.* ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin @@ -171,6 +172,7 @@ jobs: if: ${{ runner.os == 'Windows' }} working-directory: build run: mkdir -p ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && + cp bin/Release/actuator.dll ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && cp bin/Release/elasticity.dll ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin && cp bin/Release/sensor.dll ${{ matrix.tmpdir }}/mujoco_install/mujoco_plugin - name: Configure samples diff --git a/CMakeLists.txt b/CMakeLists.txt index 7bafe59a..c9723a29 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,7 @@ target_include_directories( ) add_subdirectory(plugin/elasticity) +add_subdirectory(plugin/actuator) add_subdirectory(plugin/sensor) add_subdirectory(plugin/sdf) add_subdirectory(src/engine) diff --git a/doc/changelog.rst b/doc/changelog.rst index 70a4fb68..3f649e2a 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -19,6 +19,11 @@ Plugins ``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks` that compute activation variables, and they can be used with built-in :ref:`dyntype` actuator dynamics. +- Added `pid `__ actuator plugin: + + - A configurable PID controller that implements an integral term, which is not available with the default MuJoCo + actuators. + Bug fixes ^^^^^^^^^ - Fix bug in Cartesian actuation with movable refsite, as when using body-centric Cartesian actuators on a quadruped. diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 9a69a702..49ba77ff 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -267,8 +267,11 @@ A future version of this section will include: * Things that developers need to keep in mind in order to ensure that plugins function correctly when :ref:`mjData` is copied, stepped, or reset. -Currently, there are three directories of first-party plugins: +There are several first-party plugin directories: +* **actuator:** The plugins in the `actuator/ `__ + directory implement custom actuators, so far only a PID controller. See the + `README `__ for details. * **elasticity:** The plugins in the `elasticity/ `__ directory are passive forces based on continuum mechanics for 1-dimensional and 3-dimensional bodies. The 1D model is invariant under rotations and captures @@ -342,7 +345,6 @@ loading functions. The :ref:`mjpResourceProvider` struct stores three types of f .. _Uniform Resource Identifier: https://en.wikipedia.org/wiki/Uniform_Resource_Identifier Resource prefix - Resources are identified by prefixes in their name. The chosen prefix should have a valid `Uniform Resource Identifier`_ (URI) scheme syntax. Resource names should also have a valid URI syntax, however this isn't enforced. A resource name with the syntax ``{prefix}:{filename}`` will match a provider using the scheme ``prefix``. For diff --git a/model/plugin/actuator/pid.xml b/model/plugin/actuator/pid.xml new file mode 100644 index 00000000..357ae90a --- /dev/null +++ b/model/plugin/actuator/pid.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/plugin/actuator/CMakeLists.txt b/plugin/actuator/CMakeLists.txt new file mode 100644 index 00000000..6cc79944 --- /dev/null +++ b/plugin/actuator/CMakeLists.txt @@ -0,0 +1,41 @@ +# Copyright 2023 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +set(MUJOCO_ACTUATOR_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/../.. + ${CMAKE_CURRENT_SOURCE_DIR}/../../src +) + +set(MUJOCO_ACTUATOR_SRCS + pid.cc + pid.h + register.cc +) + +add_library(actuator SHARED) +target_sources(actuator PRIVATE ${MUJOCO_ACTUATOR_SRCS}) +target_include_directories(actuator PRIVATE ${MUJOCO_ACTUATOR_INCLUDE}) +target_link_libraries(actuator PRIVATE mujoco) +target_compile_options( + actuator + PRIVATE ${AVX_COMPILE_OPTIONS} + ${MUJOCO_MACOS_COMPILE_OPTIONS} + ${EXTRA_COMPILE_OPTIONS} + ${MUJOCO_CXX_FLAGS} +) +target_link_options( + actuator + PRIVATE + ${MUJOCO_MACOS_LINK_OPTIONS} + ${EXTRA_LINK_OPTIONS} +) diff --git a/plugin/actuator/README.md b/plugin/actuator/README.md new file mode 100644 index 00000000..534bb61e --- /dev/null +++ b/plugin/actuator/README.md @@ -0,0 +1,55 @@ +# Actuator plugins + +## PID + +The `mujoco.pid` actuator plugin implements a configurable [PID controller](https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller): + +$$f(t) = K_\text{p} e(t) + K_\text{i} \int_0^t e(\tau) \,\mathrm{d}\tau + K_\text{d} \frac{\mathrm{d}e(t)}{\mathrm{d}t},$$ +where $e(t) = u(t) - \ell(t)$ is the difference between the control $u$ and the actuator length $\ell$. + +You can use it like: + +```xml + + + + + + + + + + + + + + + + + + + + + + + + +``` + +The available options are: + +|Attribute|Default |Meaning | +|---------|--------|-------------------------------------------------------------------------------------------------------------------------------------| +|`kp` |0 |**P** gain for the controller. | +|`ki` |0 |**I** gain for the controller. | +: : : : +: : :If nonzero, one activation variable will be added to `mjData.act`, containing the current I term (in units of force). : +|`kd` |0 |**D** gain for the controller. | +|`imax` |Optional|If specified, the force produced by the I term will be clipped to the range `[-imax, -imax]`. | +|`slewmax`|Optional|The maximum rate at which the setpoint for the PID controller can change. | +: : : : +: : :If a bigger change is requested between two timesteps, it will be clipped to the range `[ctrl - slewmax * dt, ctrl + slewmax * dt]` : +: : : : +: : :If specified, one activation variable will be added to `mjData.act` containing the previous value of `ctrl`. One state variable will : +: : :be added to `mjData.plugin_state`, to indicate whether the previous value of `ctrl` should be used (0 on first step, 1 : +: : :otherwise). : diff --git a/plugin/actuator/pid.cc b/plugin/actuator/pid.cc new file mode 100644 index 00000000..6b20c0aa --- /dev/null +++ b/plugin/actuator/pid.cc @@ -0,0 +1,292 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pid.h" + +#include +#include +#include +#include +#include +#include + +#include + +namespace mujoco::plugin::actuator { +namespace { + +constexpr char kAttrPGain[] = "kp"; +constexpr char kAttrIGain[] = "ki"; +constexpr char kAttrDGain[] = "kd"; +constexpr char kAttrIMax[] = "imax"; +constexpr char kAttrSlewMax[] = "slewmax"; + +std::optional ReadOptionalDoubleAttr(const mjModel* m, int instance, + const char* attr) { + const char* value = mj_getPluginConfig(m, instance, attr); + if (value == nullptr || value[0] == '\0') { + return std::nullopt; + } + return std::strtod(value, nullptr); +} + +// returns the next act given the current act_dot, after clamping, for native +// mujoco dyntypes. +// copied from engine_forward. +mjtNum NextActivation(const mjModel* m, const mjData* d, int actuator_id, + int act_adr, mjtNum act_dot) { + mjtNum act = d->act[act_adr]; + + if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { + // exact filter integration + // act_dot(0) = (ctrl-act(0)) / tau + // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau)) + // = act(0) + act_dot(0) * tau * (1 - exp(-h / tau)) + mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id * mjNDYN]); + act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau)); + } else { + // Euler integration + act = act + act_dot * m->opt.timestep; + } + + // clamp to actrange + if (m->actuator_actlimited[actuator_id]) { + mjtNum* actrange = m->actuator_actrange + 2 * actuator_id; + act = mju_clip(act, actrange[0], actrange[1]); + } + + return act; +} + +bool HasSlew(const mjModel* m, int instance) { + return ReadOptionalDoubleAttr(m, instance, kAttrSlewMax).has_value(); +} + +} // namespace + +PidConfig PidConfig::FromModel(const mjModel* m, int instance) { + PidConfig config; + config.p_gain = ReadOptionalDoubleAttr(m, instance, kAttrPGain).value_or(0); + config.i_gain = ReadOptionalDoubleAttr(m, instance, kAttrIGain).value_or(0); + config.d_gain = ReadOptionalDoubleAttr(m, instance, kAttrDGain).value_or(0); + + // Clamps in the XML are specified in terms of maximum forces. Scale by i_gain + // to get the limits on the value of the error integral. + std::optional i_clamp_max_force = + ReadOptionalDoubleAttr(m, instance, kAttrIMax); + if (i_clamp_max_force.has_value() && config.i_gain) { + config.i_max = *i_clamp_max_force / config.i_gain; + } + + config.slew_max = ReadOptionalDoubleAttr(m, instance, kAttrSlewMax); + + return config; +} + +std::unique_ptr Pid::Create(const mjModel* m, int instance) { + PidConfig config = PidConfig::FromModel(m, instance); + + if (config.i_max.has_value() && *config.i_max < 0) { + mju_warning("negative imax"); + return nullptr; + } + + if (config.slew_max.value_or(0.0) < 0) { + mju_warning("maxslew must be non-negative"); + return nullptr; + } + + int actuator_idx = -1; + for (int i = 0; i < m->nu; i++) { + if (m->actuator_plugin[i] == instance) { + if (actuator_idx != -1) { + mju_warning("multiple actuators found for plugin instance %d", + instance); + return nullptr; + } + actuator_idx = i; + } + } + if (actuator_idx == -1) { + mju_warning("actuator not found for plugin instance %d", instance); + return nullptr; + } + return std::unique_ptr(new Pid(config, actuator_idx)); +} + +void Pid::Reset(mjtNum* plugin_state) { + integral_ = 0.0; + previous_ctrl_ = 0.0; + if (config_.slew_max.has_value()) { + plugin_state[0] = false; // previous_ctrl_exists + } +} + +mjtNum Pid::GetCtrl(const mjModel* m, const mjData* d, const State& state, + bool actearly) const { + mjtNum ctrl = 0; + if (m->actuator_dyntype[actuator_idx_] == mjDYN_NONE) { + ctrl = d->ctrl[actuator_idx_]; + // clamp ctrl + if (m->actuator_ctrllimited[actuator_idx_]) { + ctrl = mju_clip(ctrl, m->actuator_ctrlrange[2 * actuator_idx_], + m->actuator_ctrlrange[2 * actuator_idx_ + 1]); + } + } else { + // Use of act instead of ctrl, to create integrated-velocity controllers or + // to filter the controls. + int actadr = m->actuator_actadr[actuator_idx_] + + m->actuator_actnum[actuator_idx_] - 1; + if (actearly) { + ctrl = NextActivation(m, d, actuator_idx_, actadr, d->act_dot[actadr]); + } else { + ctrl = d->act[actadr]; + } + } + if (config_.slew_max.has_value() && state.previous_ctrl_exists) { + mjtNum ctrl_min = state.previous_ctrl - *config_.slew_max * m->opt.timestep; + mjtNum ctrl_max = state.previous_ctrl + *config_.slew_max * m->opt.timestep; + ctrl = mju_clip(ctrl, ctrl_min, ctrl_max); + } + return ctrl; +} + +void Pid::ActDot(const mjModel* m, mjData* d, int instance) const { + State state = GetState(m, d, instance); + mjtNum ctrl = GetCtrl(m, d, state, /*actearly=*/false); + mjtNum error = ctrl - d->actuator_length[actuator_idx_]; + + int state_idx = m->actuator_actadr[actuator_idx_]; + if (config_.i_gain) { + mjtNum integral = state.integral + error * m->opt.timestep; + if (config_.i_max.has_value()) { + integral = mju_clip(integral, -*config_.i_max, *config_.i_max); + } + d->act_dot[state_idx] = (integral - d->act[state_idx]) / m->opt.timestep; + ++state_idx; + } + if (config_.slew_max.has_value()) { + d->act_dot[state_idx] = (ctrl - d->act[state_idx]) / m->opt.timestep; + ++state_idx; + } +} + +void Pid::Compute(const mjModel* m, mjData* d, int instance) { + State state = GetState(m, d, instance); + mjtNum ctrl = GetCtrl(m, d, state, m->actuator_actearly[actuator_idx_]); + + mjtNum error = ctrl - d->actuator_length[actuator_idx_]; + + mjtNum ctrl_dot = m->actuator_dyntype[actuator_idx_] == mjDYN_NONE + ? 0 + : d->act_dot[m->actuator_actadr[actuator_idx_] + + m->actuator_actnum[actuator_idx_] - 1]; + mjtNum error_dot = ctrl_dot - d->actuator_velocity[actuator_idx_]; + + if (config_.i_gain) { + integral_ = state.integral + error * m->opt.timestep; + if (config_.i_max.has_value()) { + integral_ = mju_clip(integral_, -*config_.i_max, *config_.i_max); + } + } + + d->actuator_force[actuator_idx_] = config_.p_gain * error + + config_.d_gain * error_dot + + config_.i_gain * integral_; + previous_ctrl_ = ctrl; +} + +void Pid::Advance(const mjModel* m, mjData* d, int instance) const { + if (config_.slew_max.has_value()) { + // previous_ctrl_exists = true + d->plugin_state[m->plugin_stateadr[instance]] = true; + } + // act variables already updated by MuJoCo integrating act_dot +} + +int Pid::StateSize(const mjModel* m, int instance) { + return HasSlew(m, instance) ? 1 : 0; +} + +int Pid::ActDim(const mjModel* m, int instance, int actuator_id) { + double i_gain = ReadOptionalDoubleAttr(m, instance, kAttrIGain).value_or(0); + return (i_gain ? 1 : 0) + (HasSlew(m, instance) ? 1 : 0); +} + +Pid::State Pid::GetState(const mjModel* m, mjData* d, int instance) const { + State state; + int state_idx = m->actuator_actadr[instance]; + if (config_.i_gain) { + state.integral = d->act[state_idx++]; + } + if (config_.slew_max.has_value()) { + state.previous_ctrl = d->act[state_idx++]; + state.previous_ctrl_exists = d->plugin_state[m->plugin_stateadr[instance]]; + } + return state; +} + +void Pid::RegisterPlugin() { + mjpPlugin plugin; + mjp_defaultPlugin(&plugin); + plugin.name = "mujoco.pid"; + plugin.capabilityflags |= mjPLUGIN_ACTUATOR; + + std::vector attributes = {kAttrPGain, kAttrIGain, kAttrDGain, + kAttrIMax, kAttrSlewMax}; + plugin.nattribute = attributes.size(); + plugin.attributes = attributes.data(); + + plugin.actuator_actdim = Pid::ActDim; + plugin.nstate = Pid::StateSize; + + plugin.init = +[](const mjModel* m, mjData* d, int instance) { + std::unique_ptr pid = Pid::Create(m, instance); + if (pid == nullptr) { + return -1; + } + d->plugin_data[instance] = reinterpret_cast(pid.release()); + return 0; + }; + plugin.destroy = +[](mjData* d, int instance) { + delete reinterpret_cast(d->plugin_data[instance]); + d->plugin_data[instance] = 0; + }; + plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + int instance) { + auto* pid = reinterpret_cast(plugin_data); + pid->Reset(plugin_state); + }; + plugin.actuator_act_dot = +[](const mjModel* m, mjData* d, int instance) { + auto* pid = reinterpret_cast(d->plugin_data[instance]); + pid->ActDot(m, d, instance); + }; + plugin.compute = + +[](const mjModel* m, mjData* d, int instance, int capability_bit) { + auto* pid = reinterpret_cast(d->plugin_data[instance]); + pid->Compute(m, d, instance); + }; + plugin.advance = +[](const mjModel* m, mjData* d, int instance) { + auto* pid = reinterpret_cast(d->plugin_data[instance]); + pid->Advance(m, d, instance); + }; + // TODO: b/303823996 - allow actuator plugins to compute their derivatives wrt + // qvel, for implicit integration + mjp_registerPlugin(&plugin); +} + +Pid::Pid(PidConfig config, int actuator_idx) + : config_(std::move(config)), actuator_idx_(actuator_idx) {} + +} // namespace mujoco::plugin::actuator diff --git a/plugin/actuator/pid.h b/plugin/actuator/pid.h new file mode 100644 index 00000000..1e2c5f24 --- /dev/null +++ b/plugin/actuator/pid.h @@ -0,0 +1,103 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PLUGIN_ACTUATOR_PID_H_ +#define MUJOCO_PLUGIN_ACTUATOR_PID_H_ + +#include +#include + +#include +#include +#include + +namespace mujoco::plugin::actuator { + +struct PidConfig { + double p_gain = 0.0; + double i_gain = 0.0; + double d_gain = 0.0; + + // Maximum value of the error integral. + // NOTE: In the XML definition, the clamp values are specified as limits on + // the *force* value, so are scaled by i_gain. + std::optional i_max; + + // Maximum speed at which the setpoint can change. + std::optional slew_max; + + // Reads plugin attributes to construct PID configuration. + static PidConfig FromModel(const mjModel* m, int instance); +}; + +// An actuator plugin which implements configurable PID control. +class Pid { + public: + // Returns an instance of Pid. The result can be null in case of + // misconfiguration. + static std::unique_ptr Create(const mjModel* m, int instance); + + // Returns the number of state variables for the plugin instance + static int StateSize(const mjModel* m, int instance); + + // Returns the number of activation variables for the plugin instance + static int ActDim(const mjModel* m, int instance, int actuator_id); + + // Resets the C++ Pid instance's state. + // plugin_state is a C array pointer into mjData->plugin_state, with a size + // equal to the value returned from StateSize. + void Reset(mjtNum* plugin_state); + + // Computes the rate of change for activation variables + void ActDot(const mjModel* m, mjData* d, int instance) const; + + // Idempotent computation which updates d->actuator_force and the internal + // state of the class. Called after ActDot. + void Compute(const mjModel* m, mjData* d, int instance); + + // Updates plugin state. + void Advance(const mjModel* m, mjData* d, int instance) const; + + // Adds the PID plugin to the global registry of MuJoCo plugins. + static void RegisterPlugin(); + + private: + Pid(PidConfig config, int actuator_idx); + + struct State { + mjtNum previous_ctrl = 0; + // if using slew rate limits, mjData.act will contain an activation variable + // with the last ctrl value. If `false`, that value should be ignored, + // because it hasn't been set yet. + bool previous_ctrl_exists = false; + 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; + + // Returns the PID setpoint, which is normally d->ctrl, but can be d->act for + // actuators with dyntype != none. + mjtNum GetCtrl(const mjModel* m, const mjData* d, const State& state, + bool actearly) const; + + PidConfig config_; + int actuator_idx_ = 0; + + mjtNum integral_ = 0.0; + mjtNum previous_ctrl_ = 0.0; +}; + +} // namespace mujoco::plugin::actuator + +#endif // MUJOCO_PLUGIN_ACTUATOR_PID_H_ diff --git a/plugin/actuator/register.cc b/plugin/actuator/register.cc new file mode 100644 index 00000000..66f24b93 --- /dev/null +++ b/plugin/actuator/register.cc @@ -0,0 +1,22 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "pid.h" + +namespace mujoco::plugin::actuator { + +mjPLUGIN_LIB_INIT { Pid::RegisterPlugin(); } + +} // namespace mujoco::plugin::actuator diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6bec9110..3c70fce1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -80,3 +80,4 @@ add_subdirectory(sample) add_subdirectory(user) add_subdirectory(xml) add_subdirectory(plugin/elasticity) +add_subdirectory(plugin/actuator) diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index 42fa9aeb..fce99165 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -36,7 +36,7 @@ using ::testing::DoubleNear; using ::testing::HasSubstr; using ::testing::NotNull; -constexpr int kNumTruePlugins = 11; +constexpr int kNumTruePlugins = 12; constexpr int kNumFakePlugins = 30; constexpr int kNumTestPlugins = 3; diff --git a/test/plugin/actuator/CMakeLists.txt b/test/plugin/actuator/CMakeLists.txt new file mode 100644 index 00000000..ac795625 --- /dev/null +++ b/test/plugin/actuator/CMakeLists.txt @@ -0,0 +1,21 @@ +# Copyright 2023 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +mujoco_test( + pid_test + PROPERTIES + ENVIRONMENT + "MUJOCO_PLUGIN_DIR=$" +) +target_link_libraries(pid_test fixture gmock absl::cleanup absl::strings) diff --git a/test/plugin/actuator/pid_test.cc b/test/plugin/actuator/pid_test.cc new file mode 100644 index 00000000..5e13c439 --- /dev/null +++ b/test/plugin/actuator/pid_test.cc @@ -0,0 +1,621 @@ +// Copyright 2023 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for the PID controller plugin + +#include +#include + +#include +#include +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using PidTest = PluginTest; +using ::testing::DoubleNear; +using ::testing::HasSubstr; +using ::testing::IsNull; +using ::testing::NotNull; + +TEST_F(PidTest, PGain) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + EXPECT_EQ(m->plugin_statenum[0], 0) + << "Plugin should have no state variables"; + EXPECT_EQ(m->actuator_actnum[0], 0) + << "Plugin should have no activation variables"; + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // apply the same ctrl to both actuators and see if the same qpos results + d->ctrl[0] = 1.0; + d->ctrl[1] = 1.0; + + mj_step(m, d); + + EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]); + EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]); + EXPECT_EQ(d->qpos[0], d->qpos[1]); +} + +TEST_F(PidTest, PGainWithFilterExact) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + EXPECT_EQ(m->plugin_statenum[0], 0) + << "Plugin should have no state variables"; + EXPECT_EQ(m->actuator_actnum[0], 1) + << "Plugin should have one activation variable"; + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // apply the same ctrl to both actuators and see if the same qpos results + d->ctrl[0] = 1.0; + d->ctrl[1] = 1.0; + + for (int i = 0; i < 2; i++) { + mj_step(m, d); + + EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]); + EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]); + EXPECT_EQ(d->qpos[0], d->qpos[1]); + } +} + +TEST_F(PidTest, SlewMaxRate) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + )"; + + 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); + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to slowly + // incrementing the setpoint + d->ctrl[0] = d->ctrl[1] = 0.0; + mj_step(m, d); + + mjtNum max_slew_rate = 0.75; + for (int i = 0; i < 2; i++) { + d->ctrl[0] = 1.0; + d->ctrl[1] = d->time * max_slew_rate; + mj_step(m, d); + + EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]) + << "actuator_force mismatch at step " << i; + EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]) + << "qfrc_actuator mismatch at step " << i; + EXPECT_EQ(d->qpos[0], d->qpos[1]) << "qpos mismatch at step " << i; + } +} + +TEST_F(PidTest, IntegratedVelocitySlewMaxRate) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + + )"; + + 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); + // The integrated-velocity controller should have one activation variable too. + EXPECT_EQ(m->actuator_actnum[1], 1); + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to using + // an integrated-velocity controller, with actearly = true + d->ctrl[0] = d->ctrl[1] = 0.0; + mj_step(m, d); + + mjtNum max_slew_rate = 0.75; + for (int i = 0; i < 2; i++) { + d->ctrl[0] = 1.0; + d->ctrl[1] = max_slew_rate; + mj_step(m, d); + + EXPECT_EQ(d->act[0], d->act[1]) + << "act mismatch at step " << i; + EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]) + << "actuator_force mismatch at step " << i; + EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]) + << "qfrc_actuator mismatch at step " << i; + EXPECT_EQ(d->qpos[0], d->qpos[1]) << "qpos mismatch at step " << i; + } +} + +TEST_F(PidTest, SlewMaxRateUsesFirstCtrl) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // When starting with ctrl = 1.0, there shouldn't be a slew rate restriction + d->ctrl[0] = 1.0; + mj_forward(m, d); + EXPECT_EQ(d->actuator_force[0], 4.0); +} + +TEST_F(PidTest, ITerm) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + // only the actuators with an I term should have state. + EXPECT_EQ(m->actuator_actnum[0], 0); + EXPECT_EQ(m->actuator_actnum[1], 1); + EXPECT_EQ(m->actuator_actnum[2], 1); + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // when applying a constant 1.0 control for a while: + // - the PD controller should settle to 1.0 - m*g / kp + // - the unclamped PID controller should reach 1.0 + // - the clamped PID controller should reach 1.0 - (m*g - imax) / kp + d->ctrl[0] = d->ctrl[1] = d->ctrl[2] = 1.0; + for (int i = 0; i < 10000; i++) { + mj_step(m, d); + } + + EXPECT_THAT(d->qpos[0], DoubleNear(1.0 - 10 / 40.0, 1e-5)); + EXPECT_THAT(d->qpos[1], DoubleNear(1.0, 1e-5)); + EXPECT_THAT(d->qpos[2], DoubleNear(1.0 - (10 - 0.125 * 40.0) / 40.0, 1e-5)); +} + +TEST_F(PidTest, FiniteDifferencing) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + // actuators with an I term and max slew rate should have 2 activation + // variables. + EXPECT_EQ(m->actuator_actnum[0], 2); + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + d->ctrl[0] = 1.0; + mj_step(m, d); + EXPECT_NE(d->act[0], 0); + mjtNum state_before_finite_differencing = d->act[0]; + + std::vector A((2*m->nv+m->na) * (2*m->nv+m->na), 0); + std::vector B((2*m->nv+m->na) * m->nu, 0); + std::vector C((m->nsensordata) * (2*m->nv+m->na), 0); + std::vector D((m->nsensordata) * m->nu, 0); + mjd_transitionFD(m, d, /*eps=*/1e-3, /*flg_centered=*/true, A.data(), + B.data(), C.data(), D.data()); + + EXPECT_EQ(d->act[0], state_before_finite_differencing) + << "mjd_transitionFD should not change actuator plugin state."; +} + +TEST_F(PidTest, CtrlClamp) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + mjData* d = mj_makeData(m); + absl::Cleanup d_deleter = [d] { mj_deleteData(d); }; + + // when applying a constant 1.0 control for a while, control should be clamped + // to 0.75, and the body should reach that point. + d->ctrl[0] = 1.0; + for (int i = 0; i < 10000; i++) { + mj_step(m, d); + } + EXPECT_THAT(d->qpos[0], DoubleNear(0.75, 1e-5)); + + // when applying 0, it should be clamped to 0.25 + d->ctrl[0] = 0.0; + for (int i = 0; i < 10000; i++) { + mj_step(m, d); + } + EXPECT_THAT(d->qpos[0], DoubleNear(0.25, 1e-5)); +} + +TEST_F(PidTest, CopyData) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + absl::Cleanup m_deleter = [m] { mj_deleteModel(m); }; + + mjData* d1 = mj_makeData(m); + absl::Cleanup d1_deleter = [d1] { mj_deleteData(d1); }; + + d1->ctrl[0] = d1->ctrl[1] = -1.0; + + for (int i = 0; i < 3; i++) { + mj_step(m, d1); + } + mjData* d2 = mj_copyData(nullptr, m, d1); + absl::Cleanup d2_deleter = [d2] { mj_deleteData(d2); }; + + mj_step(m, d1); + mj_step(m, d2); + + EXPECT_EQ(d1->qpos[0], d2->qpos[0]); + EXPECT_EQ(d1->qpos[1], d2->qpos[1]); +} + +TEST_F(PidTest, InvalidClamp) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + EXPECT_THAT(m, IsNull()); + + // TODO: b/303654852 - ensure that the compilation error includes "imax" + EXPECT_THAT(std::string_view(error), HasSubstr("plugin")); +} + +TEST_F(PidTest, InvalidSlew) { + constexpr absl::string_view kModelXml = R"( + + + + + + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); + ASSERT_THAT(m, IsNull()); + + // TODO: b/303654852 - ensure that the compilation error includes "slewmax" + EXPECT_THAT(std::string_view(error), HasSubstr("plugin")); +} +} // namespace +} // namespace mujoco