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

Redesign the dcmotor controller: setpoint inputs, torque-space gains.

Browse Source 
The dcmotor input block is any subset of the canonical list [pos, vel,
ff, voltage], selected with input="pos vel ff voltage" and recorded as
mjtCtrlInput bits in actuator_ctrlspec like pid. Tokens are required in
canonical order: the attribute denotes a set, the block always packs
canonically, and accepting permutations invites reading the string as a
layout choice. The mode flag in gainprm[8] is retired (reserved,
written 0).

Controller gains are now in torque space, as for pid: the controller
commands tau = kp*(q*-l) + kd*(v*-ldot) + ki*x_I + tau_ff over the
present inputs (absent setpoints frozen at zero) and converts to drive
voltage V = R/K * tau + K*ldot. The second term compensates back-EMF,
as the current loop of a real torque-mode driver does (torque commands
are current commands): commanded torque is delivered exactly until a
limit binds, and the torque-speed envelope emerges from the Vmax clamp.
The map uses the nameplate R: thermal resistance growth is not
compensated, so a hot motor under-delivers by R/R(T). A stateless
setpoint dcmotor now matches <pid> exactly, for any K and R; the old
back-EMF droop remains available as the physical behavior of the raw
voltage path. Voltage-space datasheet gains convert by K/R. Controller
inputs require a positive motor constant (the map divides by K), and
controller gains require a controller input.

ff and voltage are distinct inputs, different in kind: ff is a torque
feedforward added to the controller output, uniform with pid's ff
(feedforward in the actuator's output space), while voltage is the raw
terminal voltage of the physical device, injected downstream of the
controller and its Vmax clamp, unclamped (ctrlrange bounds it if
desired). input="voltage" is the default: the plain voltage-commanded
motor, whose behavior is unchanged by this commit. The integrator
always accumulates position error; the old velocity mode's integral
term, ki*(int(u)dt - theta), which tracked the integral of the velocity
command, is retired without replacement, keeping ki mode-independent --
commanded integrated velocity belongs to an integrator activation
state, not to controller gains. slewmax rate-limits the first controller
input -- position setpoint (rad/s), velocity setpoint (rad/s^2) or torque
feedforward (N*m/s), each a real driver feature (reference ramping,
ramped-velocity and ramped-torque input modes); the raw voltage input
is never rate-limited and slewmax requires a controller input.

input="none" selects the empty signature: the actuator owns no controls
at all (nu = 0 is now legal with actuators present) and is purely
passive -- LuGre friction, cogging and back-EMF braking as passive
joint forces. This exists because auxiliary dynamic states (the LuGre
bristle) attach to actuators, not joints. The terminal voltage is
identically zero, i.e. a shorted motor (dynamic braking); motorconst=0
decouples the electrical branch. mjINPUT_NONE is a distinct enum value
because ctrlspec = 0 means "unset, use the type default". History and
delay require an input; the controller voltage override and input read
in mj_fwdActuation are gated on a nonempty block.

The analytic velocity derivative of the controller becomes
dV/dw = -kd*R/K + K, whose second term cancels the back-EMF bias
exactly: the net damping of an unclipped torque-mode motor is -kd, and
of a voltage-mode or passive motor -K^2/R. Viewers label inputs via
mj_actuatorInputName: pos, vel, ff, voltage.

The dcmotor LaTeX design doc is updated accordingly: torque-space
units, the tau->V map and its saturation-generated envelope, the
input-block pipeline figure, and a Passive Operation section.

PiperOrigin-RevId: 965795351
Change-Id: Ibc308ca21bd6bad014e77f950ee08feaad449b73
This commit is contained in:
Yuval Tassa
2026-08-17 00:42:47 -07:00
committed by Copybara-Service
parent 11fa4a5b45
commit 2f1843f4a7
29 changed files with 574 additions and 350 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+40 -19
View File
@@ -5691,7 +5691,7 @@ specify them independently.
.. _actuator-general-dyntype: .. _actuator-general-dyntype:
:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, pid, muscle, user], "none"` :at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, pid, dcmotor, muscle, user], "none"`
Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation
model <geActuation>` section. Repeating that description in somewhat different notation (corresponding to the mjModel model <geActuation>` section. Repeating that description in somewhat different notation (corresponding to the mjModel
and mjData fields involved) we have: and mjData fields involved) we have:
@@ -5711,7 +5711,7 @@ specify them independently.
.. _actuator-general-gaintype: .. _actuator-general-gaintype:
:at:`gaintype`: :at-val:`[fixed, affine, muscle, pid, so3, user], "fixed"` :at:`gaintype`: :at-val:`[fixed, affine, muscle, dcmotor, pid, so3, user], "fixed"`
The gain and bias together determine the output of the force generation mechanism, which is currently assumed to be The gain and bias together determine the output of the force generation mechanism, which is currently assumed to be
affine. As already explained in :ref:`Actuation model <geActuation>`, the general formula is: affine. As already explained in :ref:`Actuation model <geActuation>`, the general formula is:
scalar_force = gain_term \* (act or ctrl) + bias_term. scalar_force = gain_term \* (act or ctrl) + bias_term.
@@ -5724,6 +5724,7 @@ specify them independently.
fixed gain_term = gainprm[0] fixed gain_term = gainprm[0]
affine gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity affine gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity
muscle gain_term = mju_muscleGain(...) muscle gain_term = mju_muscleGain(...)
dcmotor DC motor gain (K or K/R), see :ref:`dcmotor<actuator-dcmotor>`
pid PID controller with setpoint inputs, see :ref:`pid<actuator-pid>` pid PID controller with setpoint inputs, see :ref:`pid<actuator-pid>`
so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation<actuator-orientation>` so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation<actuator-orientation>`
user gain_term = mjcb_act_gain(...) user gain_term = mjcb_act_gain(...)
@@ -5731,7 +5732,7 @@ specify them independently.
.. _actuator-general-biastype: .. _actuator-general-biastype:
:at:`biastype`: :at-val:`[none, affine, muscle, so3, user], "none"` :at:`biastype`: :at-val:`[none, affine, muscle, dcmotor, so3, user], "none"`
The keywords have the following meaning: The keywords have the following meaning:
======= ================================================================ ======= ================================================================
@@ -5740,6 +5741,7 @@ specify them independently.
none bias_term = 0 none bias_term = 0
affine bias_term = biasprm[0] + biasprm[1]*length + biasprm[2]*velocity affine bias_term = biasprm[0] + biasprm[1]*length + biasprm[2]*velocity
muscle bias_term = mju_muscleBias(...) muscle bias_term = mju_muscleBias(...)
dcmotor DC motor bias: back-EMF, cogging, LuGre friction, see :ref:`dcmotor<actuator-dcmotor>`
so3 damping term of the geodesic orientation servo, see :ref:`orientation<actuator-orientation>` so3 damping term of the geodesic orientation servo, see :ref:`orientation<actuator-orientation>`
user bias_term = mjcb_act_bias(...) user bias_term = mjcb_act_bias(...)
======= ================================================================ ======= ================================================================
@@ -5784,8 +5786,9 @@ specify them independently.
:at:`input`: :at-val:`string, optional` :at:`input`: :at-val:`string, optional`
Input signature of the actuator: which controls make up its control block, recorded in Input signature of the actuator: which controls make up its control block, recorded in
``mjModel.actuator_ctrlspec``. For gaintype "so3" it selects the orientation chart: "expmap" (3 controls, the ``mjModel.actuator_ctrlspec``. For gaintype "so3" it selects the orientation chart: "expmap" (3 controls, the
default) or "quat" (4 controls); see :ref:`orientation/input<actuator-orientation-input>`. For gaintype "pid" it is default) or "quat" (4 controls); see :ref:`orientation/input<actuator-orientation-input>`. For gaintypes "pid" and
a token list selecting the input subset; see :ref:`pid/input<actuator-pid-input>`. "dcmotor" it is a token list selecting the input subset; see :ref:`pid/input<actuator-pid-input>` and
:ref:`dcmotor/input<actuator-dcmotor-input>`.
.. _actuator-general-actearly: .. _actuator-general-actearly:
@@ -6806,8 +6809,11 @@ the stateless case.
- :ref:`resistance<actuator-dcmotor-resistance>`, :ref:`motorconst<actuator-dcmotor-motorconst>` and - :ref:`resistance<actuator-dcmotor-resistance>`, :ref:`motorconst<actuator-dcmotor-motorconst>` and
:ref:`nominal<actuator-dcmotor-nominal>` are each optional, but some combination of them is required. :ref:`nominal<actuator-dcmotor-nominal>` are each optional, but some combination of them is required.
See Section 2.1 of the `technical note <_static/dcmotor.pdf>`__. See Section 2.1 of the `technical note <_static/dcmotor.pdf>`__.
- The control :ref:`input<actuator-dcmotor-input>` semantic is either the voltage applied to the motor terminals (the - The control block is selected by :ref:`input<actuator-dcmotor-input>`: any subset of ``[pos, vel, ff]``, where
default), or a position or velocity target for a :ref:`PID controller<actuator-dcmotor-controller>`. ``pos`` and ``vel`` are setpoint inputs to the on-board :ref:`PID controller<actuator-dcmotor-controller>` and
``ff`` is a torque feedforward added to its output; the ``voltage`` input is the raw terminal voltage. The
default is the plain voltage-commanded motor. With
``input="none"`` the actuator has no control inputs at all and acts as a purely passive device.
- Optional features include electrical dynamics (:ref:`inductance<actuator-dcmotor-inductance>`), - Optional features include electrical dynamics (:ref:`inductance<actuator-dcmotor-inductance>`),
:ref:`cogging torque<actuator-dcmotor-cogging>`, :ref:`thermal resistance variation<actuator-dcmotor-thermal>`, and :ref:`cogging torque<actuator-dcmotor-cogging>`, :ref:`thermal resistance variation<actuator-dcmotor-thermal>`, and
:ref:`LuGre<actuator-dcmotor-lugre>` friction. :ref:`LuGre<actuator-dcmotor-lugre>` friction.
@@ -6946,22 +6952,37 @@ This element has the following custom attributes in addition to the common attri
.. _actuator-dcmotor-input: .. _actuator-dcmotor-input:
:at:`input`: :at-val:`[voltage, position, velocity], "voltage"` :at:`input`: :at-val:`string, "voltage"`
Specifies the input signal semantics. In "voltage" mode, the control directly sets applied motor voltage. In Input signature: a space-separated subset of the tokens "pos", "vel", "ff" and "voltage", required in this
"position" or "velocity" modes, the :ref:`PID controller<actuator-dcmotor-controller>` uses the control as a canonical order. The ``pos`` and ``vel`` inputs are setpoints for the on-board
reference setpoint relative to the joint trajectory. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) :ref:`controller<actuator-dcmotor-controller>`, and ``ff`` is a torque feedforward added to its output, as for
:ref:`pid/input<actuator-pid-input>`. The ``voltage`` input is different in kind: it is the raw terminal voltage
of the physical device, applied downstream of the controller and its :at-val:`Vmax` clamp.
``input="voltage"`` (the default) is the plain voltage-commanded motor. Absent setpoint inputs are fixed at zero.
The keyword "none" selects the empty signature: the actuator has no control inputs and is purely
passive, useful for modeling :ref:`friction<actuator-dcmotor-lugre>` and :ref:`cogging<actuator-dcmotor-cogging>`
as passive joint forces. The terminal voltage is zero, so back-EMF drives current through the (shorted) motor and
brakes the joint; setting :ref:`motorconst<actuator-dcmotor-motorconst>` to zero disables the electrical branch.
(see `tech note <_static/dcmotor.pdf>`__, Section 2.5)
.. _actuator-dcmotor-controller: .. _actuator-dcmotor-controller:
:at:`controller`: :at-val:`real(6), "0 0 0 0 0 0"` :at:`controller`: :at-val:`real(6), "0 0 0 0 0 0"`
PID controller parameters, defined as :at:`controller` = ":at-val:`kp` :at-val:`ki` :at-val:`kd` PID controller parameters, defined as :at:`controller` = ":at-val:`kp` :at-val:`ki` :at-val:`kd`
:at-val:`slewmax` :at-val:`Imax` :at-val:`Vmax`". Depending on the :at:`input` mode, the controller stabilizes :at-val:`slewmax` :at-val:`Imax` :at-val:`Vmax`". The gains are in torque space, as for
either position or velocity. If the :at:`input` mode is voltage, :at-val:`kp`, :at-val:`ki`, :at-val:`kd` are :ref:`pid<actuator-pid>`: the controller commands the torque
ignored. :at-val:`Vmax` sets the maximum drive voltage :math:`v_{\max}` (Volt); in position/velocity modes it clamps :math:`\tau = k_p (u_{pos} - l) + k_d (u_{vel} - \dot{l}) + k_i x_I + u_{f\!f}` over the inputs present in the
the controller output, in voltage mode it clamps the control signal (if :at:`ctrlrange` is also set, the tighter :ref:`input<actuator-dcmotor-input>` signature, absent setpoints being fixed at zero, and drives the voltage
limit wins). A value of 0 (the default) disables the respective feature. When positive, :at-val:`slewmax` limits the :math:`v = (R/K)\,\tau + K \dot{l}`, the second term compensating back-EMF as in a current-controlled driver:
setpoint rate-of-change, :at-val:`Imax` clamps the integrator state (anti-windup), and :at-val:`Vmax` clamps the commanded torque is delivered exactly until a limit is reached. Torque-space gains from datasheet voltage-space
drive voltage. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) values are obtained by multiplying by :math:`K/R`. The integrator state
:math:`x_I` accumulates position error and requires the ``pos`` input; controller gains require a controller
input and a positive :ref:`motorconst<actuator-dcmotor-motorconst>`.
A value of 0 (the default) disables the respective feature. When positive, :at-val:`slewmax` limits the
rate-of-change of the first input (position setpoint in rad/s, or with signatures lacking ``pos``, velocity
setpoint or torque feedforward), :at-val:`Imax` clamps the integrator state (anti-windup), and :at-val:`Vmax`
clamps the drive voltage :math:`v_{\max}` (Volt), upstream of the raw ``voltage`` input.
(see `tech note <_static/dcmotor.pdf>`__, Section 2.5)
.. _actuator-plugin: .. _actuator-plugin:
@@ -6982,7 +7003,7 @@ Associate this actuator with an :ref:`engine plugin<exPlugin>`. Either :at:`plug
.. _actuator-plugin-dyntype: .. _actuator-plugin-dyntype:
:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, pid, muscle, user], "none"` :at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, pid, dcmotor, muscle, user], "none"`
Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation
model <geActuation>` section. If :ref:`dyntype<actuator-general-dyntype>` is not "none", an activation variable will model <geActuation>` section. If :ref:`dyntype<actuator-general-dyntype>` is not "none", an activation variable will
be added to the actuator. This variable will be added after any activation state computed by the plugin (see be added to the actuator. This variable will be added after any activation state computed by the plugin (see
doc/_static/dcmotor.pdf
Vendored
BIN
View File
Binary file not shown.
doc/changelog.rst
+22
View File
@@ -31,6 +31,28 @@ Actuation
identical to :ref:`position<actuator-position>`. The input signature is any subset of ``[pos, vel, ff]``, selected identical to :ref:`position<actuator-position>`. The input signature is any subset of ``[pos, vel, ff]``, selected
by :ref:`input<actuator-pid-input>`; absent setpoint inputs are fixed at zero, so the control vector contains no by :ref:`input<actuator-pid-input>`; absent setpoint inputs are fixed at zero, so the control vector contains no
inert entries. inert entries.
- The :ref:`dcmotor<actuator-dcmotor>` on-board controller is redesigned: the
:ref:`input<actuator-dcmotor-input>` attribute selects any subset of ``[pos, vel, ff, voltage]``, where ``pos``
and ``vel`` are setpoints for the controller, ``ff`` is a torque feedforward, and ``voltage`` is the raw terminal
voltage (the default, a plain voltage-commanded motor). Controller gains are in torque space, as for
:ref:`pid<actuator-pid>`, and the drive voltage compensates back-EMF as in a current-controlled driver: commanded
torque is delivered exactly until a limit is reached. The keyword ``input="none"`` selects the empty signature:
the actuator has no control inputs and is purely passive, so friction, cogging and back-EMF braking can be used
as passive joint forces.
.. admonition:: Breaking API changes
:class: attention
- The mode-flag semantics of :ref:`dcmotor/input<actuator-dcmotor-input>` ("voltage", "position", "velocity",
selecting the interpretation of a single control) are replaced by input signatures, and the controller gains
changed from voltage space to torque space. The old velocity mode's integral term (integrated-velocity
tracking) is retired without replacement; the integrator always accumulates position error.
**Migration:** Voltage-commanded motors (the default) are unchanged. Replace ``input="position"`` with
``input="pos"`` and ``input="velocity"`` with ``input="vel"``, and multiply the controller gains by
:math:`K/R` (torque per volt). The motor's back-EMF damping, previously felt in addition to the controller's
damping, is now compensated: to preserve behavior when the velocity setpoint is zero, add :math:`K^2/R` to the
converted :at-val:`kd`.
Engine Engine
^^^^^^ ^^^^^^
doc/dcmotor/dcmotor.tex
+69 -112
View File
@@ -543,6 +543,7 @@ where $t_T = R_T C$ is the thermal time constant. This produces exponential rise
\begin{figure}[ht] \begin{figure}[ht]
\centering \centering
\resizebox{\columnwidth}{!}{%
\begin{tikzpicture} \begin{tikzpicture}
\pgfmathsetmacro{\Tss}{1.0} \pgfmathsetmacro{\Tss}{1.0}
\pgfmathsetmacro{\ttau}{1.0} \pgfmathsetmacro{\ttau}{1.0}
@@ -568,7 +569,7 @@ where $t_T = R_T C$ is the thermal time constant. This produces exponential rise
\node[font=\scriptsize, anchor=south] at (axis cs:\xmax*0.5, \Tss) \node[font=\scriptsize, anchor=south] at (axis cs:\xmax*0.5, \Tss)
{$T_{ss} = T_a + R_T P$}; {$T_{ss} = T_a + R_T P$};
\end{axis} \end{axis}
\end{tikzpicture} \end{tikzpicture}}%
\caption{Temperature rise under constant power dissipation $P$. \caption{Temperature rise under constant power dissipation $P$.
At $t = t_T$, it reaches $(1-1/e) \approx 63\%$ of its steady-state value.} At $t = t_T$, it reaches $(1-1/e) \approx 63\%$ of its steady-state value.}
\label{fig:thermal_response} \label{fig:thermal_response}
@@ -810,7 +811,7 @@ Attribute & Size & Description \\
\texttt{damping} & 3 & Viscous damping coefficients \\ \texttt{damping} & 3 & Viscous damping coefficients \\
\texttt{armature} & 1 & Armature inertia \\ \texttt{armature} & 1 & Armature inertia \\
\midrule \midrule
\texttt{input} & keyword & Mode (voltage/position/velocity) \\ \texttt{input} & string & Input block: subset of \texttt{pos vel ff voltage}, or \texttt{none} \\
\texttt{controller} & 6 & Gains, slew, and voltage saturation ($k_p, k_i, k_d, s, I_{\max}, v_{\max}$) \\ \texttt{controller} & 6 & Gains, slew, and voltage saturation ($k_p, k_i, k_d, s, I_{\max}, v_{\max}$) \\
\bottomrule \bottomrule
\end{tabular} \end{tabular}
@@ -1066,25 +1067,26 @@ Many actuators embed an on-board controller computing drive voltage from positio
\begin{table}[H] \begin{table}[H]
\centering \centering
\small \footnotesize
\begin{tabular}{@{}lll@{}} \begin{tabular}{@{}llp{4.2cm}@{}}
\toprule \toprule
Attribute & Type & Description \\ Attribute & Type & Description \\
\midrule \midrule
\texttt{input} & keyword & \texttt{voltage}, \texttt{position}, \texttt{velocity} \\ \texttt{input} & string & subset of \texttt{pos vel ff voltage}, in canonical order; \texttt{none} (passive) \\
\texttt{controller} & vector & Gains (mode-dependent) \\ \texttt{controller} & vector & Gains, slew, and voltage saturation ($k_p, k_i, k_d, s, I_{\max}, v_{\max}$) \\
\bottomrule \bottomrule
\end{tabular} \end{tabular}
\caption{Controller attributes. Default \texttt{input} is \texttt{voltage}.} \caption{Controller attributes. Default \texttt{input} is \texttt{voltage}.}
\label{tab:controller_attributes} \label{tab:controller_attributes}
\end{table} \end{table}
\noindent Unlike the motor parameters in Table~\ref{tab:datasheet}, controller gains are user-specified firmware settings with units that vary by manufacturer. MuJoCo uses direct {\em voltage-space} units (e.g., $k_p$ in V/rad). Torque-space (N$\cdot$m/rad) gains can be converted by multiplying by $R/K$, though empirical calibration is often necessary due to unknown internal units on real hardware. \noindent Unlike the motor parameters in Table~\ref{tab:datasheet}, controller gains are user-specified firmware settings. MuJoCo uses {\em torque-space} units (e.g., $k_p$ in N$\cdot$m/rad), matching the \texttt{pid} actuator. Voltage-space datasheet gains (V/rad) convert by multiplying by $K/R$, though empirical calibration is often necessary due to unknown internal units on real hardware.
The controller computes a target voltage $v$ from the \texttt{ctrl} command. All motor physics --- cogging, saturation, friction, etc. --- apply identically downstream of $v$. The \texttt{input} attribute selects the controller: The controller computes a target voltage $v$ from the \texttt{ctrl} command. All motor physics --- cogging, saturation, friction, etc. --- apply identically downstream of $v$. The \texttt{input} attribute selects the controller:
\begin{figure}[H] \begin{figure}[H]
\centering \centering
\resizebox{\columnwidth}{!}{%
\begin{tikzpicture}[ \begin{tikzpicture}[
block/.style={draw, rounded corners=2pt, minimum height=1.6em, block/.style={draw, rounded corners=2pt, minimum height=1.6em,
font=\scriptsize, fill=blue!5}, font=\scriptsize, fill=blue!5},
@@ -1093,20 +1095,20 @@ The controller computes a target voltage $v$ from the \texttt{ctrl} command. All
every node/.style={inner sep=2pt}, every node/.style={inner sep=2pt},
] ]
% ctrl input % ctrl input
\node[font=\small] (ctrl) at (0, 3.5) {Input $u = {}$\texttt{ctrl}}; \node[font=\small] (ctrl) at (0, 3.8) {\texttt{ctrl} $=$ Input block $(\theta^*, \dot\theta^*, \tau_{f\!f}, v_{\text{raw}})$ \; (present members only)};
% Mode selector box % Controller box
\node[block, minimum width=5.5cm, minimum height=6.5em, align=center] \node[block, minimum width=7.2cm, minimum height=6.5em, align=center]
(sel) at (0, 1.8) {}; (sel) at (0, 1.8) {};
\node[font=\footnotesize\bfseries, anchor=north] at (0, 2.7) \node[font=\footnotesize\bfseries, anchor=north] at (0, 2.7)
{Controller mode}; {Controller};
\node[mode] at (0, 1.45) {$\begin{aligned} \node[mode] at (0, 1.45) {$\begin{aligned}
\texttt{voltage:}\quad v &= u \\[2pt] \tau &= k_p(\theta^*\!-\!\theta) + k_d(\dot\theta^*\!-\!\dot\theta) + k_i x_I + \tau_{f\!f} \\[2pt]
\texttt{position:}\quad v &= k_p(u\!-\!\theta) + k_i x_I - k_d\dot\theta \\[2pt] v &= \text{clip}(\tfrac{R}{K}\tau + K\dot\theta,\, \pm v_{\max}) + v_{\text{raw}} \\[2pt]
\texttt{velocity:}\quad v &= k_p(u\!-\!\dot\theta) + k_i(x_I\!-\!\theta) &\phantom{=}\ \text{absent inputs are zero;}\quad \text{\texttt{input=\textquotedbl none\textquotedbl:}}\ v = 0
\end{aligned}$}; \end{aligned}$};
% arrow ctrl to mode % arrow ctrl to controller
\draw[arr] (ctrl.south) -- (sel.north); \draw[arr] (ctrl.south) -- (sel.north);
% Motor block % Motor block
@@ -1121,101 +1123,52 @@ The controller computes a target voltage $v$ from the \texttt{ctrl} command. All
\node[font=\small] (tau) at (0, -1.8) {Torque $\tau$}; \node[font=\small] (tau) at (0, -1.8) {Torque $\tau$};
\draw[arr] (motor.south) -- (tau.north); \draw[arr] (motor.south) -- (tau.north);
\end{tikzpicture} \end{tikzpicture}}%
\caption{Controller pipeline. The \texttt{input} attribute selects how $v$ is derived from \texttt{ctrl}; motor physics is identical downstream.} \caption{Controller pipeline. The \texttt{input} attribute selects which members of the control block are present; motor physics is identical downstream.}
\label{fig:controller_pipeline} \label{fig:controller_pipeline}
\end{figure} \end{figure}
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \subsubsection{Setpoint Mode}
\subsubsection{Position Mode} \label{sec:setpoint_mode}
\label{sec:position_mode}
When \texttt{input="position"}, the user command $u = {}$\texttt{ctrl} is a target position, yielding voltage: When \texttt{input="pos vel ff"} (or subset thereof), the actuator owns a
\begin{equation} block of controls $u = (\theta^*, \dot\theta^*, \tau_{f\!f})$: a position setpoint, a velocity
v = k_p \, (u - \theta) + k_i \, x_I - k_d \, \dot\theta setpoint and a torque feedforward. Any subset may be selected, and the \texttt{dcmotor}'s \texttt{actuator\_ctrlnum} is variable.
\label{eq:position_mode} An absent input corresponds to a setpoint frozen at
\end{equation} zero. The controller commands a torque and converts it to drive voltage:
where $\theta$ is the actuator length, $\dot\theta \equiv \omega$ is the actuator velocity, $x_I$ is the integral of position error, and $k_p$, $k_i$, $k_d$ are the proportional, integral, and derivative gains. \begin{align}
\tau &= k_p \, (\theta^* - \theta) + k_d \, (\dot\theta^* - \dot\theta)
+ k_i \, x_I \; + \, \tau_{f\!f}
\label{eq:setpoint_mode} \\
v &= \frac{R}{K} \, \tau + K \dot\theta
\label{eq:torque_to_voltage}
\end{align}
where the second term of \eqref{eq:torque_to_voltage} compensates back-EMF, as the
current loop of a real torque-mode driver does: commanded torque is delivered {\em
exactly} until a limit binds, and the torque-speed envelope emerges from voltage
saturation (\S\ref{sec:voltage_saturation}). The map uses the nameplate $R$; thermal
resistance growth (\S\ref{sec:resistance_temperature}) is not compensated. The
integrator state accumulates the position error, $\dot x_I = \theta^* - \theta$,
subject to anti-windup (\S\ref{sec:anti_windup}). When $k_i = 0$, no integral state
is added and the controller reduces to PD; when $k_p = k_i = 0$, it is a pure
velocity servo. The back-EMF compensation also cancels the motor's $-K^2/R$ damping
bias, so the net physical damping delivered by an unclipped torque-mode motor is
$-k_d$, matching \texttt{pid}. The separate \texttt{voltage} input is the raw
terminal voltage, added downstream of the controller and its saturation.
\noindent The signs in~\eqref{eq:position_mode} follow MuJoCo convention: $k_p > 0$ drives toward the target, $k_d > 0$ provides damping (opposing velocity), and $k_i > 0$ reduces steady-state error. \subsubsection{Passive Operation}
\label{sec:passive}
\paragraph{Integral state.} When $k_i > 0$, one additional activation state $x_I$ is allocated, governed by: \texttt{input="none"} selects the {\em empty} input block: the actuator owns no
\begin{equation*} controls at all and acts as a purely passive device. The terminal voltage is
\dot{x}_I = u - \theta identically zero, so the motor operates with shorted terminals: back-EMF drives
\label{eq:position_integral} current through $R$ and brakes the joint (dynamic braking), while cogging
\end{equation*} \eqref{eq:cogging} and LuGre friction (\S\ref{sec:lugre}) act as passive joint
When $k_i = 0$, no integral state is added and the controller reduces to PD. forces. Setting \texttt{motorconst} to zero decouples the electrical branch,
leaving pure friction and cogging. This exists because MuJoCo associates auxiliary
\paragraph{Effective torque.} Substituting~\eqref{eq:position_mode} into the stateless torque equation~\eqref{eq:torque_speed}: dynamic states (such as the LuGre bristle) with actuators, not joints: a passive
\begin{equation*} \texttt{dcmotor} is the idiomatic way to attach these passive phenomena to a joint.
\tau = \frac{K}{R} v - \frac{K^2}{R}\dot\theta Controller gains, slew rate limiting and integral gain require a controller input and are rejected.
= \underbrace{\frac{K k_p}{R}}_{\text{stiffness}} (u - \theta)
+ \frac{K k_i}{R} x_I
- \underbrace{\frac{K(K + k_d)}{R}}_{\text{damping}} \dot\theta
\label{eq:position_torque}
\end{equation*}
Note that the motor's back-EMF term $K^2\dot\theta/R$ contributes {\em additional damping} beyond the controller $k_d$ term. Even with $k_d\!=\!0$, the motor provides natural damping $K^2/R$. The computed $v$ is subject to voltage saturation (\S\ref{sec:voltage_saturation}).
\begin{table}[H]
\centering
\small
\begin{tabular}{@{}lll@{}}
\toprule
Attribute & Symbol & Units \\
\midrule
\atKP{} & $k_p$ & V/rad \\
\atKI{} & $k_i$ & V/(rad$\cdot$s) \\
\atKD{} & $k_d$ & V$\cdot$s/rad \\
\bottomrule
\end{tabular}
\caption{Position mode controller gains.}
\label{tab:position_params}
\end{table}
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
\subsubsection{Velocity Mode}
\label{sec:velocity_mode}
When \texttt{input="velocity"}, the user command $u = {}$\texttt{ctrl} is a target velocity, and $k_p$, $k_i$ are the proportional and integral gains.
\begin{equation}
v = k_p \, (u - \dot\theta) + k_i \, (x_I - \theta)
\label{eq:velocity_mode}
\end{equation}
\paragraph{Integral state.} When $k_i > 0$, one additional activation state $x_I$ is allocated, governed by the integrator:
\begin{equation*}
\dot{x}_I = u
\label{eq:velocity_integral}
\end{equation*}
The term $k_i(x_I - \theta)$ then tracks a target position $x_I$ advancing at the commanded velocity $u$. This matches MuJoCo's \texttt{intvelocity} actuator behavior.
When $k_i = 0$, no integral state is added and the controller provides pure velocity feedback. The computed $v$ is subject to voltage saturation (\S\ref{sec:voltage_saturation}).
\paragraph{Effective torque.} Substituting~\eqref{eq:velocity_mode} into~\eqref{eq:torque_speed}:
\begin{equation*}
\tau = \underbrace{\frac{K k_i}{R}}_{\text{stiffness}} (x_I - \theta)
- \underbrace{\frac{K(K + k_p)}{R}}_{\text{damping}} \dot\theta
+ \frac{K k_p}{R} u
\label{eq:velocity_torque}
\end{equation*}
Note the role swap compared to position mode: $k_i$ provides stiffness (position tracking to $x_I$) while $k_p$ adds damping alongside the motor's natural back-EMF damping $K^2/R$.
\begin{table}[H]
\centering
\small
\begin{tabular}{@{}lll@{}}
\toprule
Attribute & Symbol & Units \\
\midrule
\atKP{} & $k_p$ & V$\cdot$s/rad \\
\atKI{} & $k_i$ & V/rad \\
\bottomrule
\end{tabular}
\caption{Velocity mode controller gains.}
\label{tab:velocity_params}
\end{table}
\pagebreak
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - % - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
\subsubsection{Setpoint Slew Rate} \subsubsection{Setpoint Slew Rate}
@@ -1229,7 +1182,7 @@ where $u_{\text{prev}}$ is the previous effective setpoint and $\Delta t$ is the
\paragraph{State variable.} When $s > 0$, one activation state $u_{\text{prev}}$ is allocated, and updated each step to $u$ (post-clamping). \paragraph{State variable.} When $s > 0$, one activation state $u_{\text{prev}}$ is allocated, and updated each step to $u$ (post-clamping).
\paragraph{Units.} The slew rate $s$ has mode-dependent units: rad/s for position mode (limiting setpoint velocity), rad/s\textsuperscript{2} for velocity mode (limiting setpoint acceleration), and V/s for voltage mode. \paragraph{Units.} The slew rate $s$ limits the first controller input, with corresponding units: position setpoint (rad/s), velocity setpoint (rad/s$^2$), or torque feedforward (N$\cdot$m/s). The raw \texttt{voltage} input is never rate-limited; \texttt{slewmax} requires a controller input.
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - % - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
\subsubsection{Anti-windup} \subsubsection{Anti-windup}
@@ -1245,12 +1198,12 @@ This prevents controller windup even when drive signals are saturated.
\subsubsection{Voltage Saturation} \subsubsection{Voltage Saturation}
\label{sec:voltage_saturation} \label{sec:voltage_saturation}
In \texttt{position} and \texttt{velocity} modes, the computed voltage $v$ can be arbitrarily large (proportional to the error). Real motor drivers are limited by their supply voltage. When \atVMAX{} is set ($v_{\max} > 0$), a voltage clamp is applied before the motor equations: With setpoint inputs, the computed voltage $v$ can be arbitrarily large (proportional to the error). Real motor drivers are limited by their supply voltage. When \atVMAX{} is set ($v_{\max} > 0$), a voltage clamp is applied before the motor equations:
\begin{equation*} \begin{equation*}
v \leftarrow \text{clip}(v, \pm v_{\max}) v \leftarrow \text{clip}(v, \pm v_{\max})
\label{eq:vlimit} \label{eq:vlimit}
\end{equation*} \end{equation*}
This differs from \texttt{ctrlrange} (clamping user command $u$) and \texttt{forcerange} (clamping output torque). In position and velocity modes, \texttt{ctrlrange} limits the setpoint while \atVMAX{} limits the drive signal. In voltage mode ($v = u$), both clamp the voltage; if both are set, the tighter limit wins. This differs from \texttt{ctrlrange} (clamping user commands $u$) and \texttt{forcerange} (clamping output torque). The clamp applies to the controller output only: the raw \texttt{voltage} input is added downstream, unclamped (use \texttt{ctrlrange} to bound it). Because the torque-to-voltage map \eqref{eq:torque_to_voltage} compensates back-EMF, the achievable torque under \atVMAX{} shrinks with speed: the torque-speed envelope of \S\ref{sec:electromagnetics}.
\begin{table}[H] \begin{table}[H]
\centering \centering
@@ -1259,11 +1212,11 @@ This differs from \texttt{ctrlrange} (clamping user command $u$) and \texttt{for
\toprule \toprule
Attribute & Symbol & Units \\ Attribute & Symbol & Units \\
\midrule \midrule
\atKP{} & $k_p$ & mode-dependent \\ \atKP{} & $k_p$ & N$\cdot$m/rad \\
\atKI{} & $k_i$ & mode-dependent \\ \atKI{} & $k_i$ & N$\cdot$m/(rad$\cdot$s) \\
\atKD{} & $k_d$ & V$\cdot$s/rad \\ \atKD{} & $k_d$ & N$\cdot$m$\cdot$s/rad \\
\atSLEW{} & $s$ & ctrl-units/s \\ \atSLEW{} & $s$ & ctrl-units/s \\
\atIMAXINT{} & $I_{\max}$ & mode-dependent \\ \atIMAXINT{} & $I_{\max}$ & rad$\cdot$s \\
\atVMAX{} & $v_{\max}$ & Volt \\ \atVMAX{} & $v_{\max}$ & Volt \\
\bottomrule \bottomrule
\end{tabular} \end{tabular}
@@ -1272,6 +1225,8 @@ Attribute & Symbol & Units \\
\end{table} \end{table}
% --------------------------------------------------------------------------- % ---------------------------------------------------------------------------
\vfill\newpage
% Low-Level Semantics % Low-Level Semantics
% --------------------------------------------------------------------------- % ---------------------------------------------------------------------------
\subsection{Low-Level Semantics} \subsection{Low-Level Semantics}
@@ -1294,7 +1249,7 @@ Array & Index & Symbol & Description \\
& 5 & $k_i$ & Controller integral gain \\ & 5 & $k_i$ & Controller integral gain \\
& 6 & $k_d$ & Controller derivative gain \\ & 6 & $k_d$ & Controller derivative gain \\
& 7 & $v_{\max}$ & Voltage saturation (V) \\ & 7 & $v_{\max}$ & Voltage saturation (V) \\
& 8 & --- & Input mode (0:\ $v$, 1:\ $\theta$, 2:\ $\dot\theta$) \\ & 8 & --- & Reserved \\
\midrule \midrule
\texttt{dynprm} & 0 & $t_e$ & Electrical time constant (s) \\ \texttt{dynprm} & 0 & $t_e$ & Electrical time constant (s) \\
& 1 & $(di{/}dt)_{\max}$ & Current rate limit (A/s) \\ & 1 & $(di{/}dt)_{\max}$ & Current rate limit (A/s) \\
@@ -1323,6 +1278,8 @@ Array & Index & Symbol & Description \\
\texttt{armature} & 0 & $J_r$ & Actuator armature \\ \texttt{armature} & 0 & $J_r$ & Actuator armature \\
\midrule \midrule
\texttt{gear} & 0 & $N$ & Gear ratio \\ \texttt{gear} & 0 & $N$ & Gear ratio \\
\midrule
\texttt{ctrlspec} & 0 & --- & Input signature (\texttt{mjtCtrlInput} bitmask) \\
\bottomrule \bottomrule
\end{tabular} \end{tabular}
\caption{\texttt{mjModel} array semantics for the \texttt{dcmotor} actuator.} \caption{\texttt{mjModel} array semantics for the \texttt{dcmotor} actuator.}
doc/includes/references.h
+4 -2
View File
@@ -2548,7 +2548,9 @@ typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): or
typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits
mjINPUT_POS = 1, // position setpoint input mjINPUT_POS = 1, // position setpoint input
mjINPUT_VEL = 2, // velocity setpoint input mjINPUT_VEL = 2, // velocity setpoint input
mjINPUT_FF = 4 // feedforward input mjINPUT_FF = 4, // feedforward input, in the actuator's output space
mjINPUT_VOLTAGE = 8, // raw terminal voltage input (dcmotor)
mjINPUT_NONE = 16 // explicitly no inputs: purely passive (dcmotor)
} mjtCtrlInput; } mjtCtrlInput;
typedef enum mjtObj { // type of MujoCo object typedef enum mjtObj { // type of MujoCo object
mjOBJ_UNKNOWN = 0, // unknown object type mjOBJ_UNKNOWN = 0, // unknown object type
@@ -4018,7 +4020,7 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[3], double inductance[2], double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6], double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode); double lugre[5], int ctrlspec);
mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def);
mjsHField* mjs_addHField(mjSpec* s); mjsHField* mjs_addHField(mjSpec* s);
mjsSkin* mjs_addSkin(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s);
include/mujoco/mjtype.h
+3 -1
View File
@@ -282,7 +282,9 @@ typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): or
typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits
mjINPUT_POS = 1, // position setpoint input mjINPUT_POS = 1, // position setpoint input
mjINPUT_VEL = 2, // velocity setpoint input mjINPUT_VEL = 2, // velocity setpoint input
mjINPUT_FF = 4 // feedforward input mjINPUT_FF = 4, // feedforward input, in the actuator's output space
mjINPUT_VOLTAGE = 8, // raw terminal voltage input (dcmotor)
mjINPUT_NONE = 16 // explicitly no inputs: purely passive (dcmotor)
} mjtCtrlInput; } mjtCtrlInput;
include/mujoco/mujoco.h
+1 -1
View File
@@ -1785,7 +1785,7 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[3], double inductance[2], double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6], double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode); double lugre[5], int ctrlspec);
//---------------------------------- Assets -------------------------------------------------------- //---------------------------------- Assets --------------------------------------------------------
python/mujoco/introspect/enums.py
+2
View File
@@ -314,6 +314,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjINPUT_POS', 1), ('mjINPUT_POS', 1),
('mjINPUT_VEL', 2), ('mjINPUT_VEL', 2),
('mjINPUT_FF', 4), ('mjINPUT_FF', 4),
('mjINPUT_VOLTAGE', 8),
('mjINPUT_NONE', 16),
]), ]),
)), )),
('mjtObj', ('mjtObj',
python/mujoco/introspect/functions.py
+1 -1
View File
@@ -11314,7 +11314,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
nullable=True, nullable=True,
), ),
FunctionParameterDecl( FunctionParameterDecl(
name='input_mode', name='ctrlspec',
type=ValueType(name='int'), type=ValueType(name='int'),
), ),
), ),
python/mujoco/specs.cc
+3 -3
View File
@@ -1639,11 +1639,11 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) {
std::array<double, 3> nominal, std::array<double, 3> saturation, std::array<double, 3> nominal, std::array<double, 3> saturation,
std::array<double, 2> inductance, std::array<double, 3> cogging, std::array<double, 2> inductance, std::array<double, 3> cogging,
std::array<double, 6> controller, std::array<double, 6> thermal, std::array<double, 6> controller, std::array<double, 6> thermal,
std::array<double, 5> lugre, int input_mode) { std::array<double, 5> lugre, int ctrlspec) {
std::string err = mjs_setToDCMotor( std::string err = mjs_setToDCMotor(
self, motorconst.data(), resistance, nominal.data(), self, motorconst.data(), resistance, nominal.data(),
saturation.data(), inductance.data(), cogging.data(), saturation.data(), inductance.data(), cogging.data(),
controller.data(), thermal.data(), lugre.data(), input_mode); controller.data(), thermal.data(), lugre.data(), ctrlspec);
if (!err.empty()) { if (!err.empty()) {
throw pybind11::value_error(err); throw pybind11::value_error(err);
} }
@@ -1656,7 +1656,7 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) {
py::arg("controller") = std::array<double, 6>{0, 0, 0, 0, 0, 0}, py::arg("controller") = std::array<double, 6>{0, 0, 0, 0, 0, 0},
py::arg("thermal") = std::array<double, 6>{0, 0, 0, 0, 0, 0}, py::arg("thermal") = std::array<double, 6>{0, 0, 0, 0, 0, 0},
py::arg("lugre") = std::array<double, 5>{0, 0, 0, 0, 0}, py::arg("lugre") = std::array<double, 5>{0, 0, 0, 0, 0},
py::arg("input_mode") = 0); py::arg("ctrlspec") = 0);
// ============================= MJSTENDONPATH =============================== // ============================= MJSTENDONPATH ===============================
// helper struct for tendon path indexing // helper struct for tendon path indexing
src/engine/engine_derivative.c
+9 -4
View File
@@ -2433,11 +2433,16 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
mjtNum te = dynprm[0]; mjtNum te = dynprm[0];
// controller velocity derivative: dV/dω // controller velocity derivative dV/dw: torque-space kd through the tau->V map,
int input_mode = (int)gainprm[8]; // plus the back-EMF compensation K, which cancels the -K^2/R back-EMF bias term so
// the net damping of an unclipped torque-mode motor is -kd; Vmax clipping is ignored
// here, matching the treatment of the other saturations
mjtNum dVdw = 0; mjtNum dVdw = 0;
if (input_mode == 1) dVdw = -gainprm[6]; // position: -kd if (m->actuator_ctrlspec[i] & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) {
else if (input_mode == 2) dVdw = -gainprm[4]; // velocity: -kp mjtNum R = mju_max(mjMINVAL, gainprm[0]);
mjtNum K = gainprm[1]; // K > 0 on this path, enforced by the compiler
dVdw = -gainprm[6]*R/K + K;
}
if (te > 0) { if (te > 0) {
// stateful current with actearly: d(K*next_act)/dω // stateful current with actearly: d(K*next_act)/dω
src/engine/engine_forward.c
+37 -40
View File
@@ -227,42 +227,44 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
// unpack servo-family inputs from control block in canonical order [pos, vel, ff] // unpack servo-family inputs from control block in canonical order [pos, vel, ff]
// absent input: setpoint 0 // absent input: setpoint 0
static void unpackServoInputs(const mjtNum* u, int spec, mjtNum out[3]) { static void unpackServoInputs(const mjtNum* u, int spec, mjtNum out[4]) {
int adr = 0; int adr = 0;
out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0; out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0;
out[1] = (spec & mjINPUT_VEL) ? u[adr++] : 0; out[1] = (spec & mjINPUT_VEL) ? u[adr++] : 0;
out[2] = (spec & mjINPUT_FF) ? u[adr] : 0; out[2] = (spec & mjINPUT_FF) ? u[adr++] : 0;
out[3] = (spec & mjINPUT_VOLTAGE) ? u[adr] : 0;
} }
// helper for DC motor: computes control voltage from PID state // helper for DC motor: computes control voltage from PID state
static mjtNum dcmotorVoltage(mjtNum ctrl, mjtNum length, mjtNum velocity, static mjtNum dcmotorVoltage(const mjtNum* u, int spec, mjtNum length, mjtNum velocity,
mjtNum x_I, const mjtNum* gainprm) { mjtNum x_I, const mjtNum* gainprm) {
int input_mode = (int)gainprm[8]; mjtNum voltage = 0;
mjtNum Vmax = gainprm[7];
mjtNum voltage;
// get voltage // unpack present inputs in canonical order [pos, vel, ff, voltage]; absent input: 0
if (input_mode > 0) { mjtNum u4[4];
unpackServoInputs(u, spec, u4);
// on-board controller: torque-space PID + torque feedforward
if (spec & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) {
mjtNum kp = gainprm[4]; // proportional gain mjtNum kp = gainprm[4]; // proportional gain
mjtNum ki = gainprm[5]; // integral gain mjtNum ki = gainprm[5]; // integral gain
mjtNum kd = gainprm[6]; // derivative gain mjtNum kd = gainprm[6]; // derivative gain
mjtNum torque = kp*(u4[0] - length) + kd*(u4[1] - velocity) + ki*x_I + u4[2];
if (input_mode == 1) { // torque mode is current control: V = R/K * torque + K * velocity, the second
// position mode // term compensating back-EMF; the compiler requires K > 0 on this path
voltage = kp * (ctrl - length) + ki * x_I - kd * velocity; mjtNum R = gainprm[0];
} else { mjtNum K = gainprm[1];
// velocity mode voltage = R/K * torque + K*velocity;
voltage = kp * (ctrl - velocity) + ki * (x_I - length);
}
} else {
voltage = ctrl;
}
// clip voltage // driver supply limit
mjtNum Vmax = gainprm[7];
if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax); if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax);
}
return voltage; // raw terminal voltage input: downstream of the controller, unclamped
return voltage + u4[3];
} }
@@ -504,26 +506,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// controller state: slew rate limiting // controller state: slew rate limiting
mjtNum slew_s = dynprm[7]; // slew rate limit mjtNum slew_s = dynprm[7]; // slew rate limit
if (slew_s > 0) { if (slew_s > 0) {
mjtNum u_prev = d->act[adr]; ctrl[uadr] = slewLimit(ctrl[uadr], d->act[adr], slew_s, m->opt.timestep,
mjtNum slew = slew_s * m->opt.timestep; 0, d->act_dot + adr);
mjtNum u_eff = mju_clip(ctrl[uadr], u_prev - slew, u_prev + slew);
d->act_dot[adr] = (u_eff - u_prev) / m->opt.timestep;
ctrl[uadr] = u_eff;
adr++; adr++;
} }
// controller state: integral state // controller state: integral of the position error (setpoint mode only)
mjtNum x_I = 0; mjtNum x_I = 0;
if (ki > 0) { if (ki > 0) {
x_I = d->act[adr]; x_I = d->act[adr];
int input_mode = (int)gainprm[8];
mjtNum Imax = dynprm[8]; // integral clamp mjtNum Imax = dynprm[8]; // integral clamp
mjtNum act_dot = ctrl[uadr]; // default raw accumulator for voltage and velocity modes mjtNum act_dot = ctrl[uadr] - d->actuator_length[oadr];
// position mode
if (input_mode == 1) {
act_dot = ctrl[uadr] - d->actuator_length[oadr];
}
// clamp act_dot based on integral state // clamp act_dot based on integral state
if (Imax > 0) { if (Imax > 0) {
@@ -538,7 +531,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
} }
// compute physical voltage to feed into current and temperature equations // compute physical voltage to feed into current and temperature equations
mjtNum V = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr], velocity, x_I, gainprm); mjtNum V = dcmotorVoltage(ctrl + uadr, m->actuator_ctrlspec[i],
d->actuator_length[oadr], velocity, x_I, gainprm);
// temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient // temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient
mjtNum RT = dynprm[2]; // thermal resistance mjtNum RT = dynprm[2]; // thermal resistance
@@ -738,9 +732,11 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R); gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);
// controller: compute voltage, override ctrl[uadr] for force computation // controller: compute voltage, override ctrl[uadr] for force computation
if ((int)gainprm[8] > 0) { // (pure raw-voltage motor reads ctrl directly; empty block reads as 0 below)
if (m->actuator_ctrlspec[i] != mjINPUT_VOLTAGE && m->actuator_ctrlnum[i] > 0) {
mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0; mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0;
ctrl[uadr] = dcmotorVoltage(ctrl[uadr], d->actuator_length[oadr], ctrl[uadr] = dcmotorVoltage(ctrl + uadr, m->actuator_ctrlspec[i],
d->actuator_length[oadr],
d->actuator_velocity[oadr], x_I, gainprm); d->actuator_velocity[oadr], x_I, gainprm);
} }
break; break;
@@ -769,9 +765,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i; const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
// unpack present inputs in canonical order [pos, vel, ff]; absent input: setpoint 0 // unpack present inputs in canonical order [pos, vel, ff]; absent input: setpoint 0
mjtNum u3[3]; mjtNum u4[4];
unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u3); unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u4);
mjtNum qref = u3[0], vref = u3[1], ff = u3[2]; mjtNum qref = u4[0], vref = u4[1], ff = u4[2];
// position setpoint: representative nearest the length on rotational transmissions // position setpoint: representative nearest the length on rotational transmissions
mjtNum period = wrapPeriod(m, i); mjtNum period = wrapPeriod(m, i);
@@ -792,7 +788,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
} }
} }
else if (actnum == 0 || dcmotor_no_current) { else if (actnum == 0 || dcmotor_no_current) {
mjtNum input = ctrl[uadr]; // empty input block (passive dcmotor): input is 0
mjtNum input = m->actuator_ctrlnum[i] ? ctrl[uadr] : 0;
// rotational setpoint: use representative nearest the length (local, no state change) // rotational setpoint: use representative nearest the length (local, no state change)
mjtNum period = wrapPeriod(m, i); mjtNum period = wrapPeriod(m, i);
src/engine/engine_name.c
+5 -5
View File
@@ -297,12 +297,12 @@ const char* mj_actuatorInputName(const mjModel* m, int id, int input) {
return m->actuator_ctrlspec[id] == mjCHART_QUAT ? quat[input] : expmap[input]; return m->actuator_ctrlspec[id] == mjCHART_QUAT ? quat[input] : expmap[input];
} }
// servo family: input names are the present members of [pos, vel, ff] // servo family: input names are the present members of [pos, vel, ff, voltage]
if (m->actuator_gaintype[id] == mjGAIN_PID) { if (m->actuator_gaintype[id] == mjGAIN_PID || m->actuator_gaintype[id] == mjGAIN_DCMOTOR) {
static const char* servo[3] = {"pos", "vel", "ff"}; static const char* servo[4] = {"pos", "vel", "ff", "voltage"};
static const int bits[3] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF}; static const int bits[4] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF, mjINPUT_VOLTAGE};
int spec = m->actuator_ctrlspec[id]; int spec = m->actuator_ctrlspec[id];
for (int k=0; k < 3; k++) { for (int k=0; k < 4; k++) {
if (spec & bits[k]) { if (spec & bits[k]) {
if (input == 0) { if (input == 0) {
return servo[k]; return servo[k];
src/user/user_api.cc
+3 -2
View File
@@ -1506,7 +1506,7 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) {
const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[3], double inductance[2], double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6], double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode) { double lugre[5], int ctrlspec) {
double R = resistance; // electrical resistance double R = resistance; // electrical resistance
double Kt = motorconst ? motorconst[0] : 0; // torque constant double Kt = motorconst ? motorconst[0] : 0; // torque constant
double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant
@@ -1652,7 +1652,8 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
} }
// set input mode and activation dimension // set input mode and activation dimension
actuator->gainprm[8] = input_mode; actuator->gainprm[8] = 0; // reserved (was input_mode)
actuator->ctrlspec = ctrlspec;
actuator->actdim = actdim; actuator->actdim = actdim;
// enforce actlimited = 0; homogeneous bounds are invalid across DC motor states // enforce actlimited = 0; homogeneous bounds are invalid across DC motor states
src/user/user_api.h
+1 -1
View File
@@ -213,7 +213,7 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[3], double inductance[2], double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6], double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode); double lugre[5], int ctrlspec);
//---------------------------------- Add assets ---------------------------------------------------- //---------------------------------- Add assets ----------------------------------------------------
src/user/user_objects.cc
+43 -6
View File
@@ -7233,16 +7233,49 @@ void mjCActuator::Compile(void) {
!!(ctrlspec_ & mjINPUT_FF); !!(ctrlspec_ & mjINPUT_FF);
} }
// DC motor: resolve input block (default: raw voltage command)
if (gaintype == mjGAIN_DCMOTOR) {
ctrlspec_ = ctrlspec ? ctrlspec : mjINPUT_VOLTAGE;
if (ctrlspec_ != mjINPUT_NONE &&
(ctrlspec_ & ~(mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF | mjINPUT_VOLTAGE))) {
throw mjCError(this, "dcmotor inputs are 'none' or a subset of [pos, vel, ff, voltage] in "
"actuator '%s' (id = %d)", name.c_str(), id);
}
// controller inputs engage the torque-space controller, which divides by the motor constant
int controller = ctrlspec_ == mjINPUT_NONE ?
0 : ctrlspec_ & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF);
if (controller && gainprm[1] <= 0) {
throw mjCError(this, "dcmotor controller inputs require a positive motor constant in "
"actuator '%s' (id = %d)", name.c_str(), id);
}
if (!controller && (gainprm[4] || gainprm[5] || gainprm[6])) {
throw mjCError(this, "dcmotor controller gains require a controller input [pos, vel, ff] "
"in actuator '%s' (id = %d)", name.c_str(), id);
}
if (gainprm[5] > 0 && !(ctrlspec_ & mjINPUT_POS)) {
throw mjCError(this, "dcmotor integral gain requires the pos input in actuator '%s' "
"(id = %d)", name.c_str(), id);
}
ctrlnum_ = !!(ctrlspec_ & mjINPUT_POS) + !!(ctrlspec_ & mjINPUT_VEL) +
!!(ctrlspec_ & mjINPUT_FF) + !!(ctrlspec_ & mjINPUT_VOLTAGE);
if (!controller && dynprm[7] > 0) {
throw mjCError(this, "dcmotor slew rate limiting requires a controller input [pos, vel, "
"ff] in actuator '%s' (id = %d)", name.c_str(), id);
}
}
// pid dynamics are pid-only // pid dynamics are pid-only
if (dyntype == mjDYN_PID && gaintype != mjGAIN_PID) { if (dyntype == mjDYN_PID && gaintype != mjGAIN_PID) {
throw mjCError(this, "dyntype 'pid' requires gaintype 'pid', actuator '%s' (id = %d)", throw mjCError(this, "dyntype 'pid' requires gaintype 'pid', actuator '%s' (id = %d)",
name.c_str(), id); name.c_str(), id);
} }
// input signature selection is so3- or pid-only // input signature selection is so3-, pid- or dcmotor-only
if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID) { if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID &&
throw mjCError(this, "input is only available for so3 and pid actuators, actuator '%s' " gaintype != mjGAIN_DCMOTOR) {
"(id = %d)", name.c_str(), id); throw mjCError(this, "input is only available for so3, pid and dcmotor actuators, "
"actuator '%s' (id = %d)", name.c_str(), id);
} }
// check damping/armature only valid for joint and tendon transmission // check damping/armature only valid for joint and tendon transmission
@@ -7273,7 +7306,7 @@ void mjCActuator::Compile(void) {
double* range; double* range;
if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT || if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT ||
dyntype == mjDYN_PID) { dyntype == mjDYN_PID) {
// position or pd actuator: range applies to the position input // position or pid actuator: range applies to the position input
range = ctrlrange; range = ctrlrange;
} else if (dyntype == mjDYN_INTEGRATOR) { } else if (dyntype == mjDYN_INTEGRATOR) {
// intvelocity actuator // intvelocity actuator
@@ -7417,6 +7450,10 @@ void mjCActuator::Compile(void) {
if (delay > 0 && nsample <= 0) { if (delay > 0 && nsample <= 0) {
throw mjCError(this, "setting delay > 0 without a history buffer"); throw mjCError(this, "setting delay > 0 without a history buffer");
} }
if ((delay > 0 || nsample > 0) && ctrlnum_ == 0) {
throw mjCError(this, "history and delay require an input in actuator '%s' (id = %d)",
name.c_str(), id);
}
// nsample is limited to 2^24 because the cursor is stored as an mjtNum, which may be a float // nsample is limited to 2^24 because the cursor is stored as an mjtNum, which may be a float
// single-precision floats can represent all integers up to 2^24 exactly // single-precision floats can represent all integers up to 2^24 exactly
@@ -7424,7 +7461,7 @@ void mjCActuator::Compile(void) {
throw mjCError(this, "at most 2^24 samples in history buffer, got %d", nullptr, nsample); throw mjCError(this, "at most 2^24 samples in history buffer, got %d", nullptr, nsample);
} }
// resolve per-input control ranges: broadcast ctrlrange, pd overrides vel and ff // resolve per-input control ranges: broadcast ctrlrange, pid overrides vel and ff
for (int j=0; j < ctrlnum_ && j < 4; j++) { for (int j=0; j < ctrlnum_ && j < 4; j++) {
ctrllimiteds_[j] = (mjtByte)is_ctrllimited(); ctrllimiteds_[j] = (mjtByte)is_ctrllimited();
ctrlranges_[j][0] = ctrlrange[0]; ctrlranges_[j][0] = ctrlrange[0];
src/xml/generated/mjcf.xsd
+17 -7
View File
@@ -223,11 +223,12 @@
</xs:restriction> </xs:restriction>
</xs:simpleType> </xs:simpleType>
<xs:simpleType name="kw_dcmotorinput"> <xs:simpleType name="kw_inputkeyword">
<xs:annotation>
<xs:documentation>whole-attribute keyword: the empty signature</xs:documentation>
</xs:annotation>
<xs:restriction base="xs:string"> <xs:restriction base="xs:string">
<xs:enumeration value="voltage"/> <xs:enumeration value="none"/>
<xs:enumeration value="position"/>
<xs:enumeration value="velocity"/>
</xs:restriction> </xs:restriction>
</xs:simpleType> </xs:simpleType>
@@ -252,12 +253,13 @@
<xs:simpleType name="kw_inputbit"> <xs:simpleType name="kw_inputbit">
<xs:annotation> <xs:annotation>
<xs:documentation>bitflags: keywords combine bitwise</xs:documentation> <xs:documentation>bitflags: tokens combine bitwise, canonical order enforced</xs:documentation>
</xs:annotation> </xs:annotation>
<xs:restriction base="xs:string"> <xs:restriction base="xs:string">
<xs:enumeration value="pos"/> <xs:enumeration value="pos"/>
<xs:enumeration value="vel"/> <xs:enumeration value="vel"/>
<xs:enumeration value="ff"/> <xs:enumeration value="ff"/>
<xs:enumeration value="voltage"/>
</xs:restriction> </xs:restriction>
</xs:simpleType> </xs:simpleType>
<xs:simpleType name="kwlist_inputbit"> <xs:simpleType name="kwlist_inputbit">
@@ -1880,7 +1882,11 @@
<xs:attribute name="controller" type="double1to6"/> <xs:attribute name="controller" type="double1to6"/>
<xs:attribute name="thermal" type="double1to6"/> <xs:attribute name="thermal" type="double1to6"/>
<xs:attribute name="lugre" type="double1to5"/> <xs:attribute name="lugre" type="double1to5"/>
<xs:attribute name="input" type="kw_dcmotorinput"/> <xs:attribute name="input" type="kwlist_inputbit">
<xs:annotation>
<xs:documentation>token subset, or the none keyword</xs:documentation>
</xs:annotation>
</xs:attribute>
</xs:complexType> </xs:complexType>
<xs:complexType name="extension_plugin"> <xs:complexType name="extension_plugin">
@@ -3055,7 +3061,11 @@
<xs:attribute name="controller" type="double1to6"/> <xs:attribute name="controller" type="double1to6"/>
<xs:attribute name="thermal" type="double1to6"/> <xs:attribute name="thermal" type="double1to6"/>
<xs:attribute name="lugre" type="double1to5"/> <xs:attribute name="lugre" type="double1to5"/>
<xs:attribute name="input" type="kw_dcmotorinput"/> <xs:attribute name="input" type="kwlist_inputbit">
<xs:annotation>
<xs:documentation>token subset, or the none keyword</xs:documentation>
</xs:annotation>
</xs:attribute>
</xs:complexType> </xs:complexType>
<xs:complexType name="actuator_plugin"> <xs:complexType name="actuator_plugin">
src/xml/generated/mjcf_map.h
+6 -7
View File
@@ -243,13 +243,11 @@ inline constexpr mjMap dyn_map[] = {
}; };
inline constexpr int dyn_sz = 8; inline constexpr int dyn_sz = 8;
// enum dcmotorinput // enum inputkeyword
inline constexpr mjMap dcmotorinput_map[] = { inline constexpr mjMap inputkeyword_map[] = {
{"voltage", 0}, {"none", mjINPUT_NONE},
{"position", 1},
{"velocity", 2},
}; };
inline constexpr int dcmotorinput_sz = 3; inline constexpr int inputkeyword_sz = 1;
// enum gain // enum gain
inline constexpr mjMap gain_map[] = { inline constexpr mjMap gain_map[] = {
@@ -275,8 +273,9 @@ inline constexpr mjMap inputbit_map[] = {
{"pos", mjINPUT_POS}, {"pos", mjINPUT_POS},
{"vel", mjINPUT_VEL}, {"vel", mjINPUT_VEL},
{"ff", mjINPUT_FF}, {"ff", mjINPUT_FF},
{"voltage", mjINPUT_VOLTAGE},
}; };
inline constexpr int inputbit_sz = 3; inline constexpr int inputbit_sz = 4;
// enum bias // enum bias
inline constexpr mjMap bias_map[] = { inline constexpr mjMap bias_map[] = {
src/xml/mjcf.schema
+5 -6
View File
@@ -237,10 +237,8 @@ enum dyn : mjtDyn {
user = mjDYN_USER user = mjDYN_USER
} }
enum dcmotorinput { enum inputkeyword : mjtCtrlInput { # whole-attribute keyword: the empty signature
voltage = 0 none = mjINPUT_NONE
position = 1
velocity = 2
} }
enum gain : mjtGain { enum gain : mjtGain {
@@ -258,10 +256,11 @@ enum inputchart : mjtCtrlChart {
quat = mjCHART_QUAT quat = mjCHART_QUAT
} }
enum inputbit : mjtCtrlInput { # bitflags: keywords combine bitwise enum inputbit : mjtCtrlInput { # bitflags: tokens combine bitwise, canonical order enforced
pos = mjINPUT_POS pos = mjINPUT_POS
vel = mjINPUT_VEL vel = mjINPUT_VEL
ff = mjINPUT_FF ff = mjINPUT_FF
voltage = mjINPUT_VOLTAGE
} }
enum bias : mjtBias { enum bias : mjtBias {
@@ -1680,7 +1679,7 @@ element dcmotor : mjsActuator {
controller : double[1..6] controller : double[1..6]
thermal : double[1..6] thermal : double[1..6]
lugre : double[1..5] lugre : double[1..5]
input : enum<dcmotorinput> input : flags<inputbit> (reading=custom) # token subset, or the none keyword
} }
element actuator_plugin : mjsActuator (xml=plugin) { element actuator_plugin : mjsActuator (xml=plugin) {
src/xml/xml_native_reader.cc
+17 -8
View File
@@ -1149,7 +1149,8 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) {
// read the "input" attribute: so3 chart keyword, or servo input token list // read the "input" attribute: so3 chart keyword, the "none" keyword (empty signature),
// or a servo input token list, required to be in canonical order [pos, vel, ff, voltage]
static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) { static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) {
std::string text; std::string text;
if (!mjXUtil::ReadAttrTxt(elem, "input", text)) { if (!mjXUtil::ReadAttrTxt(elem, "input", text)) {
@@ -1163,11 +1164,21 @@ static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) {
return true; return true;
} }
// servo input tokens // empty-signature keyword
int keyword = mjXUtil::FindKey(inputkeyword_map, inputkeyword_sz, text);
if (keyword >= 0) {
*ctrlspec = keyword;
return true;
}
// servo input tokens; strictly ascending bits = canonical order, no duplicates
int bits[inputbit_sz]; int bits[inputbit_sz];
int nbit = mjXUtil::MapValues(elem, "input", bits, inputbit_map, inputbit_sz); int nbit = mjXUtil::MapValues(elem, "input", bits, inputbit_map, inputbit_sz);
int spec = 0; int spec = 0;
for (int k=0; k < nbit; k++) { for (int k=0; k < nbit; k++) {
if (bits[k] <= (k ? bits[k-1] : 0)) {
throw mjXError(elem, "inputs must be listed in canonical order [pos, vel, ff, voltage]");
}
spec |= bits[k]; spec |= bits[k];
} }
*ctrlspec = spec; *ctrlspec = spec;
@@ -1235,7 +1246,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
// explicit attributes // explicit attributes
string err; string err;
if (type == "general") { if (type == "general") {
// so3 chart keyword or servo token subset // so3 chart keyword or servo token subset; dcmotor accepts the voltage keyword
ReadInputSpec(elem, &actuator->ctrlspec); ReadInputSpec(elem, &actuator->ctrlspec);
} }
@@ -1431,7 +1442,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
inherited ? actuator->biasprm[3] : 0, inherited ? actuator->biasprm[3] : 0,
inherited ? actuator->biasprm[4] : 0, inherited ? actuator->biasprm[4] : 0,
inherited ? actuator->biasprm[5] : 0}; inherited ? actuator->biasprm[5] : 0};
int input_mode = inherited ? (int)actuator->gainprm[8] : 0; int ctrlspec = inherited ? actuator->ctrlspec : 0;
ReadAttr(elem, "motorconst", 2, motorconst, text, false, false); ReadAttr(elem, "motorconst", 2, motorconst, text, false, false);
ReadAttr(elem, "resistance", 1, &resistance, text); ReadAttr(elem, "resistance", 1, &resistance, text);
ReadAttr(elem, "nominal", 3, nominal, text, false, false); ReadAttr(elem, "nominal", 3, nominal, text, false, false);
@@ -1441,12 +1452,10 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
ReadAttr(elem, "controller", 6, controller, text, false, false); ReadAttr(elem, "controller", 6, controller, text, false, false);
ReadAttr(elem, "thermal", 6, thermal, text, false, false); ReadAttr(elem, "thermal", 6, thermal, text, false, false);
ReadAttr(elem, "lugre", 5, lugre, text, false, false); ReadAttr(elem, "lugre", 5, lugre, text, false, false);
if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) { ReadInputSpec(elem, &ctrlspec);
// successfully parsed
}
err = mjs_setToDCMotor(actuator, motorconst, resistance, err = mjs_setToDCMotor(actuator, motorconst, resistance,
nominal, saturation, inductance, nominal, saturation, inductance,
cogging, controller, thermal, lugre, input_mode); cogging, controller, thermal, lugre, ctrlspec);
} }
else if (type == "plugin") { else if (type == "plugin") {
src/xml/xml_native_writer.cc
+4
View File
@@ -838,6 +838,9 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec, WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec,
def->Actuator().ctrlspec); def->Actuator().ctrlspec);
} else if (actuator->ctrlspec != def->Actuator().ctrlspec) { } else if (actuator->ctrlspec != def->Actuator().ctrlspec) {
if (actuator->ctrlspec == mjINPUT_NONE) {
WriteAttrTxt(elem, "input", "none");
} else {
std::string tokens; std::string tokens;
for (int k=0; k < inputbit_sz; k++) { for (int k=0; k < inputbit_sz; k++) {
if (actuator->ctrlspec & inputbit_map[k].value) { if (actuator->ctrlspec & inputbit_map[k].value) {
@@ -846,6 +849,7 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
} }
WriteAttrTxt(elem, "input", tokens); WriteAttrTxt(elem, "input", tokens);
} }
}
WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype); WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype);
WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true); WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true);
WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true); WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true);
test/doc/doc_test.py
+3 -1
View File
@@ -219,7 +219,9 @@ class DocTest(googletest.TestCase):
}, },
} }
# deliberately partial: keywords are a documented subset of the C enum # deliberately partial: keywords are a documented subset of the C enum
partial = {'frameobj'} # (inputbit: combinable tokens exclude the whole-attribute keyword
# mjINPUT_NONE; inputkeyword: the whole-attribute keyword excludes the tokens)
partial = {'frameobj', 'inputbit', 'inputkeyword'}
enums_c = {} enums_c = {}
for name in ('mjtype.h', 'mjspec.h'): for name in ('mjtype.h', 'mjspec.h'):
test/engine/engine_derivative_test.cc
+7 -7
View File
@@ -1414,7 +1414,7 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor name="dc" joint="j" motorconst="2.0" resistance="0.5" <dcmotor name="dc" joint="j" motorconst="2.0" resistance="0.5"
inductance="0 0.001" input="position" controller="10 0 5"/> inductance="0 0.001" input="pos vel" controller="10 0 5"/>
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
@@ -1437,10 +1437,10 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
// extract diagonal of qDeriv // extract diagonal of qDeriv
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1]; mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
// expected: K*(dVdw - K)*(1 - exp(-h/te))/R // expected: K*(dVdw - K)*(1 - exp(-h/te))/R with the torque-space map
// with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5 // dVdw = -kd*R/K + K, so the expression reduces to -kd*(1 - exp(-h/te))
mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0; mjtNum te = 0.001, h = 0.002, kd = 5.0;
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R; mjtNum expected = -kd * (1 - mju_exp(-h / te));
EXPECT_NEAR(qDeriv_diag, expected, 1e-10) EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
<< "stateful DC motor derivative should match analytical formula"; << "stateful DC motor derivative should match analytical formula";
} }
@@ -1459,7 +1459,7 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0" <dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
input="position" controller="10 0 5"/> input="pos vel" controller="10 0 5"/>
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
@@ -1476,7 +1476,7 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0" <dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
inductance="0 1e-8" input="position" controller="10 0 5"/> inductance="0 1e-8" input="pos vel" controller="10 0 5"/>
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
test/engine/engine_forward_test.cc
+215 -91
View File
@@ -67,6 +67,8 @@ using ::testing::Pointwise;
using ::testing::_; using ::testing::_;
using ::testing::Gt; using ::testing::Gt;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::Ne; using ::testing::Ne;
using ::testing::NotNull; using ::testing::NotNull;
@@ -1452,63 +1454,58 @@ TEST_F(ActuatorTest, DampRatioTendon) {
using DCMotorTest = MujocoTest; using DCMotorTest = MujocoTest;
TEST_F(DCMotorTest, IntVelocityEquivalence) { // A stateless dcmotor with setpoint inputs matches <pid> exactly, for any
// motor constant and resistance: the torque-space controller commands
// kp*(qref - l) + kd*(vref - ldot) and the tau->V map compensates back-EMF.
TEST_F(DCMotorTest, SetpointMatchesPid) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<option integrator="implicit"/> <option timestep="0.001" integrator="implicitfast">
<flag contact="disable" gravity="disable"/>
</option>
<worldbody> <worldbody>
<body pos="0 0 0"> <body>
<joint name="slide1" type="slide" axis="1 0 0"/> <joint name="slide1" type="slide" axis="1 0 0"/>
<geom size=".1"/> <geom size="0.1" mass="1"/>
</body> </body>
<body pos="0 1 0"> <body>
<joint name="slide2" type="slide" axis="1 0 0"/> <joint name="slide2" type="slide" axis="1 0 0"/>
<geom size=".1"/> <geom size="0.1" mass="1"/>
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<!-- <pid name="pid" joint="slide1" kp="10" kv="5"/>
Equivalence mapping: <dcmotor name="dcmotor" joint="slide2" motorconst="0.05" resistance="2.0"
intvelocity force: F = kp * \int(ctrl - v) - kv * v input="pos vel" controller="10 0 5"/>
dcmotor force: F = (V*K - K^2*v) / R
where V = ki * \int(ctrl - v) (since kp=0, kd=0)
Setting K=1, R=0.2, ki=2 yields:
F = (2 * \int(ctrl - v) - v) / 0.2
= 10 * \int(ctrl - v) - 5 * v
This perfectly matches intvelocity with kp=10, kv=5.
-->
<intvelocity name="intvel" joint="slide1" kp="10" kv="5" actrange="-0.01 0.01"/>
<dcmotor name="dcmotor" joint="slide2" motorconst="1" resistance="0.2" input="velocity" controller="0 2 0 0 0.01"/>
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
// both actuators own [pos, vel] blocks
ASSERT_EQ(model->nu, 4);
MjDataPtr data = MakeData(model); MjDataPtr data = MakeData(model);
// Apply a time-varying velocity command // time-varying position and velocity commands, identical for both
while (data->time < 1.0) { while (data->time < 1.0) {
data->ctrl[0] = mju_sin(20 * data->time); mjtNum qref = mju_sin(5 * data->time);
data->ctrl[1] = mju_sin(20 * data->time); mjtNum vref = mju_cos(3 * data->time);
data->ctrl[0] = data->ctrl[2] = qref;
data->ctrl[1] = data->ctrl[3] = vref;
mj_step(model.get(), data.get()); mj_step(model.get(), data.get());
// Both actuators should integrate identical states EXPECT_NEAR(data->qpos[0], data->qpos[1], MjTol(1e-14, 1e-6));
EXPECT_MJTNUM_EQ(data->act[0], data->act[1]); EXPECT_NEAR(data->qvel[0], data->qvel[1], MjTol(1e-14, 1e-6));
// Both bodies should move identically // recompute forces at the post-step state before comparing: identical, the
EXPECT_NEAR(data->qpos[0], data->qpos[1], MjTol(1e-14, 1e-7)); // tau->V map cancels back-EMF so there is no extra damping to account for
EXPECT_NEAR(data->qvel[0], data->qvel[1], MjTol(1e-14, 1e-7)); mj_forward(model.get(), data.get());
EXPECT_NEAR(data->qacc[0], data->qacc[1], MjTol(1e-14, 1e-6));
// Both actuators should produce identical force
EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1], EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1],
MjTol(1e-14, 1e-6)); MjTol(1e-13, 1e-5));
} }
} }
TEST_F(DCMotorTest, StatelessSteadyState) { TEST_F(DCMotorTest, StatelessSteadyState) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -1528,6 +1525,9 @@ TEST_F(DCMotorTest, StatelessSteadyState) {
ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model); MjDataPtr data = MakeData(model);
// the dcmotor ff input is the terminal voltage, named accordingly
EXPECT_STREQ(mj_actuatorInputName(model.get(), 0, 0), "voltage");
double K = 0.05; double K = 0.05;
double R = 2.0; double R = 2.0;
double V = 12.0; double V = 12.0;
@@ -1776,7 +1776,8 @@ TEST_F(DCMotorTest, LuGreBristleVelocityOrderInvariance) {
ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model); MjDataPtr data = MakeData(model);
int dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc"); int dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc");
data->qvel[model->jnt_dofadr[mj_name2id(model.get(), mjOBJ_JOINT, "hinge")]] = 1; data->qvel[model->jnt_dofadr[mj_name2id(model.get(), mjOBJ_JOINT, "hinge")]] =
1;
mj_step(model.get(), data.get()); mj_step(model.get(), data.get());
double z_dc_first = data->act[model->actuator_actadr[dc]]; double z_dc_first = data->act[model->actuator_actadr[dc]];
@@ -1787,7 +1788,8 @@ TEST_F(DCMotorTest, LuGreBristleVelocityOrderInvariance) {
data = MakeData(model); data = MakeData(model);
dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc"); dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc");
ASSERT_EQ(model->actuator_outadr[dc], 3); ASSERT_EQ(model->actuator_outadr[dc], 3);
data->qvel[model->jnt_dofadr[mj_name2id(model.get(), mjOBJ_JOINT, "hinge")]] = 1; data->qvel[model->jnt_dofadr[mj_name2id(model.get(), mjOBJ_JOINT, "hinge")]] =
1;
mj_step(model.get(), data.get()); mj_step(model.get(), data.get());
double z_so3_first = data->act[model->actuator_actadr[dc]]; double z_so3_first = data->act[model->actuator_actadr[dc]];
@@ -1796,6 +1798,134 @@ TEST_F(DCMotorTest, LuGreBristleVelocityOrderInvariance) {
EXPECT_MJTNUM_EQ(z_so3_first, z_dc_first); EXPECT_MJTNUM_EQ(z_so3_first, z_dc_first);
} }
// A dcmotor with input="none" has no controls and acts as a passive device:
// LuGre friction, cogging and back-EMF braking with the terminals shorted.
TEST_F(DCMotorTest, PassiveNoInputs) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0" input="none"
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// no controls at all, one actuator, one bristle state
EXPECT_EQ(model->nu, 0);
EXPECT_EQ(model->nactuator, 1);
EXPECT_EQ(model->actuator_ctrlnum[0], 0);
ASSERT_EQ(model->actuator_actnum[0], 1);
// same force as a voltage-commanded motor with u = 0: shorted terminals
double sigma1 = 1;
double K = 0.05, R = 2.0;
double omega = 2.0;
data->qvel[0] = omega;
mj_forward(model.get(), data.get());
double electrical_force = K / R * (0 - K * omega);
double z = data->act[model->actuator_actadr[0]];
double z_dot = data->act_dot[model->actuator_actadr[0]];
double lugre_force = 100 * z + sigma1 * z_dot;
EXPECT_NEAR(data->actuator_force[0], electrical_force - lugre_force,
MjTol(1e-12, 1e-5));
// the model steps with an empty ctrl vector: friction brakes the joint
for (int i = 0; i < 100; i++) {
mj_step(model.get(), data.get());
}
EXPECT_GT(data->act[model->actuator_actadr[0]], 0);
EXPECT_LT(data->qvel[0], omega);
}
// input="none" validation: dcmotor-only, incompatible with slew and ki.
TEST_F(DCMotorTest, NoInputsCompileErrors) {
char error[1024];
// pid does not accept input="none"
static constexpr char pid_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<pid joint="joint" kp="1" input="none"/>
</actuator>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(pid_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("subset"));
// slew rate limiting requires an input
static constexpr char slew_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0" input="none"
controller="0 0 0 4.0 0 0"/>
</actuator>
</mujoco>
)";
model = LoadModelFromString(slew_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("slew"));
// the "voltage" keyword is dcmotor-only
static constexpr char voltage_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<pid joint="joint" kp="1" input="voltage"/>
</actuator>
</mujoco>
)";
model = LoadModelFromString(voltage_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
// integral gain requires the pos input
static constexpr char ki_xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0" input="vel ff"
controller="0 2.0 0 0 0 0"/>
</actuator>
</mujoco>
)";
model = LoadModelFromString(ki_xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("pos input"));
}
TEST_F(DCMotorTest, ThermalRiseAndFall) { TEST_F(DCMotorTest, ThermalRiseAndFall) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -1928,7 +2058,7 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) {
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0" <dcmotor joint="joint" motorconst="0.05" resistance="2.0"
input="position" controller="1.0 1.0 0 5.0 0" input="pos vel" controller="1.0 1.0 0 5.0 0"
thermal="0.1 0.1 0 0.004 25 25"/> thermal="0.1 0.1 0 0.004 25 25"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -1943,7 +2073,7 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) {
int adr = model->actuator_actadr[0]; int adr = model->actuator_actadr[0];
int temp_adr = adr + 2; // temperature is slot 2 int temp_adr = adr + 2; // temperature is slot 2
double K = 0.05, R = 2.0, alpha = 0.004; double R = 2.0, alpha = 0.004;
double dT = 50; double dT = 50;
data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied
data->act[adr + 1] = 0.0; // integral state x_I = 0 data->act[adr + 1] = 0.0; // integral state x_I = 0
@@ -1952,11 +2082,11 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) {
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// u_eff = ctrl = 1.0 (no slew applied since act[slew] == ctrl) // u_eff = ctrl = 1.0 (no slew applied since act[slew] == ctrl)
// V = kp*(u_eff - length) + ki*x_I - kd*omega = 1.0*1.0 + 1.0*0.0 - 0*0 = 1.0 // torque command tau = kp*(u_eff - length) = 1.0; the tau->V map uses the
// R(T) = 2.0 * (1 + 0.004 * 50) = 2.4 // nameplate resistance, so the hot motor under-delivers by R/R(T):
// stateless (no te): force = K/R(T) * V = 0.05/2.4 * 1.0 // V = R/K * tau = 40, force = K/R(T) * V = (R/R(T)) * tau
double R_hot = R * (1 + alpha * dT); double R_hot = R * (1 + alpha * dT);
EXPECT_NEAR(data->actuator_force[0], K / R_hot * 1.0, MjTol(1e-12, 1e-5)); EXPECT_NEAR(data->actuator_force[0], R / R_hot * 1.0, MjTol(1e-12, 1e-5));
} }
TEST_F(DCMotorTest, StatelessPositionMode) { TEST_F(DCMotorTest, StatelessPositionMode) {
@@ -1970,7 +2100,7 @@ TEST_F(DCMotorTest, StatelessPositionMode) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" input="position" controller="2.0 0 0.5 0 0" <dcmotor joint="joint" input="pos vel" controller="2.0 0 0.5 0 0"
motorconst="0.05" resistance="2.0"/> motorconst="0.05" resistance="2.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -1980,21 +2110,23 @@ TEST_F(DCMotorTest, StatelessPositionMode) {
ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model); MjDataPtr data = MakeData(model);
// setpoint inputs are named
EXPECT_STREQ(mj_actuatorInputName(model.get(), 0, 0), "pos");
EXPECT_STREQ(mj_actuatorInputName(model.get(), 0, 1), "vel");
// Position target 5.0, current pos 0.0, current vel 0.0 // Position target 5.0, current pos 0.0, current vel 0.0
data->ctrl[0] = 5.0; data->ctrl[0] = 5.0;
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// V = Kp * (u - theta) = 2.0 * 5.0 = 10.0 // torque-space controller with back-EMF compensation: force = kp*(qref - l)
// force = K / R * V + bias = (0.05 / 2.0) * 10.0 + 0 = 0.25 // force = 2.0 * 5.0 = 10.0, exactly
EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5)); EXPECT_NEAR(data->actuator_force[0], 10.0, MjTol(1e-12, 1e-5));
// Velocity penalty // Velocity penalty
data->qvel[0] = 2.0; data->qvel[0] = 2.0;
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// V = 10.0 - Kd * omega = 10.0 - (0.5 * 2.0) = 9.0 // force = kp*(qref - l) - kd*omega = 10.0 - 0.5*2.0 = 9.0: no back-EMF droop
// bias = - K^2 / R * omega = -0.0025 / 2.0 * 2.0 = -0.0025 EXPECT_NEAR(data->actuator_force[0], 9.0, MjTol(1e-12, 1e-5));
// force = K / R * V + bias = 0.225 - 0.0025 = 0.2225
EXPECT_NEAR(data->actuator_force[0], 0.2225, MjTol(1e-12, 1e-5));
} }
TEST_F(DCMotorTest, StatelessVelocityMode) { TEST_F(DCMotorTest, StatelessVelocityMode) {
@@ -2008,7 +2140,7 @@ TEST_F(DCMotorTest, StatelessVelocityMode) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" input="velocity" controller="3.0 0 0 0 0" <dcmotor joint="joint" input="pos vel" controller="0 0 3.0 0 0"
motorconst="0.05" resistance="2.0"/> motorconst="0.05" resistance="2.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -2018,15 +2150,13 @@ TEST_F(DCMotorTest, StatelessVelocityMode) {
ASSERT_THAT(model.get(), NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model); MjDataPtr data = MakeData(model);
// Velocity target 4.0, current vel 1.0 // Velocity target 4.0 (second input), current vel 1.0
data->ctrl[0] = 4.0; data->ctrl[1] = 4.0;
data->qvel[0] = 1.0; data->qvel[0] = 1.0;
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// V = Kp * (u - omega) = 3.0 * (4.0 - 1.0) = 9.0 // force = kd * (vref - omega) = 3.0 * (4.0 - 1.0) = 9.0, exactly
// bias = - K^2 / R * omega = -0.0025 / 2.0 * 1.0 = -0.00125 EXPECT_NEAR(data->actuator_force[0], 9.0, MjTol(1e-12, 1e-5));
// force = K / R * V + bias = (0.05 / 2.0) * 9.0 - 0.00125 = 0.22375
EXPECT_NEAR(data->actuator_force[0], 0.22375, MjTol(1e-12, 1e-5));
} }
TEST_F(DCMotorTest, StatefulPositionMode) { TEST_F(DCMotorTest, StatefulPositionMode) {
@@ -2040,7 +2170,7 @@ TEST_F(DCMotorTest, StatefulPositionMode) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0.1 10.0 5.0" <dcmotor joint="joint" input="pos vel" controller="2.0 0.5 0.1 10.0 5.0"
motorconst="0.05" resistance="2.0"/> motorconst="0.05" resistance="2.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -2072,11 +2202,9 @@ TEST_F(DCMotorTest, StatefulPositionMode) {
// PI error: error = u_eff - length = 1.01 - 0.0 = 1.01 // PI error: error = u_eff - length = 1.01 - 0.0 = 1.01
EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5)); EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5));
// V = Kp(u_eff - length) + Ki * x_I - Kd * omega // force = kp * (u_eff - length) + ki * x_I - kd * omega
// V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 // = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97, exactly
// bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 0.5 = -0.000625 EXPECT_NEAR(data->actuator_force[0], 2.97, MjTol(1e-12, 1e-5));
// force = K/R * V + bias = 0.025 * 2.97 - 0.000625 = 0.073625
EXPECT_NEAR(data->actuator_force[0], 0.073625, MjTol(1e-12, 1e-5));
} }
TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) {
@@ -2090,7 +2218,7 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0.1 10.0 5.0" <dcmotor joint="joint" input="pos vel" controller="2.0 0.5 0.1 10.0 5.0"
motorconst="0.05" resistance="2.0" inductance="1.0"/> motorconst="0.05" resistance="2.0" inductance="1.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -2122,16 +2250,16 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) {
// PI error: error = u_eff - length = 1.01 // PI error: error = u_eff - length = 1.01
EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5)); EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5));
// Voltage computation: // torque command: tau = kp * (u_eff - length) + ki * x_I - kd * omega
// V = Kp(u_eff - length) + Ki * x_I - Kd * omega // = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97
// V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 // tau->V map with back-EMF compensation:
// V = R/K * tau + K * omega = 2.0/0.05 * 2.97 + 0.05 * 0.5 = 118.825
// Current filter: // Current filter:
// t_e = L / R = 1.0 / 2.0 = 0.5 // t_e = L / R = 1.0 / 2.0 = 0.5
// di/dt = (V/R - K/R * omega - i) / t_e // di/dt = (V/R - K/R * omega - i) / t_e
// di/dt = (2.97/2.0 - 0.05/2.0 * 0.5 - 0.5) / 0.5 // di/dt = (118.825/2.0 - 0.05/2.0 * 0.5 - 0.5) / 0.5 = 117.8
// di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945 EXPECT_NEAR(data->act_dot[adr + 2], 117.8, MjTol(1e-12, 1e-4));
EXPECT_NEAR(data->act_dot[adr + 2], 1.945, MjTol(1e-12, 1e-5));
// Force is K * next_activation (actearly is always on for DC motors) // Force is K * next_activation (actearly is always on for DC motors)
// Inline mj_nextActivation for te = 0.5 // Inline mj_nextActivation for te = 0.5
@@ -2152,7 +2280,7 @@ TEST_F(DCMotorTest, StatefulVelocityMode) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" input="velocity" controller="3.0 1.0 0 0 2.0" <dcmotor joint="joint" input="pos vel" controller="3.0 1.0 0 0 2.0"
motorconst="0.05" resistance="2.0"/> motorconst="0.05" resistance="2.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -2169,28 +2297,23 @@ TEST_F(DCMotorTest, StatefulVelocityMode) {
double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0) double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0)
data->act[adr] = x_I; data->act[adr] = x_I;
// target vel 4.0, current vel 1.0 // position target 4.0, current pos 0, current vel 1.0 (kd = 0)
data->ctrl[0] = 4.0; data->ctrl[0] = 4.0;
data->qvel[0] = 1.0; data->qvel[0] = 1.0;
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// integrate command directly: error = target = 4.0 // error = 4.0; since x_I == Imax (2.0) and error > 0, act_dot clamps to 0
// since x_I == Imax (2.0) and error (4.0) > 0, act_dot should be clamped to 0
EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5));
// V = Kp * (u_eff - omega) + Ki * (x_I - length) // force = kp * (qref - l) + ki * x_I = 3.0 * (4.0 - 0.0) + 1.0 * 2.0 = 14.0
// V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 0.0) = 9.0 + 2.0 = 11.0 EXPECT_NEAR(data->actuator_force[0], 14.0, MjTol(1e-12, 1e-5));
// bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 1.0 = -0.00125
// force = K/R * V + bias = 0.025 * 11.0 - 0.00125 = 0.275 - 0.00125 = 0.27375
EXPECT_NEAR(data->actuator_force[0], 0.27375, MjTol(1e-12, 1e-5));
// repeat with non-zero joint position // repeat with non-zero joint position
data->qpos[0] = 1.5; data->qpos[0] = 1.5;
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 1.5) = 9.0 + 0.5 = 9.5 // force = 3.0 * (4.0 - 1.5) + 1.0 * 2.0 = 9.5
// force = K/R * V + bias = 0.025 * 9.5 - 0.00125 = 0.2375 - 0.00125 = 0.23625 EXPECT_NEAR(data->actuator_force[0], 9.5, MjTol(1e-12, 1e-5));
EXPECT_NEAR(data->actuator_force[0], 0.23625, MjTol(1e-12, 1e-5));
} }
TEST_F(DCMotorTest, CurrentPlusThermal) { TEST_F(DCMotorTest, CurrentPlusThermal) {
@@ -2300,7 +2423,7 @@ TEST_F(DCMotorTest, VoltageLimit) {
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0" <dcmotor joint="joint" motorconst="0.05" resistance="2.0"
input="position" controller="1 0 0 0 0 10.0"/> input="pos vel" controller="1 0 0 0 0 10.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
@@ -2335,7 +2458,7 @@ TEST_F(DCMotorTest, IntegralClamp) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0 0 5.0" <dcmotor joint="joint" input="pos vel" controller="2.0 0.5 0 0 5.0"
motorconst="0.05" resistance="2.0"/> motorconst="0.05" resistance="2.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
@@ -3121,9 +3244,8 @@ TEST_F(ForwardTest, FlexParentCoupling) {
if (diff > max_diff) max_diff = diff; if (diff > max_diff) max_diff = diff;
} }
// tolerance rebaselined 2e-5 -> 5e-5 for the in-solver implicit flex treatment: implicit // implicit and explicit flex damping legitimately differ at
// and explicit flex damping legitimately differ at O(h*damping*K/M) in this comparison, and // O(h*damping*K/M) in this comparison
// the in-solver form lands at ~3e-5 where the old post-hoc operator landed just under 2e-5
EXPECT_LT(max_diff, MjTol(5e-5, 1.5e-2)) EXPECT_LT(max_diff, MjTol(5e-5, 1.5e-2))
<< "Implicit integrator should match Euler at small timestep"; << "Implicit integrator should match Euler at small timestep";
} }
@@ -3849,10 +3971,10 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) {
<< ", initial=" << initial_energy; << ", initial=" << initial_energy;
} }
} // namespace // with the implicit effective metric active, inverse dynamics must recover the
// with the implicit effective metric active, inverse dynamics must recover the applied force // applied force (zero here): the forward solve is (M+B)*qacc = qfrc_smooth + c
// (zero here): the forward solve is (M+B)*qacc = qfrc_smooth + c + J'*f and the inverse adds // + J'*f and the inverse adds the same B*qacc - c terms. This is the fwd/inv
// the same B*qacc - c terms. This is the fwd/inv consistency fence for the flex-CG dispatch. // consistency fence for the flex-CG dispatch.
TEST_F(ForwardTest, GatedFlexInverseConsistency) { TEST_F(ForwardTest, GatedFlexInverseConsistency) {
static const char* const kXml = R"( static const char* const kXml = R"(
<mujoco> <mujoco>
@@ -3886,10 +4008,12 @@ TEST_F(ForwardTest, GatedFlexInverseConsistency) {
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mj_inverse(model.get(), data.get()); mj_inverse(model.get(), data.get());
// no applied forces: the inverse must return ~zero, at the scale of the passive forces // no applied forces: the inverse must return ~zero, at the scale of the
// passive forces
mjtNum scale = 1 + mju_norm(data->qfrc_passive, nv); mjtNum scale = 1 + mju_norm(data->qfrc_passive, nv);
EXPECT_LT(mju_norm(data->qfrc_inverse, nv), 1e-6 * scale); EXPECT_LT(mju_norm(data->qfrc_inverse, nv), 1e-6 * scale);
} }
} // namespace
} // namespace mujoco } // namespace mujoco
test/engine/testdata/derivative/dcmotor.xml
Vendored
+5 -5
View File
@@ -34,11 +34,11 @@
<!-- DC motor back-EMF stateless damping --> <!-- DC motor back-EMF stateless damping -->
<dcmotor name="dc_bias" joint="slide1" motorconst="2.0" resistance="0.5"/> <dcmotor name="dc_bias" joint="slide1" motorconst="2.0" resistance="0.5"/>
<!-- DC motor velocity controller damping --> <!-- DC motor velocity setpoint (kd) damping -->
<dcmotor name="dc_vel" joint="slide2" motorconst="1.0" resistance="1.0" input="velocity" controller="0 5"/> <dcmotor name="dc_vel" joint="slide2" motorconst="1.0" resistance="1.0" input="pos vel" controller="0 0 5"/>
<!-- DC motor position controller damping --> <!-- DC motor position controller damping -->
<dcmotor name="dc_pos" joint="slide3" motorconst="1.0" resistance="1.0" input="position" controller="10 0 5"/> <dcmotor name="dc_pos" joint="slide3" motorconst="1.0" resistance="1.0" input="pos vel" controller="10 0 5"/>
<!-- DC motor with LuGre friction (sigma1 micro-damping) --> <!-- DC motor with LuGre friction (sigma1 micro-damping) -->
<dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0" <dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0"
@@ -51,11 +51,11 @@
<!-- Stateful current, position mode (controller + back-EMF through act_dot) --> <!-- Stateful current, position mode (controller + back-EMF through act_dot) -->
<dcmotor name="dc_stateful_pos" joint="slide6" <dcmotor name="dc_stateful_pos" joint="slide6"
motorconst="1.0" resistance="1.0" inductance="0 0.001" motorconst="1.0" resistance="1.0" inductance="0 0.001"
input="position" controller="10 0 5"/> input="pos vel" controller="10 0 5"/>
<!-- Stateful current, velocity mode --> <!-- Stateful current, velocity mode -->
<dcmotor name="dc_stateful_vel" joint="slide7" <dcmotor name="dc_stateful_vel" joint="slide7"
motorconst="1.0" resistance="1.0" inductance="0 0.001" motorconst="1.0" resistance="1.0" inductance="0 0.001"
input="velocity" controller="5 0"/> input="pos vel" controller="0 0 5"/>
</actuator> </actuator>
</mujoco> </mujoco>
test/xml/xml_native_reader_test.cc
+5 -4
View File
@@ -3149,7 +3149,7 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) {
<mujoco> <mujoco>
<default> <default>
<dcmotor motorconst="1.0" resistance="1.0" controller="2.0 0.5 0.1 10.0 5.0 12.0" <dcmotor motorconst="1.0" resistance="1.0" controller="2.0 0.5 0.1 10.0 5.0 12.0"
saturation="0 0 0" inductance="0 0.01" input="velocity"/> saturation="0 0 0" inductance="0 0.01" input="pos vel"/>
</default> </default>
<worldbody> <worldbody>
<body> <body>
@@ -3182,8 +3182,9 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) {
// check Vmax in gainprm[7] // check Vmax in gainprm[7]
EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 12.0); EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 12.0);
// check input mode in gainprm[8] // check setpoint input block (gainprm[8] is retired)
EXPECT_MJTNUM_EQ(model->actuator_gainprm[8], 2.0); EXPECT_MJTNUM_EQ(model->actuator_gainprm[8], 0.0);
EXPECT_EQ(model->actuator_ctrlnum[0], 2);
// check inductance (te) in dynprm[0] // check inductance (te) in dynprm[0]
EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01); EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01);
@@ -3199,7 +3200,7 @@ TEST_F(ActuatorParseTest, DCMotorControllerFull) {
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0" <dcmotor joint="jnt" motorconst="0.05" resistance="2.0" input="pos vel"
controller="1.0 2.0 3.0 4.0 5.0 6.0"/> controller="1.0 2.0 3.0 4.0 5.0 6.0"/>
</actuator> </actuator>
</mujoco> </mujoco>
test/xml/xml_native_writer_test.cc
+26
View File
@@ -54,6 +54,32 @@ TEST_F(XMLWriterTest, EmptyModel) {
EXPECT_THAT(saved_xml, Not(HasSubstr("default"))); EXPECT_THAT(saved_xml, Not(HasSubstr("default")));
} }
TEST_F(XMLWriterTest, KeepsDCMotorNoInputs) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0" input="none"/>
</actuator>
</mujoco>
)";
MjModelPtr model = LoadModelFromString(xml);
ASSERT_THAT(model.get(), NotNull());
std::string saved_xml = SaveAndReadXml(model.get());
EXPECT_THAT(saved_xml, HasSubstr("input=\"none\""));
// reload: the empty input block survives the round trip
MjModelPtr model2 = LoadModelFromString(saved_xml.c_str());
ASSERT_THAT(model2.get(), NotNull());
EXPECT_EQ(model2->nu, 0);
EXPECT_EQ(model2->actuator_ctrlnum[0], 0);
}
TEST_F(XMLWriterTest, SavesMemory) { TEST_F(XMLWriterTest, SavesMemory) {
{ {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
unity/Runtime/Bindings/MjBindings.cs
+2
View File
@@ -305,6 +305,8 @@ public enum mjtCtrlInput : int{
mjINPUT_POS = 1, mjINPUT_POS = 1,
mjINPUT_VEL = 2, mjINPUT_VEL = 2,
mjINPUT_FF = 4, mjINPUT_FF = 4,
mjINPUT_VOLTAGE = 8,
mjINPUT_NONE = 16,
} }
public enum mjtObj : int{ public enum mjtObj : int{
mjOBJ_UNKNOWN = 0, mjOBJ_UNKNOWN = 0,
wasm/codegen/generated/bindings.cc
+5 -3
View File
@@ -2884,7 +2884,7 @@ std::string mjs_setToCylinder_wrapper(MjsActuator& actuator, double timeconst, d
return std::string(mjs_setToCylinder(actuator.get(), timeconst, bias, area, diameter)); return std::string(mjs_setToCylinder(actuator.get(), timeconst, bias, area, diameter));
} }
std::string mjs_setToDCMotor_wrapper(MjsActuator& actuator, const val& motorconst, double resistance, const val& nominal, const val& saturation, const val& inductance, const val& cogging, const val& controller, const val& thermal, const val& lugre, int input_mode) { std::string mjs_setToDCMotor_wrapper(MjsActuator& actuator, const val& motorconst, double resistance, const val& nominal, const val& saturation, const val& inductance, const val& cogging, const val& controller, const val& thermal, const val& lugre, int ctrlspec) {
UNPACK_NULLABLE_VALUE(double, motorconst); UNPACK_NULLABLE_VALUE(double, motorconst);
UNPACK_NULLABLE_VALUE(double, nominal); UNPACK_NULLABLE_VALUE(double, nominal);
UNPACK_NULLABLE_VALUE(double, saturation); UNPACK_NULLABLE_VALUE(double, saturation);
@@ -2893,7 +2893,7 @@ std::string mjs_setToDCMotor_wrapper(MjsActuator& actuator, const val& motorcons
UNPACK_NULLABLE_VALUE(double, controller); UNPACK_NULLABLE_VALUE(double, controller);
UNPACK_NULLABLE_VALUE(double, thermal); UNPACK_NULLABLE_VALUE(double, thermal);
UNPACK_NULLABLE_VALUE(double, lugre); UNPACK_NULLABLE_VALUE(double, lugre);
return std::string(mjs_setToDCMotor(actuator.get(), motorconst_.data(), resistance, nominal_.data(), saturation_.data(), inductance_.data(), cogging_.data(), controller_.data(), thermal_.data(), lugre_.data(), input_mode)); return std::string(mjs_setToDCMotor(actuator.get(), motorconst_.data(), resistance, nominal_.data(), saturation_.data(), inductance_.data(), cogging_.data(), controller_.data(), thermal_.data(), lugre_.data(), ctrlspec));
} }
std::string mjs_setToDamper_wrapper(MjsActuator& actuator, double kv) { std::string mjs_setToDamper_wrapper(MjsActuator& actuator, double kv) {
@@ -3993,7 +3993,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) {
enum_<mjtCtrlInput>("mjtCtrlInput") enum_<mjtCtrlInput>("mjtCtrlInput")
.value("mjINPUT_POS", mjINPUT_POS) .value("mjINPUT_POS", mjINPUT_POS)
.value("mjINPUT_VEL", mjINPUT_VEL) .value("mjINPUT_VEL", mjINPUT_VEL)
.value("mjINPUT_FF", mjINPUT_FF); .value("mjINPUT_FF", mjINPUT_FF)
.value("mjINPUT_VOLTAGE", mjINPUT_VOLTAGE)
.value("mjINPUT_NONE", mjINPUT_NONE);
enum_<mjtDataType>("mjtDataType") enum_<mjtDataType>("mjtDataType")
.value("mjDATATYPE_REAL", mjDATATYPE_REAL) .value("mjDATATYPE_REAL", mjDATATYPE_REAL)
.value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE) .value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE)