diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 0280c97d..56ecda56 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5691,7 +5691,7 @@ specify them independently. .. _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 model ` section. Repeating that description in somewhat different notation (corresponding to the mjModel and mjData fields involved) we have: @@ -5711,7 +5711,7 @@ specify them independently. .. _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 affine. As already explained in :ref:`Actuation model `, the general formula is: scalar_force = gain_term \* (act or ctrl) + bias_term. @@ -5724,6 +5724,7 @@ specify them independently. fixed gain_term = gainprm[0] affine gain_term = gain_prm[0] + gain_prm[1]*length + gain_prm[2]*velocity muscle gain_term = mju_muscleGain(...) + dcmotor DC motor gain (K or K/R), see :ref:`dcmotor` pid PID controller with setpoint inputs, see :ref:`pid` so3 geodesic orientation servo, computed jointly over 3 force outputs, see :ref:`orientation` user gain_term = mjcb_act_gain(...) @@ -5731,7 +5732,7 @@ specify them independently. .. _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: ======= ================================================================ @@ -5740,6 +5741,7 @@ specify them independently. none bias_term = 0 affine bias_term = biasprm[0] + biasprm[1]*length + biasprm[2]*velocity muscle bias_term = mju_muscleBias(...) + dcmotor DC motor bias: back-EMF, cogging, LuGre friction, see :ref:`dcmotor` so3 damping term of the geodesic orientation servo, see :ref:`orientation` user bias_term = mjcb_act_bias(...) ======= ================================================================ @@ -5784,8 +5786,9 @@ specify them independently. :at:`input`: :at-val:`string, optional` 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 - default) or "quat" (4 controls); see :ref:`orientation/input`. For gaintype "pid" it is - a token list selecting the input subset; see :ref:`pid/input`. + default) or "quat" (4 controls); see :ref:`orientation/input`. For gaintypes "pid" and + "dcmotor" it is a token list selecting the input subset; see :ref:`pid/input` and + :ref:`dcmotor/input`. .. _actuator-general-actearly: @@ -6806,8 +6809,11 @@ the stateless case. - :ref:`resistance`, :ref:`motorconst` and :ref:`nominal` are each optional, but some combination of them is required. See Section 2.1 of the `technical note <_static/dcmotor.pdf>`__. -- The control :ref:`input` semantic is either the voltage applied to the motor terminals (the - default), or a position or velocity target for a :ref:`PID controller`. +- The control block is selected by :ref:`input`: any subset of ``[pos, vel, ff]``, where + ``pos`` and ``vel`` are setpoint inputs to the on-board :ref:`PID 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`), :ref:`cogging torque`, :ref:`thermal resistance variation`, and :ref:`LuGre` friction. @@ -6946,22 +6952,37 @@ This element has the following custom attributes in addition to the common attri .. _actuator-dcmotor-input: -:at:`input`: :at-val:`[voltage, position, velocity], "voltage"` - Specifies the input signal semantics. In "voltage" mode, the control directly sets applied motor voltage. In - "position" or "velocity" modes, the :ref:`PID controller` uses the control as a - reference setpoint relative to the joint trajectory. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) +:at:`input`: :at-val:`string, "voltage"` + Input signature: a space-separated subset of the tokens "pos", "vel", "ff" and "voltage", required in this + canonical order. The ``pos`` and ``vel`` inputs are setpoints for the on-board + :ref:`controller`, and ``ff`` is a torque feedforward added to its output, as for + :ref:`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` and :ref:`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` to zero disables the electrical branch. + (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) .. _actuator-dcmotor-controller: :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` - :at-val:`slewmax` :at-val:`Imax` :at-val:`Vmax`". Depending on the :at:`input` mode, the controller stabilizes - either position or velocity. If the :at:`input` mode is voltage, :at-val:`kp`, :at-val:`ki`, :at-val:`kd` are - ignored. :at-val:`Vmax` sets the maximum drive voltage :math:`v_{\max}` (Volt); in position/velocity modes it clamps - the controller output, in voltage mode it clamps the control signal (if :at:`ctrlrange` is also set, the tighter - limit wins). A value of 0 (the default) disables the respective feature. When positive, :at-val:`slewmax` limits the - setpoint rate-of-change, :at-val:`Imax` clamps the integrator state (anti-windup), and :at-val:`Vmax` clamps the - drive voltage. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) + :at-val:`slewmax` :at-val:`Imax` :at-val:`Vmax`". The gains are in torque space, as for + :ref:`pid`: the controller commands the torque + :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 + :ref:`input` signature, absent setpoints being fixed at zero, and drives the voltage + :math:`v = (R/K)\,\tau + K \dot{l}`, the second term compensating back-EMF as in a current-controlled driver: + commanded torque is delivered exactly until a limit is reached. Torque-space gains from datasheet voltage-space + 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`. + 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: @@ -6982,7 +7003,7 @@ Associate this actuator with an :ref:`engine plugin`. Either :at:`plug .. _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 model ` section. If :ref:`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 diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf index be5c53de..e9945ada 100644 Binary files a/doc/_static/dcmotor.pdf and b/doc/_static/dcmotor.pdf differ diff --git a/doc/changelog.rst b/doc/changelog.rst index 0c2f4bc4..8d06f8dd 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -31,6 +31,28 @@ Actuation identical to :ref:`position`. The input signature is any subset of ``[pos, vel, ff]``, selected by :ref:`input`; absent setpoint inputs are fixed at zero, so the control vector contains no inert entries. +- The :ref:`dcmotor` on-board controller is redesigned: the + :ref:`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`, 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` ("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 ^^^^^^ diff --git a/doc/dcmotor/dcmotor.tex b/doc/dcmotor/dcmotor.tex index 6d91e476..d1cd5980 100644 --- a/doc/dcmotor/dcmotor.tex +++ b/doc/dcmotor/dcmotor.tex @@ -543,6 +543,7 @@ where $t_T = R_T C$ is the thermal time constant. This produces exponential rise \begin{figure}[ht] \centering +\resizebox{\columnwidth}{!}{% \begin{tikzpicture} \pgfmathsetmacro{\Tss}{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) {$T_{ss} = T_a + R_T P$}; \end{axis} -\end{tikzpicture} +\end{tikzpicture}}% \caption{Temperature rise under constant power dissipation $P$. At $t = t_T$, it reaches $(1-1/e) \approx 63\%$ of its steady-state value.} \label{fig:thermal_response} @@ -810,7 +811,7 @@ Attribute & Size & Description \\ \texttt{damping} & 3 & Viscous damping coefficients \\ \texttt{armature} & 1 & Armature inertia \\ \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}$) \\ \bottomrule \end{tabular} @@ -1066,25 +1067,26 @@ Many actuators embed an on-board controller computing drive voltage from positio \begin{table}[H] \centering -\small -\begin{tabular}{@{}lll@{}} +\footnotesize +\begin{tabular}{@{}llp{4.2cm}@{}} \toprule Attribute & Type & Description \\ \midrule -\texttt{input} & keyword & \texttt{voltage}, \texttt{position}, \texttt{velocity} \\ -\texttt{controller} & vector & Gains (mode-dependent) \\ +\texttt{input} & string & subset of \texttt{pos vel ff voltage}, in canonical order; \texttt{none} (passive) \\ +\texttt{controller} & vector & Gains, slew, and voltage saturation ($k_p, k_i, k_d, s, I_{\max}, v_{\max}$) \\ \bottomrule \end{tabular} \caption{Controller attributes. Default \texttt{input} is \texttt{voltage}.} \label{tab:controller_attributes} \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: \begin{figure}[H] \centering +\resizebox{\columnwidth}{!}{% \begin{tikzpicture}[ block/.style={draw, rounded corners=2pt, minimum height=1.6em, 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}, ] % 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 -\node[block, minimum width=5.5cm, minimum height=6.5em, align=center] +% Controller box +\node[block, minimum width=7.2cm, minimum height=6.5em, align=center] (sel) at (0, 1.8) {}; \node[font=\footnotesize\bfseries, anchor=north] at (0, 2.7) - {Controller mode}; + {Controller}; \node[mode] at (0, 1.45) {$\begin{aligned} - \texttt{voltage:}\quad v &= u \\[2pt] - \texttt{position:}\quad v &= k_p(u\!-\!\theta) + k_i x_I - k_d\dot\theta \\[2pt] - \texttt{velocity:}\quad v &= k_p(u\!-\!\dot\theta) + k_i(x_I\!-\!\theta) + \tau &= k_p(\theta^*\!-\!\theta) + k_d(\dot\theta^*\!-\!\dot\theta) + k_i x_I + \tau_{f\!f} \\[2pt] + v &= \text{clip}(\tfrac{R}{K}\tau + K\dot\theta,\, \pm v_{\max}) + v_{\text{raw}} \\[2pt] + &\phantom{=}\ \text{absent inputs are zero;}\quad \text{\texttt{input=\textquotedbl none\textquotedbl:}}\ v = 0 \end{aligned}$}; -% arrow ctrl to mode +% arrow ctrl to controller \draw[arr] (ctrl.south) -- (sel.north); % 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$}; \draw[arr] (motor.south) -- (tau.north); -\end{tikzpicture} -\caption{Controller pipeline. The \texttt{input} attribute selects how $v$ is derived from \texttt{ctrl}; motor physics is identical downstream.} +\end{tikzpicture}}% +\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} \end{figure} -% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -\subsubsection{Position Mode} -\label{sec:position_mode} +\subsubsection{Setpoint Mode} +\label{sec:setpoint_mode} -When \texttt{input="position"}, the user command $u = {}$\texttt{ctrl} is a target position, yielding voltage: -\begin{equation} - v = k_p \, (u - \theta) + k_i \, x_I - k_d \, \dot\theta - \label{eq:position_mode} -\end{equation} -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. +When \texttt{input="pos vel ff"} (or subset thereof), the actuator owns a +block of controls $u = (\theta^*, \dot\theta^*, \tau_{f\!f})$: a position setpoint, a velocity +setpoint and a torque feedforward. Any subset may be selected, and the \texttt{dcmotor}'s \texttt{actuator\_ctrlnum} is variable. +An absent input corresponds to a setpoint frozen at +zero. The controller commands a torque and converts it to drive voltage: +\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: -\begin{equation*} - \dot{x}_I = u - \theta - \label{eq:position_integral} -\end{equation*} -When $k_i = 0$, no integral state is added and the controller reduces to PD. - -\paragraph{Effective torque.} Substituting~\eqref{eq:position_mode} into the stateless torque equation~\eqref{eq:torque_speed}: -\begin{equation*} - \tau = \frac{K}{R} v - \frac{K^2}{R}\dot\theta - = \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 +\texttt{input="none"} selects the {\em empty} input block: the actuator owns no +controls at all and acts as a purely passive device. The terminal voltage is +identically zero, so the motor operates with shorted terminals: back-EMF drives +current through $R$ and brakes the joint (dynamic braking), while cogging +\eqref{eq:cogging} and LuGre friction (\S\ref{sec:lugre}) act as passive joint +forces. Setting \texttt{motorconst} to zero decouples the electrical branch, +leaving pure friction and cogging. This exists because MuJoCo associates auxiliary +dynamic states (such as the LuGre bristle) with actuators, not joints: a passive +\texttt{dcmotor} is the idiomatic way to attach these passive phenomena to a joint. +Controller gains, slew rate limiting and integral gain require a controller input and are rejected. % - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \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{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} @@ -1245,12 +1198,12 @@ This prevents controller windup even when drive signals are saturated. \subsubsection{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*} v \leftarrow \text{clip}(v, \pm v_{\max}) \label{eq:vlimit} \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] \centering @@ -1259,11 +1212,11 @@ This differs from \texttt{ctrlrange} (clamping user command $u$) and \texttt{for \toprule Attribute & Symbol & Units \\ \midrule -\atKP{} & $k_p$ & mode-dependent \\ -\atKI{} & $k_i$ & mode-dependent \\ -\atKD{} & $k_d$ & V$\cdot$s/rad \\ +\atKP{} & $k_p$ & N$\cdot$m/rad \\ +\atKI{} & $k_i$ & N$\cdot$m/(rad$\cdot$s) \\ +\atKD{} & $k_d$ & N$\cdot$m$\cdot$s/rad \\ \atSLEW{} & $s$ & ctrl-units/s \\ -\atIMAXINT{} & $I_{\max}$ & mode-dependent \\ +\atIMAXINT{} & $I_{\max}$ & rad$\cdot$s \\ \atVMAX{} & $v_{\max}$ & Volt \\ \bottomrule \end{tabular} @@ -1272,6 +1225,8 @@ Attribute & Symbol & Units \\ \end{table} % --------------------------------------------------------------------------- +\vfill\newpage + % Low-Level Semantics % --------------------------------------------------------------------------- \subsection{Low-Level Semantics} @@ -1294,7 +1249,7 @@ Array & Index & Symbol & Description \\ & 5 & $k_i$ & Controller integral gain \\ & 6 & $k_d$ & Controller derivative gain \\ & 7 & $v_{\max}$ & Voltage saturation (V) \\ -& 8 & --- & Input mode (0:\ $v$, 1:\ $\theta$, 2:\ $\dot\theta$) \\ +& 8 & --- & Reserved \\ \midrule \texttt{dynprm} & 0 & $t_e$ & Electrical time constant (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 \\ \midrule \texttt{gear} & 0 & $N$ & Gear ratio \\ +\midrule +\texttt{ctrlspec} & 0 & --- & Input signature (\texttt{mjtCtrlInput} bitmask) \\ \bottomrule \end{tabular} \caption{\texttt{mjModel} array semantics for the \texttt{dcmotor} actuator.} diff --git a/doc/includes/references.h b/doc/includes/references.h index 4ce5b8be..497f754a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2548,7 +2548,9 @@ typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): or typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits mjINPUT_POS = 1, // position 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; typedef enum mjtObj { // type of MujoCo object 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, double nominal[3], double saturation[3], double inductance[2], 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); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); diff --git a/include/mujoco/mjtype.h b/include/mujoco/mjtype.h index 65c99e15..1d3f0937 100644 --- a/include/mujoco/mjtype.h +++ b/include/mujoco/mjtype.h @@ -282,7 +282,9 @@ typedef enum mjtCtrlChart { // so3 input signature (actuator_ctrlspec): or typedef enum mjtCtrlInput { // servo input signature (actuator_ctrlspec): present-input bits mjINPUT_POS = 1, // position 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; diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 3dd2d36b..68f9832b 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -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, double nominal[3], double saturation[3], double inductance[2], double cogging[3], double controller[6], double thermal[6], - double lugre[5], int input_mode); + double lugre[5], int ctrlspec); //---------------------------------- Assets -------------------------------------------------------- diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index b235fcd8..7967bffe 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -314,6 +314,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjINPUT_POS', 1), ('mjINPUT_VEL', 2), ('mjINPUT_FF', 4), + ('mjINPUT_VOLTAGE', 8), + ('mjINPUT_NONE', 16), ]), )), ('mjtObj', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 4e61aa68..c1ba0996 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -11314,7 +11314,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ nullable=True, ), FunctionParameterDecl( - name='input_mode', + name='ctrlspec', type=ValueType(name='int'), ), ), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 68b5c4ac..d2719612 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1639,11 +1639,11 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) { std::array nominal, std::array saturation, std::array inductance, std::array cogging, std::array controller, std::array thermal, - std::array lugre, int input_mode) { + std::array lugre, int ctrlspec) { std::string err = mjs_setToDCMotor( self, motorconst.data(), resistance, nominal.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()) { throw pybind11::value_error(err); } @@ -1656,7 +1656,7 @@ PYBIND11_MODULE(_specs, m, pybind11::mod_gil_not_used()) { py::arg("controller") = std::array{0, 0, 0, 0, 0, 0}, py::arg("thermal") = std::array{0, 0, 0, 0, 0, 0}, py::arg("lugre") = std::array{0, 0, 0, 0, 0}, - py::arg("input_mode") = 0); + py::arg("ctrlspec") = 0); // ============================= MJSTENDONPATH =============================== // helper struct for tendon path indexing diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 1cc88526..e164b7f3 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -2433,11 +2433,16 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; mjtNum te = dynprm[0]; - // controller velocity derivative: dV/dω - int input_mode = (int)gainprm[8]; + // controller velocity derivative dV/dw: torque-space kd through the tau->V map, + // 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; - if (input_mode == 1) dVdw = -gainprm[6]; // position: -kd - else if (input_mode == 2) dVdw = -gainprm[4]; // velocity: -kp + if (m->actuator_ctrlspec[i] & (mjINPUT_POS | mjINPUT_VEL | mjINPUT_FF)) { + 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) { // stateful current with actearly: d(K*next_act)/dω diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index e3505e31..858a25a8 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -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] // 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; - out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0; - out[1] = (spec & mjINPUT_VEL) ? u[adr++] : 0; - out[2] = (spec & mjINPUT_FF) ? u[adr] : 0; + out[0] = (spec & mjINPUT_POS) ? u[adr++] : 0; + out[1] = (spec & mjINPUT_VEL) ? 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 -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) { - int input_mode = (int)gainprm[8]; - mjtNum Vmax = gainprm[7]; - mjtNum voltage; + mjtNum voltage = 0; - // get voltage - if (input_mode > 0) { + // unpack present inputs in canonical order [pos, vel, ff, voltage]; absent input: 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 ki = gainprm[5]; // integral 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) { - // position mode - voltage = kp * (ctrl - length) + ki * x_I - kd * velocity; - } else { - // velocity mode - voltage = kp * (ctrl - velocity) + ki * (x_I - length); - } - } else { - voltage = ctrl; + // torque mode is current control: V = R/K * torque + K * velocity, the second + // term compensating back-EMF; the compiler requires K > 0 on this path + mjtNum R = gainprm[0]; + mjtNum K = gainprm[1]; + voltage = R/K * torque + K*velocity; + + // driver supply limit + mjtNum Vmax = gainprm[7]; + if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax); } - // clip voltage - 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 mjtNum slew_s = dynprm[7]; // slew rate limit if (slew_s > 0) { - mjtNum u_prev = d->act[adr]; - mjtNum slew = slew_s * m->opt.timestep; - 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; + ctrl[uadr] = slewLimit(ctrl[uadr], d->act[adr], slew_s, m->opt.timestep, + 0, d->act_dot + adr); adr++; } - // controller state: integral state + // controller state: integral of the position error (setpoint mode only) mjtNum x_I = 0; if (ki > 0) { x_I = d->act[adr]; - int input_mode = (int)gainprm[8]; mjtNum Imax = dynprm[8]; // integral clamp - mjtNum act_dot = ctrl[uadr]; // default raw accumulator for voltage and velocity modes - - // position mode - if (input_mode == 1) { - act_dot = ctrl[uadr] - d->actuator_length[oadr]; - } + mjtNum act_dot = ctrl[uadr] - d->actuator_length[oadr]; // clamp act_dot based on integral state 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 - 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 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); // 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; - 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); } break; @@ -769,9 +765,9 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { const mjtNum* prm = m->actuator_biasprm + mjNBIAS*i; // unpack present inputs in canonical order [pos, vel, ff]; absent input: setpoint 0 - mjtNum u3[3]; - unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u3); - mjtNum qref = u3[0], vref = u3[1], ff = u3[2]; + mjtNum u4[4]; + unpackServoInputs(ctrl + uadr, m->actuator_ctrlspec[i], u4); + mjtNum qref = u4[0], vref = u4[1], ff = u4[2]; // position setpoint: representative nearest the length on rotational transmissions mjtNum period = wrapPeriod(m, i); @@ -792,7 +788,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } } 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) mjtNum period = wrapPeriod(m, i); diff --git a/src/engine/engine_name.c b/src/engine/engine_name.c index f3bf8c57..f199076e 100644 --- a/src/engine/engine_name.c +++ b/src/engine/engine_name.c @@ -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]; } - // servo family: input names are the present members of [pos, vel, ff] - if (m->actuator_gaintype[id] == mjGAIN_PID) { - static const char* servo[3] = {"pos", "vel", "ff"}; - static const int bits[3] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF}; + // servo family: input names are the present members of [pos, vel, ff, voltage] + if (m->actuator_gaintype[id] == mjGAIN_PID || m->actuator_gaintype[id] == mjGAIN_DCMOTOR) { + static const char* servo[4] = {"pos", "vel", "ff", "voltage"}; + static const int bits[4] = {mjINPUT_POS, mjINPUT_VEL, mjINPUT_FF, mjINPUT_VOLTAGE}; 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 (input == 0) { return servo[k]; diff --git a/src/user/user_api.cc b/src/user/user_api.cc index d2d51abf..fba218a5 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1506,7 +1506,7 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) { const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, double nominal[3], double saturation[3], double inductance[2], 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 Kt = motorconst ? motorconst[0] : 0; // torque 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 - actuator->gainprm[8] = input_mode; + actuator->gainprm[8] = 0; // reserved (was input_mode) + actuator->ctrlspec = ctrlspec; actuator->actdim = actdim; // enforce actlimited = 0; homogeneous bounds are invalid across DC motor states diff --git a/src/user/user_api.h b/src/user/user_api.h index 291b1ab5..77b5c03f 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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, double nominal[3], double saturation[3], double inductance[2], double cogging[3], double controller[6], double thermal[6], - double lugre[5], int input_mode); + double lugre[5], int ctrlspec); //---------------------------------- Add assets ---------------------------------------------------- diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 68f8075c..37012236 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7233,16 +7233,49 @@ void mjCActuator::Compile(void) { !!(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 if (dyntype == mjDYN_PID && gaintype != mjGAIN_PID) { throw mjCError(this, "dyntype 'pid' requires gaintype 'pid', actuator '%s' (id = %d)", name.c_str(), id); } - // input signature selection is so3- or pid-only - if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID) { - throw mjCError(this, "input is only available for so3 and pid actuators, actuator '%s' " - "(id = %d)", name.c_str(), id); + // input signature selection is so3-, pid- or dcmotor-only + if (ctrlspec && gaintype != mjGAIN_SO3 && gaintype != mjGAIN_PID && + gaintype != mjGAIN_DCMOTOR) { + 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 @@ -7273,7 +7306,7 @@ void mjCActuator::Compile(void) { double* range; if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT || 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; } else if (dyntype == mjDYN_INTEGRATOR) { // intvelocity actuator @@ -7417,6 +7450,10 @@ void mjCActuator::Compile(void) { if (delay > 0 && nsample <= 0) { 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 // 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); } - // 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++) { ctrllimiteds_[j] = (mjtByte)is_ctrllimited(); ctrlranges_[j][0] = ctrlrange[0]; diff --git a/src/xml/generated/mjcf.xsd b/src/xml/generated/mjcf.xsd index fcdcfe3a..8ee7be6f 100644 --- a/src/xml/generated/mjcf.xsd +++ b/src/xml/generated/mjcf.xsd @@ -223,11 +223,12 @@ - + + + whole-attribute keyword: the empty signature + - - - + @@ -252,12 +253,13 @@ - bitflags: keywords combine bitwise + bitflags: tokens combine bitwise, canonical order enforced + @@ -1880,7 +1882,11 @@ - + + + token subset, or the none keyword + + @@ -3055,7 +3061,11 @@ - + + + token subset, or the none keyword + + diff --git a/src/xml/generated/mjcf_map.h b/src/xml/generated/mjcf_map.h index 25be5c4e..df4ecac4 100644 --- a/src/xml/generated/mjcf_map.h +++ b/src/xml/generated/mjcf_map.h @@ -243,13 +243,11 @@ inline constexpr mjMap dyn_map[] = { }; inline constexpr int dyn_sz = 8; -// enum dcmotorinput -inline constexpr mjMap dcmotorinput_map[] = { - {"voltage", 0}, - {"position", 1}, - {"velocity", 2}, +// enum inputkeyword +inline constexpr mjMap inputkeyword_map[] = { + {"none", mjINPUT_NONE}, }; -inline constexpr int dcmotorinput_sz = 3; +inline constexpr int inputkeyword_sz = 1; // enum gain inline constexpr mjMap gain_map[] = { @@ -272,11 +270,12 @@ inline constexpr int inputchart_sz = 2; // enum inputbit inline constexpr mjMap inputbit_map[] = { - {"pos", mjINPUT_POS}, - {"vel", mjINPUT_VEL}, - {"ff", mjINPUT_FF}, + {"pos", mjINPUT_POS}, + {"vel", mjINPUT_VEL}, + {"ff", mjINPUT_FF}, + {"voltage", mjINPUT_VOLTAGE}, }; -inline constexpr int inputbit_sz = 3; +inline constexpr int inputbit_sz = 4; // enum bias inline constexpr mjMap bias_map[] = { diff --git a/src/xml/mjcf.schema b/src/xml/mjcf.schema index a5572d17..421e4188 100644 --- a/src/xml/mjcf.schema +++ b/src/xml/mjcf.schema @@ -237,10 +237,8 @@ enum dyn : mjtDyn { user = mjDYN_USER } -enum dcmotorinput { - voltage = 0 - position = 1 - velocity = 2 +enum inputkeyword : mjtCtrlInput { # whole-attribute keyword: the empty signature + none = mjINPUT_NONE } enum gain : mjtGain { @@ -258,10 +256,11 @@ enum inputchart : mjtCtrlChart { quat = mjCHART_QUAT } -enum inputbit : mjtCtrlInput { # bitflags: keywords combine bitwise - pos = mjINPUT_POS - vel = mjINPUT_VEL - ff = mjINPUT_FF +enum inputbit : mjtCtrlInput { # bitflags: tokens combine bitwise, canonical order enforced + pos = mjINPUT_POS + vel = mjINPUT_VEL + ff = mjINPUT_FF + voltage = mjINPUT_VOLTAGE } enum bias : mjtBias { @@ -1680,7 +1679,7 @@ element dcmotor : mjsActuator { controller : double[1..6] thermal : double[1..6] lugre : double[1..5] - input : enum + input : flags (reading=custom) # token subset, or the none keyword } element actuator_plugin : mjsActuator (xml=plugin) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 43ceb411..a1285758 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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) { std::string text; if (!mjXUtil::ReadAttrTxt(elem, "input", text)) { @@ -1163,11 +1164,21 @@ static bool ReadInputSpec(tinyxml2::XMLElement* elem, int* ctrlspec) { 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 nbit = mjXUtil::MapValues(elem, "input", bits, inputbit_map, inputbit_sz); int spec = 0; 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]; } *ctrlspec = spec; @@ -1235,7 +1246,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { // explicit attributes string err; 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); } @@ -1431,7 +1442,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { inherited ? actuator->biasprm[3] : 0, inherited ? actuator->biasprm[4] : 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, "resistance", 1, &resistance, text); 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, "thermal", 6, thermal, text, false, false); ReadAttr(elem, "lugre", 5, lugre, text, false, false); - if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) { - // successfully parsed - } + ReadInputSpec(elem, &ctrlspec); err = mjs_setToDCMotor(actuator, motorconst, resistance, nominal, saturation, inductance, - cogging, controller, thermal, lugre, input_mode); + cogging, controller, thermal, lugre, ctrlspec); } else if (type == "plugin") { diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index d987a090..f5431818 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -838,13 +838,17 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe WriteAttrKey(elem, "input", inputchart_map, inputchart_sz, actuator->ctrlspec, def->Actuator().ctrlspec); } else if (actuator->ctrlspec != def->Actuator().ctrlspec) { - std::string tokens; - for (int k=0; k < inputbit_sz; k++) { - if (actuator->ctrlspec & inputbit_map[k].value) { - tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key; + if (actuator->ctrlspec == mjINPUT_NONE) { + WriteAttrTxt(elem, "input", "none"); + } else { + std::string tokens; + for (int k=0; k < inputbit_sz; k++) { + if (actuator->ctrlspec & inputbit_map[k].value) { + tokens += std::string(tokens.empty() ? "" : " ") + inputbit_map[k].key; + } } + WriteAttrTxt(elem, "input", tokens); } - WriteAttrTxt(elem, "input", tokens); } WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype); WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true); diff --git a/test/doc/doc_test.py b/test/doc/doc_test.py index b0b2009b..f542ddd6 100644 --- a/test/doc/doc_test.py +++ b/test/doc/doc_test.py @@ -219,7 +219,9 @@ class DocTest(googletest.TestCase): }, } # 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 = {} for name in ('mjtype.h', 'mjspec.h'): diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index f5847720..e31e48ab 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1414,7 +1414,7 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) { + inductance="0 0.001" input="pos vel" controller="10 0 5"/> )"; @@ -1437,10 +1437,10 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) { // extract diagonal of qDeriv mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1]; - // expected: K*(dVdw - K)*(1 - exp(-h/te))/R - // with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5 - mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0; - mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R; + // expected: K*(dVdw - K)*(1 - exp(-h/te))/R with the torque-space map + // dVdw = -kd*R/K + K, so the expression reduces to -kd*(1 - exp(-h/te)) + mjtNum te = 0.001, h = 0.002, kd = 5.0; + mjtNum expected = -kd * (1 - mju_exp(-h / te)); EXPECT_NEAR(qDeriv_diag, expected, 1e-10) << "stateful DC motor derivative should match analytical formula"; } @@ -1459,7 +1459,7 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) { + input="pos vel" controller="10 0 5"/> )"; @@ -1476,7 +1476,7 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) { + inductance="0 1e-8" input="pos vel" controller="10 0 5"/> )"; diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 2f89696f..f4893b43 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -67,6 +67,8 @@ using ::testing::Pointwise; using ::testing::_; using ::testing::Gt; +using ::testing::HasSubstr; +using ::testing::IsNull; using ::testing::Ne; using ::testing::NotNull; @@ -1452,63 +1454,58 @@ TEST_F(ActuatorTest, DampRatioTendon) { using DCMotorTest = MujocoTest; -TEST_F(DCMotorTest, IntVelocityEquivalence) { +// A stateless dcmotor with setpoint inputs matches 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"( - - + - + - + - + - - - + + )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; + + // both actuators own [pos, vel] blocks + ASSERT_EQ(model->nu, 4); MjDataPtr data = MakeData(model); - // Apply a time-varying velocity command + // time-varying position and velocity commands, identical for both while (data->time < 1.0) { - data->ctrl[0] = mju_sin(20 * data->time); - data->ctrl[1] = mju_sin(20 * data->time); + mjtNum qref = mju_sin(5 * 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()); - // Both actuators should integrate identical states - EXPECT_MJTNUM_EQ(data->act[0], data->act[1]); + EXPECT_NEAR(data->qpos[0], data->qpos[1], MjTol(1e-14, 1e-6)); + EXPECT_NEAR(data->qvel[0], data->qvel[1], MjTol(1e-14, 1e-6)); - // Both bodies should move identically - EXPECT_NEAR(data->qpos[0], data->qpos[1], MjTol(1e-14, 1e-7)); - EXPECT_NEAR(data->qvel[0], data->qvel[1], MjTol(1e-14, 1e-7)); - EXPECT_NEAR(data->qacc[0], data->qacc[1], MjTol(1e-14, 1e-6)); - - // Both actuators should produce identical force + // recompute forces at the post-step state before comparing: identical, the + // tau->V map cancels back-EMF so there is no extra damping to account for + mj_forward(model.get(), data.get()); EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1], - MjTol(1e-14, 1e-6)); + MjTol(1e-13, 1e-5)); } } - TEST_F(DCMotorTest, StatelessSteadyState) { static constexpr char xml[] = R"( @@ -1528,6 +1525,9 @@ TEST_F(DCMotorTest, StatelessSteadyState) { ASSERT_THAT(model.get(), NotNull()) << error; 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 R = 2.0; double V = 12.0; @@ -1776,7 +1776,8 @@ TEST_F(DCMotorTest, LuGreBristleVelocityOrderInvariance) { ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); 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()); double z_dc_first = data->act[model->actuator_actadr[dc]]; @@ -1787,7 +1788,8 @@ TEST_F(DCMotorTest, LuGreBristleVelocityOrderInvariance) { data = MakeData(model); dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc"); 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()); 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); } +// 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"( + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + )"; + model = LoadModelFromString(voltage_xml, error, sizeof(error)); + EXPECT_THAT(model.get(), IsNull()); + + // integral gain requires the pos input + static constexpr char ki_xml[] = R"( + + + + + + + + + + + + )"; + model = LoadModelFromString(ki_xml, error, sizeof(error)); + EXPECT_THAT(model.get(), IsNull()); + EXPECT_THAT(error, HasSubstr("pos input")); +} + TEST_F(DCMotorTest, ThermalRiseAndFall) { static constexpr char xml[] = R"( @@ -1928,7 +2058,7 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) { @@ -1943,7 +2073,7 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) { int adr = model->actuator_actadr[0]; 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; data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied 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()); // 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 - // R(T) = 2.0 * (1 + 0.004 * 50) = 2.4 - // stateless (no te): force = K/R(T) * V = 0.05/2.4 * 1.0 + // torque command tau = kp*(u_eff - length) = 1.0; the tau->V map uses the + // nameplate resistance, so the hot motor under-delivers by R/R(T): + // V = R/K * tau = 40, force = K/R(T) * V = (R/R(T)) * tau 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) { @@ -1970,7 +2100,7 @@ TEST_F(DCMotorTest, StatelessPositionMode) { - @@ -1980,21 +2110,23 @@ TEST_F(DCMotorTest, StatelessPositionMode) { ASSERT_THAT(model.get(), NotNull()) << error; 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 data->ctrl[0] = 5.0; mj_forward(model.get(), data.get()); - // V = Kp * (u - theta) = 2.0 * 5.0 = 10.0 - // force = K / R * V + bias = (0.05 / 2.0) * 10.0 + 0 = 0.25 - EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5)); + // torque-space controller with back-EMF compensation: force = kp*(qref - l) + // force = 2.0 * 5.0 = 10.0, exactly + EXPECT_NEAR(data->actuator_force[0], 10.0, MjTol(1e-12, 1e-5)); // Velocity penalty data->qvel[0] = 2.0; mj_forward(model.get(), data.get()); - // V = 10.0 - Kd * omega = 10.0 - (0.5 * 2.0) = 9.0 - // bias = - K^2 / R * omega = -0.0025 / 2.0 * 2.0 = -0.0025 - // force = K / R * V + bias = 0.225 - 0.0025 = 0.2225 - EXPECT_NEAR(data->actuator_force[0], 0.2225, MjTol(1e-12, 1e-5)); + // force = kp*(qref - l) - kd*omega = 10.0 - 0.5*2.0 = 9.0: no back-EMF droop + EXPECT_NEAR(data->actuator_force[0], 9.0, MjTol(1e-12, 1e-5)); } TEST_F(DCMotorTest, StatelessVelocityMode) { @@ -2008,7 +2140,7 @@ TEST_F(DCMotorTest, StatelessVelocityMode) { - @@ -2018,15 +2150,13 @@ TEST_F(DCMotorTest, StatelessVelocityMode) { ASSERT_THAT(model.get(), NotNull()) << error; MjDataPtr data = MakeData(model); - // Velocity target 4.0, current vel 1.0 - data->ctrl[0] = 4.0; + // Velocity target 4.0 (second input), current vel 1.0 + data->ctrl[1] = 4.0; data->qvel[0] = 1.0; mj_forward(model.get(), data.get()); - // V = Kp * (u - omega) = 3.0 * (4.0 - 1.0) = 9.0 - // bias = - K^2 / R * omega = -0.0025 / 2.0 * 1.0 = -0.00125 - // 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)); + // force = kd * (vref - omega) = 3.0 * (4.0 - 1.0) = 9.0, exactly + EXPECT_NEAR(data->actuator_force[0], 9.0, MjTol(1e-12, 1e-5)); } TEST_F(DCMotorTest, StatefulPositionMode) { @@ -2040,7 +2170,7 @@ TEST_F(DCMotorTest, StatefulPositionMode) { - @@ -2072,11 +2202,9 @@ TEST_F(DCMotorTest, StatefulPositionMode) { // 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)); - // V = Kp(u_eff - length) + Ki * x_I - Kd * omega - // V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 - // bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 0.5 = -0.000625 - // 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)); + // force = kp * (u_eff - length) + ki * x_I - kd * omega + // = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97, exactly + EXPECT_NEAR(data->actuator_force[0], 2.97, MjTol(1e-12, 1e-5)); } TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { @@ -2090,7 +2218,7 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { - @@ -2122,16 +2250,16 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { // PI error: error = u_eff - length = 1.01 EXPECT_NEAR(data->act_dot[adr + 1], 1.01, MjTol(1e-12, 1e-5)); - // Voltage computation: - // V = Kp(u_eff - length) + Ki * x_I - Kd * omega - // V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 + // torque command: tau = kp * (u_eff - length) + ki * x_I - kd * omega + // = 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: // t_e = L / R = 1.0 / 2.0 = 0.5 // 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 = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945 - EXPECT_NEAR(data->act_dot[adr + 2], 1.945, MjTol(1e-12, 1e-5)); + // di/dt = (118.825/2.0 - 0.05/2.0 * 0.5 - 0.5) / 0.5 = 117.8 + EXPECT_NEAR(data->act_dot[adr + 2], 117.8, MjTol(1e-12, 1e-4)); // Force is K * next_activation (actearly is always on for DC motors) // Inline mj_nextActivation for te = 0.5 @@ -2152,7 +2280,7 @@ TEST_F(DCMotorTest, StatefulVelocityMode) { - @@ -2169,28 +2297,23 @@ TEST_F(DCMotorTest, StatefulVelocityMode) { double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0) 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->qvel[0] = 1.0; mj_forward(model.get(), data.get()); - // integrate command directly: error = target = 4.0 - // since x_I == Imax (2.0) and error (4.0) > 0, act_dot should be clamped to 0 + // error = 4.0; since x_I == Imax (2.0) and error > 0, act_dot clamps to 0 EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); - // V = Kp * (u_eff - omega) + Ki * (x_I - length) - // V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 0.0) = 9.0 + 2.0 = 11.0 - // 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)); + // force = kp * (qref - l) + ki * x_I = 3.0 * (4.0 - 0.0) + 1.0 * 2.0 = 14.0 + EXPECT_NEAR(data->actuator_force[0], 14.0, MjTol(1e-12, 1e-5)); // repeat with non-zero joint position data->qpos[0] = 1.5; 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 = K/R * V + bias = 0.025 * 9.5 - 0.00125 = 0.2375 - 0.00125 = 0.23625 - EXPECT_NEAR(data->actuator_force[0], 0.23625, MjTol(1e-12, 1e-5)); + // force = 3.0 * (4.0 - 1.5) + 1.0 * 2.0 = 9.5 + EXPECT_NEAR(data->actuator_force[0], 9.5, MjTol(1e-12, 1e-5)); } TEST_F(DCMotorTest, CurrentPlusThermal) { @@ -2300,7 +2423,7 @@ TEST_F(DCMotorTest, VoltageLimit) { + input="pos vel" controller="1 0 0 0 0 10.0"/> )"; @@ -2335,7 +2458,7 @@ TEST_F(DCMotorTest, IntegralClamp) { - @@ -3121,9 +3244,8 @@ TEST_F(ForwardTest, FlexParentCoupling) { if (diff > max_diff) max_diff = diff; } - // tolerance rebaselined 2e-5 -> 5e-5 for the in-solver implicit flex treatment: implicit - // and explicit flex damping legitimately differ at O(h*damping*K/M) in this comparison, and - // the in-solver form lands at ~3e-5 where the old post-hoc operator landed just under 2e-5 + // implicit and explicit flex damping legitimately differ at + // O(h*damping*K/M) in this comparison EXPECT_LT(max_diff, MjTol(5e-5, 1.5e-2)) << "Implicit integrator should match Euler at small timestep"; } @@ -3849,10 +3971,10 @@ TEST_F(ImplicitIntegratorTest, InterpStretchEnergy) { << ", initial=" << initial_energy; } -} // namespace -// with the implicit effective metric active, inverse dynamics must recover the applied force -// (zero here): the forward solve is (M+B)*qacc = qfrc_smooth + c + J'*f and the inverse adds -// the same B*qacc - c terms. This is the fwd/inv consistency fence for the flex-CG dispatch. +// with the implicit effective metric active, inverse dynamics must recover the +// applied force (zero here): the forward solve is (M+B)*qacc = qfrc_smooth + c +// + J'*f and the inverse adds the same B*qacc - c terms. This is the fwd/inv +// consistency fence for the flex-CG dispatch. TEST_F(ForwardTest, GatedFlexInverseConsistency) { static const char* const kXml = R"( @@ -3886,10 +4008,12 @@ TEST_F(ForwardTest, GatedFlexInverseConsistency) { mj_forward(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); EXPECT_LT(mju_norm(data->qfrc_inverse, nv), 1e-6 * scale); } +} // namespace } // namespace mujoco diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml index 3d9e895a..75acce3e 100644 --- a/test/engine/testdata/derivative/dcmotor.xml +++ b/test/engine/testdata/derivative/dcmotor.xml @@ -34,11 +34,11 @@ - - + + - + + input="pos vel" controller="10 0 5"/> + input="pos vel" controller="0 0 5"/> diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index a2aa3c67..0f582407 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3149,7 +3149,7 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) { + saturation="0 0 0" inductance="0 0.01" input="pos vel"/> @@ -3182,8 +3182,9 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) { // check Vmax in gainprm[7] EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 12.0); - // check input mode in gainprm[8] - EXPECT_MJTNUM_EQ(model->actuator_gainprm[8], 2.0); + // check setpoint input block (gainprm[8] is retired) + EXPECT_MJTNUM_EQ(model->actuator_gainprm[8], 0.0); + EXPECT_EQ(model->actuator_ctrlnum[0], 2); // check inductance (te) in dynprm[0] EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01); @@ -3199,7 +3200,7 @@ TEST_F(ActuatorParseTest, DCMotorControllerFull) { - diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 716266d8..13585402 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -54,6 +54,32 @@ TEST_F(XMLWriterTest, EmptyModel) { EXPECT_THAT(saved_xml, Not(HasSubstr("default"))); } +TEST_F(XMLWriterTest, KeepsDCMotorNoInputs) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + 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) { { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d64794c1..419e4723 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -305,6 +305,8 @@ public enum mjtCtrlInput : int{ mjINPUT_POS = 1, mjINPUT_VEL = 2, mjINPUT_FF = 4, + mjINPUT_VOLTAGE = 8, + mjINPUT_NONE = 16, } public enum mjtObj : int{ mjOBJ_UNKNOWN = 0, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 58025338..ac8092ec 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -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)); } -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, nominal); 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, thermal); 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) { @@ -3993,7 +3993,9 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { enum_("mjtCtrlInput") .value("mjINPUT_POS", mjINPUT_POS) .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") .value("mjDATATYPE_REAL", mjDATATYPE_REAL) .value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE)