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

PID Controller as an actuator plugin.

Browse Source 
Implement a configurable PID controller.

PiperOrigin-RevId: 585282014
Change-Id: I2d884554629101a0a25b5e0979ffb1ff8ed9cab4
This commit is contained in:
Nimrod Gileadi
2023-11-25 08:40:13 -08:00
committed by Copybara-Service
parent d070b1893e
commit 002b427a32
14 changed files with 1237 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/build.yml
+2
View File
@@ -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
CMakeLists.txt
+1
View File
@@ -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)
doc/changelog.rst
+5
View File
@@ -19,6 +19,11 @@ Plugins
``mjData.plugin_state``. Actuator plugins can now specify :ref:`callbacks<mjpPlugin>` that compute activation
variables, and they can be used with built-in :ref:`dyntype<actuator-plugin-dyntype>` actuator dynamics.
- Added `pid <https://github.com/deepmind/mujoco/blob/main/plugin/actuator/README.md>`__ 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.
doc/programming/extension.rst
+4 -2
View File
@@ -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/ <https://github.com/google-deepmind/mujoco/tree/main/plugin/actuator>`__
directory implement custom actuators, so far only a PID controller. See the
`README <https://github.com/google-deepmind/mujoco/blob/main/plugin/actuator/README.md>`__ for details.
* **elasticity:** The plugins in the `elasticity/
<https://github.com/google-deepmind/mujoco/tree/main/plugin/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
model/plugin/actuator/pid.xml
+68
View File
@@ -0,0 +1,68 @@
<mujoco model="PID Controllers">
<size memory="10M"/>
<visual>
<headlight diffuse=".2 .2 .2"/>
<global elevation="-30" />
</visual>
<statistic extent="0.50" />
<extension>
<plugin plugin="mujoco.pid">
<!-- pure proportional gain, bounces up and down -->
<instance name="pid1">
<config key="kp" value="4.0"/>
</instance>
<!-- PD controller - settles below setpoint -->
<instance name="pid2">
<config key="kp" value="4.0"/>
<config key="kd" value="0.4"/>
</instance>
<!-- PID controller - reaches setpoint -->
<instance name="pid3">
<config key="kp" value="4.0"/>
<config key="ki" value="4"/>
<config key="kd" value="0.4"/>
</instance>
<!-- PID controller with clamped I term. -->
<instance name="pid4">
<config key="kp" value="4.0"/>
<config key="ki" value="4"/>
<config key="kd" value="0.4"/>
<config key="imax" value=".5"/>
</instance>
</plugin>
</extension>
<default>
<geom type="sphere" size="0.01" rgba="1 1 1 1" />
</default>
<worldbody>
<light pos="0.4 -.15 .6" dir="-1 .5 -1" diffuse=".7 .7 .7"/>
<light pos="-.4 -.15 .6" dir="1 .5 -1" diffuse=".7 .7 .7"/>
<geom name="floor" type="plane" pos="0 0 -0.1" size="0 0 100" />
<body pos="-0.06 0 0">
<joint name="j1" type="slide" axis="0 0 1" />
<geom/>
</body>
<body pos="-0.02 0 0" >
<joint name="j2" type="slide" axis="0 0 1" />
<geom/>
</body>
<body pos="0.02 0 0" >
<joint name="j3" type="slide" axis="0 0 1" />
<geom/>
</body>
<body pos="0.06 0 0" >
<joint name="j4" type="slide" axis="0 0 1" />
<geom/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" ctrlrange="0 0.15" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid2" ctrlrange="0 0.15" />
<plugin joint="j3" plugin="mujoco.pid" instance="pid3" ctrlrange="0 0.15" />
<plugin joint="j4" plugin="mujoco.pid" instance="pid4" ctrlrange="0 0.15" />
</actuator>
</mujoco>
plugin/actuator/CMakeLists.txt
+41
View File
@@ -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}
)
plugin/actuator/README.md
+55
View File
@@ -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
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid">
<config key="kp" value="40.0"/>
<config key="ki" value="40"/>
<config key="kd" value="4"/>
<config key="slewmax" value="3" />
<config key="imax" value="1"/>
</instance>
</plugin>
</extension>
<worldbody>
<body>
<joint name="j" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
</worldbody>
<actuator>
<plugin joint="j" plugin="mujoco.pid" instance="pid" />
</actuator>
</mujoco>
```
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). :
plugin/actuator/pid.cc
+292
View File
@@ -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 <cstdint>
#include <cstdlib>
#include <memory>
#include <optional>
#include <utility>
#include <vector>
#include <mujoco/mujoco.h>
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<mjtNum> 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<double> 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> 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<Pid>(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<const char*> 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 = Pid::Create(m, instance);
if (pid == nullptr) {
return -1;
}
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(pid.release());
return 0;
};
plugin.destroy = +[](mjData* d, int instance) {
delete reinterpret_cast<Pid*>(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<Pid*>(plugin_data);
pid->Reset(plugin_state);
};
plugin.actuator_act_dot = +[](const mjModel* m, mjData* d, int instance) {
auto* pid = reinterpret_cast<Pid*>(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<Pid*>(d->plugin_data[instance]);
pid->Compute(m, d, instance);
};
plugin.advance = +[](const mjModel* m, mjData* d, int instance) {
auto* pid = reinterpret_cast<Pid*>(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
plugin/actuator/pid.h
+103
View File
@@ -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 <memory>
#include <optional>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
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<double> i_max;
// Maximum speed at which the setpoint can change.
std::optional<double> 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<Pid> 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_
plugin/actuator/register.cc
+22
View File
@@ -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 <mujoco/mjplugin.h>
#include "pid.h"
namespace mujoco::plugin::actuator {
mjPLUGIN_LIB_INIT { Pid::RegisterPlugin(); }
} // namespace mujoco::plugin::actuator
test/CMakeLists.txt
+1
View File
@@ -80,3 +80,4 @@ add_subdirectory(sample)
add_subdirectory(user)
add_subdirectory(xml)
add_subdirectory(plugin/elasticity)
add_subdirectory(plugin/actuator)
test/engine/engine_plugin_test.cc
+1 -1
View File
@@ -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;
test/plugin/actuator/CMakeLists.txt
+21
View File
@@ -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_FILE_DIR:actuator>"
)
target_link_libraries(pid_test fixture gmock absl::cleanup absl::strings)
test/plugin/actuator/pid_test.cc
+621
View File
@@ -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 <string_view>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/cleanup/cleanup.h>
#include <absl/strings/string_view.h>
#include <mujoco/mujoco.h>
#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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1"><config key="kp" value="4.0"/></instance>
</plugin>
</extension>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
<body pos="0.04 0 0" >
<joint name="j2" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<position joint="j2" kp="4.0" />
</actuator>
</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); };
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1"><config key="kp" value="4.0"/></instance>
</plugin>
</extension>
<option gravity="0 0 0"/>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
<body pos="0.04 0 0" >
<joint name="j2" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1"
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"/>
</actuator>
</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); };
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="4.0"/>
<config key="slewmax" value="0.75"/>
</instance>
</plugin>
</extension>
<option gravity="0 0 0" timestep="0.001"/>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
<body pos="0.04 0 0" >
<joint name="j2" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<position joint="j2" kp="4.0" />
</actuator>
</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);
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="4.0"/>
<config key="slewmax" value="0.75"/>
</instance>
<instance name="pid2">
<config key="kp" value="4.0"/>
</instance>
</plugin>
</extension>
<option gravity="0 0 0" timestep="0.001"/>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
<body pos="0.04 0 0" >
<joint name="j2" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<!-- 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"/>
</actuator>
</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);
// 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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="4.0"/>
<config key="slewmax" value="0.75"/>
</instance>
</plugin>
</extension>
<option gravity="0 0 0" timestep="0.001"/>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.01"/>
</body>
</worldbody>
<actuator><plugin joint="j1" plugin="mujoco.pid" instance="pid1" /></actuator>
</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); };
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="40.0"/>
<config key="kd" value="4"/>
</instance>
<instance name="pid2">
<config key="kp" value="40.0"/>
<config key="ki" value="40"/>
<config key="kd" value="4"/>
</instance>
<instance name="pid3">
<config key="kp" value="40.0"/>
<config key="ki" value="40"/>
<config key="kd" value="4"/>
<config key="imax" value="5"/>
</instance>
</plugin>
</extension>
<option gravity="0 0 -10" timestep="0.001"/>
<worldbody>
<body>
<joint name="j1" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
<body pos="0.04 0 0" >
<joint name="j2" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
<body pos="0.08 0 0" >
<joint name="j3" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
</worldbody>
<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" />
</actuator>
</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); };
// 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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid">
<config key="ki" value="40"/>
<config key="slewmax" value="0.75"/>
</instance>
</plugin>
</extension>
<worldbody>
<body><joint name="j"/><geom size="0.01"/></body>
</worldbody>
<actuator>
<plugin joint="j" plugin="mujoco.pid" instance="pid" />
</actuator>
</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); };
// 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<mjtNum> A((2*m->nv+m->na) * (2*m->nv+m->na), 0);
std::vector<mjtNum> B((2*m->nv+m->na) * m->nu, 0);
std::vector<mjtNum> C((m->nsensordata) * (2*m->nv+m->na), 0);
std::vector<mjtNum> 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"(
<mujoco>
<compiler autolimits="true"/>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid">
<config key="kp" value="40.0"/>
<config key="ki" value="40"/>
<config key="kd" value="4"/>
</instance>
</plugin>
</extension>
<option gravity="0 0 0" timestep="0.001"/>
<worldbody>
<body>
<joint name="j1" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid"
ctrlrange="0.25 0.75" />
</actuator>
</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); };
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="40.0"/>
<config key="kd" value="4"/>
</instance>
<instance name="pid2">
<config key="kp" value="40.0"/>
<config key="ki" value="40"/>
<config key="kd" value="4"/>
<config key="slewmax" value="3.14" />
<config key="imax" value="1"/>
</instance>
</plugin>
</extension>
<option gravity="0 0 -10" timestep="0.001"/>
<worldbody>
<body>
<joint name="j1" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
<body pos="0.04 0 0" >
<joint name="j2" type="slide" axis="0 0 1" />
<geom size="0.01" mass="1"/>
</body>
</worldbody>
<actuator>
<plugin joint="j1" plugin="mujoco.pid" instance="pid1" />
<plugin joint="j2" plugin="mujoco.pid" instance="pid2" />
</actuator>
</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); };
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="ki" value="40"/>
<!-- imax should be nonnegative -->
<config key="imax" value="-0.01"/>
</instance>
</plugin>
</extension>
<worldbody>
<body><joint name="j1" /><geom size="0.01" /></body>
</worldbody>
<actuator><plugin joint="j1" plugin="mujoco.pid" instance="pid1" /></actuator>
</mujoco>
)";
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"(
<mujoco>
<extension>
<plugin plugin="mujoco.pid">
<instance name="pid1">
<config key="kp" value="40"/>
<!-- slewmax must be nonnegative -->
<config key="slewmax" value="-0.1"/>
</instance>
</plugin>
</extension>
<worldbody>
<body><joint name="j" /><geom size="0.01" /></body>
</worldbody>
<actuator>
<plugin joint="j" plugin="mujoco.pid" instance="pid1" />
</actuator>
</mujoco>
)";
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