Redesign the dcmotor controller: setpoint inputs, torque-space gains.
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
committed by
Copybara-Service
Copybara-Service
parent
11fa4a5b45
commit
2f1843f4a7
+40
-19
@@ -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 <geActuation>` 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 <geActuation>`, 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<actuator-dcmotor>`
|
||||
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>`
|
||||
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<actuator-dcmotor>`
|
||||
so3 damping term of the geodesic orientation servo, see :ref:`orientation<actuator-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<actuator-orientation-input>`. For gaintype "pid" it is
|
||||
a token list selecting the input subset; see :ref:`pid/input<actuator-pid-input>`.
|
||||
default) or "quat" (4 controls); see :ref:`orientation/input<actuator-orientation-input>`. For gaintypes "pid" and
|
||||
"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:
|
||||
|
||||
@@ -6806,8 +6809,11 @@ the stateless case.
|
||||
- :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.
|
||||
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
|
||||
default), or a position or velocity target for a :ref:`PID controller<actuator-dcmotor-controller>`.
|
||||
- The control block is selected by :ref:`input<actuator-dcmotor-input>`: any subset of ``[pos, vel, ff]``, where
|
||||
``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>`),
|
||||
:ref:`cogging torque<actuator-dcmotor-cogging>`, :ref:`thermal resistance variation<actuator-dcmotor-thermal>`, and
|
||||
: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:
|
||||
|
||||
: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<actuator-dcmotor-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<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:
|
||||
|
||||
: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<actuator-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<actuator-dcmotor-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<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:
|
||||
|
||||
@@ -6982,7 +7003,7 @@ Associate this actuator with an :ref:`engine plugin<exPlugin>`. 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 <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
|
||||
|
||||
Vendored
BIN
Binary file not shown.
@@ -31,6 +31,28 @@ Actuation
|
||||
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
|
||||
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
|
||||
^^^^^^
|
||||
|
||||
+69
-112
@@ -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.}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
|
||||
@@ -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 --------------------------------------------------------
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -11314,7 +11314,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
|
||||
nullable=True,
|
||||
),
|
||||
FunctionParameterDecl(
|
||||
name='input_mode',
|
||||
name='ctrlspec',
|
||||
type=ValueType(name='int'),
|
||||
),
|
||||
),
|
||||
|
||||
@@ -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, 2> inductance, std::array<double, 3> cogging,
|
||||
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(
|
||||
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<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("input_mode") = 0);
|
||||
py::arg("ctrlspec") = 0);
|
||||
|
||||
// ============================= MJSTENDONPATH ===============================
|
||||
// helper struct for tendon path indexing
|
||||
|
||||
@@ -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ω
|
||||
|
||||
+40
-43
@@ -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);
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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
|
||||
|
||||
+1
-1
@@ -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 ----------------------------------------------------
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -223,11 +223,12 @@
|
||||
</xs:restriction>
|
||||
</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:enumeration value="voltage"/>
|
||||
<xs:enumeration value="position"/>
|
||||
<xs:enumeration value="velocity"/>
|
||||
<xs:enumeration value="none"/>
|
||||
</xs:restriction>
|
||||
</xs:simpleType>
|
||||
|
||||
@@ -252,12 +253,13 @@
|
||||
|
||||
<xs:simpleType name="kw_inputbit">
|
||||
<xs:annotation>
|
||||
<xs:documentation>bitflags: keywords combine bitwise</xs:documentation>
|
||||
<xs:documentation>bitflags: tokens combine bitwise, canonical order enforced</xs:documentation>
|
||||
</xs:annotation>
|
||||
<xs:restriction base="xs:string">
|
||||
<xs:enumeration value="pos"/>
|
||||
<xs:enumeration value="vel"/>
|
||||
<xs:enumeration value="ff"/>
|
||||
<xs:enumeration value="voltage"/>
|
||||
</xs:restriction>
|
||||
</xs:simpleType>
|
||||
<xs:simpleType name="kwlist_inputbit">
|
||||
@@ -1880,7 +1882,11 @@
|
||||
<xs:attribute name="controller" type="double1to6"/>
|
||||
<xs:attribute name="thermal" type="double1to6"/>
|
||||
<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 name="extension_plugin">
|
||||
@@ -3055,7 +3061,11 @@
|
||||
<xs:attribute name="controller" type="double1to6"/>
|
||||
<xs:attribute name="thermal" type="double1to6"/>
|
||||
<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 name="actuator_plugin">
|
||||
|
||||
@@ -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[] = {
|
||||
|
||||
+8
-9
@@ -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<dcmotorinput>
|
||||
input : flags<inputbit> (reading=custom) # token subset, or the none keyword
|
||||
}
|
||||
|
||||
element actuator_plugin : mjsActuator (xml=plugin) {
|
||||
|
||||
@@ -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") {
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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'):
|
||||
|
||||
@@ -1414,7 +1414,7 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<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>
|
||||
</mujoco>
|
||||
)";
|
||||
@@ -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) {
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<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>
|
||||
</mujoco>
|
||||
)";
|
||||
@@ -1476,7 +1476,7 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<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>
|
||||
</mujoco>
|
||||
)";
|
||||
|
||||
@@ -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 <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"(
|
||||
<mujoco>
|
||||
<option integrator="implicit"/>
|
||||
<option timestep="0.001" integrator="implicitfast">
|
||||
<flag contact="disable" gravity="disable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<body pos="0 0 0">
|
||||
<body>
|
||||
<joint name="slide1" type="slide" axis="1 0 0"/>
|
||||
<geom size=".1"/>
|
||||
<geom size="0.1" mass="1"/>
|
||||
</body>
|
||||
<body pos="0 1 0">
|
||||
<body>
|
||||
<joint name="slide2" type="slide" axis="1 0 0"/>
|
||||
<geom size=".1"/>
|
||||
<geom size="0.1" mass="1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<!--
|
||||
Equivalence mapping:
|
||||
intvelocity force: F = kp * \int(ctrl - v) - kv * v
|
||||
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"/>
|
||||
<pid name="pid" joint="slide1" kp="10" kv="5"/>
|
||||
<dcmotor name="dcmotor" joint="slide2" motorconst="0.05" resistance="2.0"
|
||||
input="pos vel" controller="10 0 5"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
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"(
|
||||
<mujoco>
|
||||
@@ -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"(
|
||||
<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) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
@@ -1928,7 +2058,7 @@ TEST_F(DCMotorTest, ThermalAffectsForceWithController) {
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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) {
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<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>
|
||||
</mujoco>
|
||||
)";
|
||||
@@ -2335,7 +2458,7 @@ TEST_F(DCMotorTest, IntegralClamp) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -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"(
|
||||
<mujoco>
|
||||
@@ -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
|
||||
|
||||
+5
-5
@@ -34,11 +34,11 @@
|
||||
<!-- DC motor back-EMF stateless damping -->
|
||||
<dcmotor name="dc_bias" joint="slide1" motorconst="2.0" resistance="0.5"/>
|
||||
|
||||
<!-- DC motor velocity controller damping -->
|
||||
<dcmotor name="dc_vel" joint="slide2" motorconst="1.0" resistance="1.0" input="velocity" controller="0 5"/>
|
||||
<!-- DC motor velocity setpoint (kd) damping -->
|
||||
<dcmotor name="dc_vel" joint="slide2" motorconst="1.0" resistance="1.0" input="pos vel" controller="0 0 5"/>
|
||||
|
||||
<!-- 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) -->
|
||||
<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) -->
|
||||
<dcmotor name="dc_stateful_pos" joint="slide6"
|
||||
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 -->
|
||||
<dcmotor name="dc_stateful_vel" joint="slide7"
|
||||
motorconst="1.0" resistance="1.0" inductance="0 0.001"
|
||||
input="velocity" controller="5 0"/>
|
||||
input="pos vel" controller="0 0 5"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
|
||||
@@ -3149,7 +3149,7 @@ TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) {
|
||||
<mujoco>
|
||||
<default>
|
||||
<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>
|
||||
<worldbody>
|
||||
<body>
|
||||
@@ -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) {
|
||||
</body>
|
||||
</worldbody>
|
||||
<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"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
|
||||
@@ -54,6 +54,32 @@ TEST_F(XMLWriterTest, EmptyModel) {
|
||||
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) {
|
||||
{
|
||||
static constexpr char xml[] = R"(
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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>("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>("mjtDataType")
|
||||
.value("mjDATATYPE_REAL", mjDATATYPE_REAL)
|
||||
.value("mjDATATYPE_POSITIVE", mjDATATYPE_POSITIVE)
|
||||
|
||||
Reference in New Issue
Block a user