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
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