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

Changes to dcmotor:

Browse Source 
- Remove `lugre:viscous`, should now be added directly to actuator `damping`. Trying to do this for the user was incompatible with default inheritance (compounding instead of overriding).
- Move voltage limiting from the `saturation` to the `controller` attribute.
- Fix indexing issues in default inheritance.

PiperOrigin-RevId: 897087642
Change-Id: I5388c2633e15c7e223992e7eb5d6a28db75a6438
This commit is contained in:
Yuval Tassa
2026-04-09 06:51:57 -07:00
committed by Copybara-Service
parent 26fb65c7a7
commit 81720071b8
14 changed files with 335 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+18 -15
View File
@@ -6446,14 +6446,14 @@ This element has the following custom attributes in addition to the common attri
.. _actuator-dcmotor-saturation:
:at:`saturation`: :at-val:`real(4), "0 0 0 0"`
Limits on the actuator, defined as :at:`saturation` = ":at-val:`torque` :at-val:`current` :at-val:`voltage`
:at:`saturation`: :at-val:`real(3), "0 0 0"`
Limits on the actuator, defined as :at:`saturation` = ":at-val:`torque` :at-val:`current`
:at-val:`current_rate`". :at-val:`torque` and :at-val:`current` are alternative specifications of the maximum
continuous torque: if :at-val:`current` is given, :at-val:`torque` :math:`= K \cdot` :at-val:`current`; if both are
given, :at-val:`torque` takes precedence. Sets :at:`forcerange` to [:math:`-\tau_{\max},\, \tau_{\max}`].
:at-val:`voltage` sets the maximum voltage :math:`V_{\max}`. :at-val:`current_rate` sets the maximum rate of change
of current :math:`(di/dt)_{\max}` (requires :ref:`inductance<actuator-dcmotor-inductance>`). A value of 0 (the
default) for any sub-value disables the respective limit. (see `tech note <_static/dcmotor.pdf>`__, Section 2)
:at-val:`current_rate` sets the maximum rate of change of current :math:`(di/dt)_{\max}` (requires
:ref:`inductance<actuator-dcmotor-inductance>`). A value of 0 (the default) for any sub-value disables the respective
limit. (see `tech note <_static/dcmotor.pdf>`__, Section 2)
.. _actuator-dcmotor-cogging:
@@ -6465,12 +6465,12 @@ This element has the following custom attributes in addition to the common attri
.. _actuator-dcmotor-lugre:
:at:`lugre`: :at-val:`real(6), "0 0 0 0 0 0"`
LuGre friction, defined as :at:`lugre` = ":at-val:`stiffness` :at-val:`damping` :at-val:`viscous` :at-val:`coulomb`
:at-val:`static` :at-val:`stribeck`" (N·m/rad, N·m·s/rad, N·m·s/rad, N·m, N·m, rad/s). Disabled when
:at:`lugre`: :at-val:`real(5), "0 0 0 0 0"`
LuGre friction, defined as :at:`lugre` = ":at-val:`stiffness` :at-val:`damping` :at-val:`coulomb`
:at-val:`static` :at-val:`stribeck`" (N·m/rad, N·m·s/rad, N·m, N·m, rad/s). Disabled when
:at-val:`stiffness` = 0 (the default). Adds one activation variable for bristle deflection. Note that the
:at-val:`viscous` coefficient is mapped directly to the actuator :ref:`damping<actuator-general-damping>` array
(specifically the linear term, :at-val:`damping[0]`). If both are specified, their values are summed.
viscous damping coefficient :math:`\sigma_2` is not part of the :at:`lugre` attribute and should be
added to the standard actuator :ref:`damping<actuator-general-damping>` attribute.
(see `tech note <_static/dcmotor.pdf>`__, Sections 1.4 and 2.4)
.. _actuator-dcmotor-input:
@@ -6482,12 +6482,15 @@ This element has the following custom attributes in addition to the common attri
.. _actuator-dcmotor-controller:
:at:`controller`: :at-val:`real(5), "0 0 0 0 0"`
:at:`controller`: :at-val:`real(6), "0 0 0 0 0 0"`
PID controller parameters, defined as :at:`controller` = ":at-val:`kp` :at-val:`ki` :at-val:`kd`
:at-val:`slewmax` :at-val:`Imax`". Depending on the :at:`input` mode, the controller stabilizes either position or
velocity. If the :at:`input` mode is voltage, this attribute is ignored. A value of 0 (the default) disables the
respective feature: :at-val:`slewmax` = 0 means no slew-rate limiting, :at-val:`Imax` = 0 means no anti-windup
clamping. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5)
: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)
.. _actuator-plugin:
doc/_static/dcmotor.pdf
Vendored
BIN
View File
Binary file not shown.
doc/dcmotor/dcmotor.tex
+12 -14
View File
@@ -81,7 +81,7 @@
\newcommand{\atNLS}{\texttt{nominal:no\_load\_speed}}
\newcommand{\atTMAX}{\texttt{saturation:torque}}
\newcommand{\atIMAX}{\texttt{saturation:current}}
\newcommand{\atVMAX}{\texttt{saturation:voltage}}
\newcommand{\atVMAX}{\texttt{controller:Vmax}}
\newcommand{\atCRATE}{\texttt{saturation:current\_rate}}
\newcommand{\atKP}{\texttt{controller:kp}}
\newcommand{\atKI}{\texttt{controller:ki}}
@@ -104,7 +104,6 @@
\newcommand{\atTAUC}{\texttt{lugre:coulomb}}
\newcommand{\atTAUS}{\texttt{lugre:static}}
\newcommand{\atWS}{\texttt{lugre:stribeck}}
\newcommand{\atSIGV}{\texttt{lugre:viscous}}
\title{MuJoCo DC Motor Model}
\author{Google DeepMind}
@@ -338,7 +337,7 @@ Motor drivers often impose a hard limit on $di/dt$ to protect windings and elect
\subsection{Mechanical Model}
\label{sec:mechanical}
Several purely mechanical phenomena affect the motor's behavior and the effective delivered torque.
Several purely mechanical phenomena affect the motor's behavior and delivered torque, warping the electromagnetic performance envelope.
\paragraph{Mechanical losses.}
These reduce the net torque available at the shaft: $\tau_{\text{net}} = \tau_{\text{elec}} - \tau_{\text{loss}}$.
@@ -355,7 +354,7 @@ These reduce the net torque available at the shaft: $\tau_{\text{net}} = \tau_{\
\end{equation}
This provides one constraint on two unknowns ($\tau_c$ and $B$). Without additional data, the user must either assume one dominates or obtain friction measurements at multiple speeds. In MuJoCo terms, $\tau_c$ maps to \texttt{frictionloss} and $B$ to \texttt{damping}.
\noindent Combining current saturation with both mechanical losses, the net torque is:
Combining current saturation with both mechanical losses, the net torque is:
\begin{equation*}
\tau_{\text{net}} = \text{clip}\!\left( \frac{K}{R}(v - K \, \omega),\;
\pm K\, i_{\max} \right) - B \, \omega - \tau_c \, \text{sgn}(\omega)
@@ -485,7 +484,7 @@ where $A$ is the amplitude, $N_p$ is the number of pole pairs times the number o
Symbol & Description & Formula / Note \\
\midrule
$\tau_c$ & Coulomb friction & $\tau_c\,\text{sgn}(\omega)$ \\
$B$ & Viscous drag (linear) & $B\,\omega$ \\
$B$ & Viscous drag (linear) & $-B\,\omega$ \\
$\omega_0$ & No-load speed &
$\omega_0 = v\,K / (K^2 + R\,B)$ \\
$J_r$ & Rotor inertia & units: kg$\cdot$m$^2$ \\
@@ -496,7 +495,7 @@ $N_p$ & Cogging periodicity & poles $\times$ slots/pole \\
$\phi$ & Cogging phase & offset \\
\bottomrule
\end{tabular}
\caption{Named constants related to mechanical properties. Note that unlike in Table~\ref{tab:electromech_constants}, the non-approximate expression for $\omega_0$ takes into account the linear drag $B$ (assuming no high-order terms).}
\caption{Named constants related to mechanical properties. Unlike in Table~\ref{tab:electromech_constants}, the non-approximate expression for $\omega_0$ takes into account the linear drag $B$ (assuming no high-order terms).}
\label{tab:key_constants}
\end{table}
@@ -795,7 +794,7 @@ Here we describe MuJoCo's \texttt{dcmotor} actuator. Some scalars are grouped in
\begin{table}[H]
\centering
\footnotesize
\begin{tabular}{@{}lll@{}}
\begin{tabular}{@{}llp{5cm}@{}}
\toprule
Attribute & Size & Description \\
\midrule
@@ -804,18 +803,18 @@ Attribute & Size & Description \\
\texttt{nominal} & 3 & Nominal operating point ($v_n, \tau_0, \omega_0$) \\
\texttt{inductance} & 2 & Electrical dynamics ($L, t_e$) \\
\texttt{thermal} & 6 & Thermal model ($R_T, C, t_T, \alpha, T_0, T_a$) \\
\texttt{saturation} & 4 & Limits ($\tau_{\max}, i_{\max}, v_{\max}, (di{/}dt)_{\max}$) \\
\texttt{saturation} & 3 & Limits ($\tau_{\max}, i_{\max}, (di{/}dt)_{\max}$) \\
\midrule
\texttt{cogging} & 3 & Cogging torque ($A, N_p, \phi$) \\
\texttt{lugre} & 6 & LuGre friction ($\sigma_0, \sigma_1, \sigma_2, \tau_c, \tau_s, \omega_s$) \\
\texttt{lugre} & 5 & LuGre friction ($\sigma_0, \sigma_1, \tau_c, \tau_s, \omega_s$) \\
\texttt{damping} & 3 & Viscous damping coefficients \\
\texttt{armature} & 1 & Armature inertia \\
\midrule
\texttt{input} & keyword & Mode (voltage/position/velocity) \\
\texttt{controller} & 5 & Gains and slew ($k_p, k_i, k_d, s, I_{\max}$) \\
\texttt{controller} & 6 & Gains, slew, and voltage saturation ($k_p, k_i, k_d, s, I_{\max}, v_{\max}$) \\
\bottomrule
\end{tabular}
\caption{MJCF attributes for the \texttt{dcmotor} actuator, split into electrical, mechanical and control groupings.}
\caption{MJCF attributes for the \texttt{dcmotor} actuator, split into electrical, mechanical and controller groupings.}
\label{tab:mjcf_attributes}
\end{table}
@@ -1025,7 +1024,7 @@ Iron losses (\S\ref{sec:thermal_losses}), magnet flux derating (\S\ref{sec:magne
A bristle deflection state governed by the LuGre model (\S\ref{sec:lugre}) is added if the bristle stiffness $\sigma_0 > 0$.
The Stribeck function $g(\omega)$, Eq.~\eqref{eq:stribeck}, determines velocity-dependent friction, and the friction force is given by Eq.~\eqref{eq:lugre_force}. The bristle state is integrated using the exact ZOH scheme~\eqref{eq:zoh}. The viscous term $\sigma_2 \omega$ is mapped directly to the standard \texttt{actuator\_damping} attribute to leverage MuJoCo's implicit integration, while maintaining the $\sigma_2$ \texttt{lugre} sub-attribute for convenience.
The Stribeck function $g(\omega)$, Eq.~\eqref{eq:stribeck}, determines velocity-dependent friction, and the friction force is given by Eq.~\eqref{eq:lugre_force}. The bristle state is integrated using the exact ZOH scheme~\eqref{eq:zoh}. The viscous term $\sigma_2 \omega$ is specified by via the standard \texttt{damping} attribute to leverage MuJoCo's implicit integration.
\paragraph{Integration.}
The bristle stiffness $\sigma_0$ is typically very large ($10^5$--$10^6$ N$\cdot$m/rad), creating a stiff ODE. At constant velocity, the state equation~\eqref{eq:lugre_state} has the form $\dot{z} = a z + b \omega$ where $a = -\sigma_0 |\omega| / g(\omega)$ and $b = 1$. Euler integration is unstable unless $|1 + a \Delta t| < 1$, requiring impractically small timesteps ($\Delta t < 2g(\omega)/(\sigma_0 |\omega|)$, on the order of microseconds).
Under a zero-order hold assumption ($\omega$ constant over the timestep), the linear ODE $\dot{z} = az + b\omega$ can be solved exactly:
@@ -1045,7 +1044,6 @@ Attribute & Symbol & Units \\
\midrule
\atSIG{} & $\sigma_0$ & N$\cdot$m/rad \\
\atSIGD{} & $\sigma_1$ & N$\cdot$m$\cdot$s/rad \\
\atSIGV{} & $\sigma_2$ & N$\cdot$m$\cdot$s/rad \\
\atTAUC{} & $\tau_c$ & N$\cdot$m \\
\atTAUS{} & $\tau_s$ & N$\cdot$m \\
\atWS{} & $\omega_s$ & rad/s \\
@@ -1081,7 +1079,7 @@ Attribute & Type & Description \\
\label{tab:controller_attributes}
\end{table}
\noindent Unlike the motor parameters in Table~\ref{tab:datasheet}, controller gains are user-specified firmware settings. Gains are in {\em voltage-space} (e.g., $k_p$ in V/rad) since the output is a voltage $v$. To convert from physical torque-space (N$\cdot$m/rad), multiply by $R/K$.
\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.
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:
doc/includes/references.h
+3 -3
View File
@@ -3664,9 +3664,9 @@ const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double t
double lmax, double vmax, double fpmax, double fvmax);
const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[4], double inductance[2],
double cogging[3], double controller[5], double thermal[6],
double lugre[6], int input_mode);
double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode);
mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def);
mjsHField* mjs_addHField(mjSpec* s);
mjsSkin* mjs_addSkin(mjSpec* s);
include/mujoco/mujoco.h
+3 -3
View File
@@ -1729,9 +1729,9 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
// Set actuator to DC motor; return error if any.
// Nullable: motorconst, nominal, saturation, inductance, cogging, controller, thermal, lugre
MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[4], double inductance[2],
double cogging[3], double controller[5], double thermal[6],
double lugre[6], int input_mode);
double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode);
//---------------------------------- Assets --------------------------------------------------------
python/mujoco/introspect/functions.py
+3 -3
View File
@@ -10823,7 +10823,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
name='saturation',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(4,),
extents=(3,),
),
nullable=True,
),
@@ -10847,7 +10847,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
name='controller',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(5,),
extents=(6,),
),
nullable=True,
),
@@ -10863,7 +10863,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
name='lugre',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(6,),
extents=(5,),
),
nullable=True,
),
python/mujoco/specs.cc
+6 -6
View File
@@ -1307,10 +1307,10 @@ PYBIND11_MODULE(_specs, m) {
"set_to_dcmotor",
[](raw::MjsActuator* self, std::array<double, 2> motorconst,
double resistance,
std::array<double, 3> nominal, std::array<double, 4> saturation,
std::array<double, 3> nominal, std::array<double, 3> saturation,
std::array<double, 2> inductance, std::array<double, 3> cogging,
std::array<double, 5> controller, std::array<double, 6> thermal,
std::array<double, 6> lugre, int input_mode) {
std::array<double, 6> controller, std::array<double, 6> thermal,
std::array<double, 5> lugre, int input_mode) {
std::string err = mjs_setToDCMotor(
self, motorconst.data(), resistance, nominal.data(),
saturation.data(), inductance.data(), cogging.data(),
@@ -1321,12 +1321,12 @@ PYBIND11_MODULE(_specs, m) {
},
py::arg("motorconst"), py::arg("resistance"),
py::arg("nominal") = std::array<double, 3>{0, 0, 0},
py::arg("saturation") = std::array<double, 4>{0, 0, 0, 0},
py::arg("saturation") = std::array<double, 3>{0, 0, 0},
py::arg("inductance") = std::array<double, 2>{0, 0},
py::arg("cogging") = std::array<double, 3>{0, 0, 0},
py::arg("controller") = std::array<double, 5>{0, 0, 0, 0, 0},
py::arg("controller") = std::array<double, 6>{0, 0, 0, 0, 0, 0},
py::arg("thermal") = std::array<double, 6>{0, 0, 0, 0, 0, 0},
py::arg("lugre") = 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);
// ============================= MJSTENDONPATH ===============================
src/user/user_api.cc
+16 -16
View File
@@ -1122,9 +1122,9 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) {
const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[4], double inductance[2],
double cogging[3], double controller[5], double thermal[6],
double lugre[6], int input_mode) {
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 R = resistance; // electrical resistance
double Kt = motorconst ? motorconst[0] : 0; // torque constant
double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant
@@ -1134,9 +1134,8 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
// derive Ke from nominal: omega0 = vn*Ke / (Ke^2 + R*B)
if (vn > 0 && Ke <= 0 && omega0 > 0) {
// viscous damping (linear), add lugre sigma2 contribution if any
// viscous damping (linear)
double B = actuator->damping[0];
if (lugre && lugre[0] > 0) B += lugre[2];
if (B > 0 && R > 0) {
// R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0
@@ -1184,12 +1183,9 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
actuator->dynprm[7] = controller ? controller[3] : 0; // slewmax
actuator->dynprm[8] = controller ? controller[4] : 0; // Imax
// saturation: [tau_max, i_max, (di/dt)_max, v_max]
if (saturation && saturation[2] > 0) {
actuator->dynprm[1] = saturation[2]; // (di/dt)_max
}
if (saturation && saturation[3] > 0) {
actuator->gainprm[7] = saturation[3]; // v_max
// controller parameters: gainprm[7] for v_max
if (controller && controller[5] > 0) {
actuator->gainprm[7] = controller[5]; // v_max
}
// saturation -> forcerange
@@ -1203,6 +1199,11 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
actuator->forcelimited = 1;
}
// saturation: [tau_max, i_max, (di/dt)_max]
if (saturation && saturation[2] > 0) {
actuator->dynprm[1] = saturation[2]; // (di/dt)_max
}
// cogging: [amplitude, periodicity, phase] -> biasprm[0:3]
actuator->biasprm[0] = cogging ? cogging[0] : 0; // amplitude
actuator->biasprm[1] = cogging ? cogging[1] : 0; // periodicity
@@ -1258,14 +1259,13 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
actdim++;
}
// lugre: {stiffness, damping, viscous, coulomb, static, stribeck}
// lugre: {stiffness, damping, coulomb, static, stribeck}
if (lugre && lugre[0] > 0) {
actuator->dynprm[5] = lugre[0]; // stiffness -> sigma0
actuator->dynprm[6] = lugre[1]; // damping -> sigma1
actuator->damping[0] += lugre[2]; // viscous -> sigma2
actuator->biasprm[3] = lugre[3]; // coulomb -> tau_c
actuator->biasprm[4] = lugre[4]; // static -> tau_s
actuator->biasprm[5] = lugre[5]; // stribeck -> omega_s
actuator->biasprm[3] = lugre[2]; // coulomb -> tau_c
actuator->biasprm[4] = lugre[3]; // static -> tau_s
actuator->biasprm[5] = lugre[4]; // stribeck -> omega_s
actdim++;
}
src/user/user_api.h
+3 -3
View File
@@ -194,9 +194,9 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain);
// Set actuator to DC motor, return error on failure.
MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance,
double nominal[3], double saturation[4], double inductance[2],
double cogging[3], double controller[5], double thermal[6],
double lugre[6], int input_mode);
double nominal[3], double saturation[3], double inductance[2],
double cogging[3], double controller[6], double thermal[6],
double lugre[5], int input_mode);
//---------------------------------- Add assets ----------------------------------------------------
src/xml/xml_native_reader.cc
+11 -12
View File
@@ -2526,14 +2526,14 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
double motorconst[2] = {inherited ? actuator->gainprm[1] : 0, 0};
double resistance = inherited ? actuator->gainprm[0] : 0;
double nominal[3] = {0, 0, 0};
double saturation[4] = {0, 0,
inherited ? actuator->dynprm[1] : 0,
inherited ? actuator->gainprm[8] : 0};
double controller[5] = {inherited ? actuator->gainprm[5] : 0,
double saturation[3] = {0, 0,
inherited ? actuator->dynprm[1] : 0};
double controller[6] = {inherited ? actuator->gainprm[4] : 0,
inherited ? actuator->gainprm[5] : 0,
inherited ? actuator->gainprm[6] : 0,
inherited ? actuator->gainprm[7] : 0,
inherited ? actuator->dynprm[7] : 0,
inherited ? actuator->dynprm[8] : 0};
inherited ? actuator->dynprm[8] : 0,
inherited ? actuator->gainprm[7] : 0};
double inductance[2] = {0, inherited ? actuator->dynprm[0] : 0};
double cogging[3] = {inherited ? actuator->biasprm[0] : 0,
inherited ? actuator->biasprm[1] : 0,
@@ -2544,22 +2544,21 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
inherited ? actuator->gainprm[2] : 0,
inherited ? actuator->gainprm[3] : 0,
inherited ? actuator->dynprm[4] : 0};
double lugre[6] = {inherited ? actuator->dynprm[5] : 0,
double lugre[5] = {inherited ? actuator->dynprm[5] : 0,
inherited ? actuator->dynprm[6] : 0,
inherited ? actuator->damping[0] : 0,
inherited ? actuator->biasprm[3] : 0,
inherited ? actuator->biasprm[4] : 0,
inherited ? actuator->biasprm[5] : 0};
int input_mode = inherited ? (int)actuator->gainprm[9] : 0;
int input_mode = inherited ? (int)actuator->gainprm[8] : 0;
ReadAttr(elem, "motorconst", 2, motorconst, text, false, false);
ReadAttr(elem, "resistance", 1, &resistance, text);
ReadAttr(elem, "nominal", 3, nominal, text, false, false);
ReadAttr(elem, "saturation", 4, saturation, text, false, false);
ReadAttr(elem, "saturation", 3, saturation, text, false, false);
ReadAttr(elem, "inductance", 2, inductance, text, false, false);
ReadAttr(elem, "cogging", 3, cogging, text, false, false);
ReadAttr(elem, "controller", 5, controller, text, false, false);
ReadAttr(elem, "controller", 6, controller, text, false, false);
ReadAttr(elem, "thermal", 6, thermal, text, false, false);
ReadAttr(elem, "lugre", 6, lugre, text, false, false);
ReadAttr(elem, "lugre", 5, lugre, text, false, false);
if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) {
// successfully parsed
}
test/engine/engine_forward_test.cc
+105 -5
View File
@@ -1416,7 +1416,7 @@ TEST_F(DCMotorTest, LuGreViscousFriction) {
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
@@ -1929,7 +1929,7 @@ TEST_F(DCMotorTest, CurrentRateLimit) {
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0" saturation="0 0 100 0"/>
inductance="0.01 0" saturation="0 0 100"/>
</actuator>
</mujoco>
)";
@@ -1963,6 +1963,106 @@ TEST_F(DCMotorTest, CurrentRateLimit) {
mj_deleteModel(model);
}
TEST_F(DCMotorTest, VoltageLimit) {
// verifies that saturation:voltage clamps voltage
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="position" controller="1 0 0 0 0 10.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// Vmax = 10.0, ctrl = 20.0
// force = K/R * Vmax = 0.05 / 2.0 * 10.0 = 0.25
data->ctrl[0] = 20.0;
mj_forward(model, data);
EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5));
// negative drive
data->ctrl[0] = -20.0;
mj_forward(model, data);
EXPECT_NEAR(data->actuator_force[0], -0.25, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(DCMotorTest, IntegralClamp) {
// verifies that controller Imax clamps integral state
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0 0 5.0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// Imax = 5.0
ASSERT_EQ(model->actuator_actnum[0], 1); // only ki is stateful
int adr = model->actuator_actadr[0];
// set integral state to Imax
data->act[adr] = 5.0;
// set target to generate positive error (ctrl - length)
data->ctrl[0] = 1.0; // target
data->qpos[0] = 0.0; // length = 0
mj_forward(model, data);
// act_dot should be clamped to 0 because act >= Imax and error > 0
EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5));
// set target to generate negative error
data->ctrl[0] = -1.0;
mj_forward(model, data);
// act_dot should be negative (not clamped)
EXPECT_NEAR(data->act_dot[adr], -1.0, MjTol(1e-12, 1e-5));
// set integral state to -Imax
data->act[adr] = -5.0;
// set target to generate negative error
data->ctrl[0] = -1.0;
data->qpos[0] = 0.0;
mj_forward(model, data);
// act_dot should be clamped to 0 because act <= -Imax and error < 0
EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(DCMotorTest, LuGreExactIntegration) {
static constexpr char xml[] = R"(
<mujoco>
@@ -1975,7 +2075,7 @@ TEST_F(DCMotorTest, LuGreExactIntegration) {
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
@@ -2021,7 +2121,7 @@ TEST_F(DCMotorTest, LuGreSteadyState) {
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
@@ -2068,7 +2168,7 @@ TEST_F(DCMotorTest, LuGreBristleSpring) {
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
damping="0.01" lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
test/engine/testdata/derivative/dcmotor.xml
Vendored
+1 -1
View File
@@ -30,6 +30,6 @@
<!-- DC motor with LuGre friction (sigma1 micro-damping) -->
<dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0"
lugre="1e4 100 0.001 0.005 0.008 0.1"/>
damping="0.001" lugre="1e4 100 0.005 0.008 0.1"/>
</actuator>
</mujoco>
test/user/user_api_test.cc
+49
View File
@@ -280,6 +280,55 @@ TEST_F(MujocoTest, SetToDCMotorDeriveKe) {
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorFull) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
double saturation[3] = {1.0, 2.0, 3.0};
double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
nullptr, saturation, nullptr,
nullptr, controller, nullptr,
nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance
EXPECT_EQ(actuator->gainprm[1], 0.05); // K
EXPECT_EQ(actuator->gainprm[4], 10.0); // kp
EXPECT_EQ(actuator->gainprm[5], 20.0); // ki
EXPECT_EQ(actuator->gainprm[6], 30.0); // kd
EXPECT_EQ(actuator->dynprm[7], 40.0); // slewmax
EXPECT_EQ(actuator->dynprm[8], 50.0); // Imax
EXPECT_EQ(actuator->gainprm[7], 60.0); // Vmax
EXPECT_EQ(actuator->dynprm[1], 3.0); // (di/dt)_max
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorLuGre) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance,
nullptr, nullptr, nullptr,
nullptr, nullptr, nullptr,
lugre, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness
EXPECT_EQ(actuator->dynprm[6], 1.0); // damping
EXPECT_EQ(actuator->biasprm[3], 0.5); // coulomb
EXPECT_EQ(actuator->biasprm[4], 0.7); // static
EXPECT_EQ(actuator->biasprm[5], 10.0); // stribeck
mj_deleteSpec(spec);
}
static constexpr char xml_plugin_1[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
test/xml/xml_native_reader_test.cc
+105 -4
View File
@@ -3108,7 +3108,55 @@ TEST_F(ActuatorParseTest, DCMotorSaturation) {
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) {
TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) {
static constexpr char xml[] = R"(
<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"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
// check motorconst and resistance are overridden by instance
EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0);
// check controller gains (kp, ki, kd) in gainprm[4:6]
EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 0.5);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[6], 0.1);
// check controller limits (slewmax, Imax) in dynprm[7,8]
EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 10.0);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0);
// 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 inductance (te) in dynprm[0]
EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorControllerFull) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -3119,7 +3167,36 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) {
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
controller="1.0 2.0 3.0 4.0 5.0 6.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 1.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 2.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[6], 3.0);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 4.0);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0);
EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 6.0);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0" damping="0.01"
lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
@@ -3135,6 +3212,30 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) {
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorLuGreInheritedDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<dcmotor motorconst="0.05" resistance="2.0" damping="0.01" lugre="100 1 0.5 0.7 10"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorActdimStateless) {
static constexpr char xml[] = R"(
<mujoco>
@@ -3216,7 +3317,7 @@ TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) {
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
@@ -3241,7 +3342,7 @@ TEST_F(ActuatorParseTest, DCMotorActdimAllThree) {
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
inductance="0.001 0"
thermal="10 5 0 0 0 25"
lugre="100 1 0.01 0.5 0.7 10"/>
lugre="100 1 0.5 0.7 10"/>
</actuator>
</mujoco>
)";