PID Controller as an actuator plugin.

Implement a configurable PID controller. PiperOrigin-RevId: 585282014 Change-Id: I2d884554629101a0a25b5e0979ffb1ff8ed9cab4
2023-11-25 08:40:13 -08:00
parent d070b1893e
commit 002b427a32
14 changed files with 1237 additions and 3 deletions
@@ -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_