diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 72bfd3d8..a1443753 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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`). 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`). 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` 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` 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: diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf index a811acdc..cfee21c4 100644 Binary files a/doc/_static/dcmotor.pdf and b/doc/_static/dcmotor.pdf differ diff --git a/doc/dcmotor/dcmotor.tex b/doc/dcmotor/dcmotor.tex index 185e6e41..0af0fa2c 100644 --- a/doc/dcmotor/dcmotor.tex +++ b/doc/dcmotor/dcmotor.tex @@ -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: diff --git a/doc/includes/references.h b/doc/includes/references.h index 89b2e81f..bea7f4d9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index e8bbe692..90a6569b 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -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 -------------------------------------------------------- diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 262d3430..f05bd8a5 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -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, ), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 28af8af1..82d43057 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1307,10 +1307,10 @@ PYBIND11_MODULE(_specs, m) { "set_to_dcmotor", [](raw::MjsActuator* self, std::array motorconst, double resistance, - std::array nominal, std::array saturation, + std::array nominal, std::array saturation, std::array inductance, std::array cogging, - std::array controller, std::array thermal, - std::array lugre, int input_mode) { + std::array controller, std::array thermal, + std::array 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{0, 0, 0}, - py::arg("saturation") = std::array{0, 0, 0, 0}, + py::arg("saturation") = std::array{0, 0, 0}, py::arg("inductance") = std::array{0, 0}, py::arg("cogging") = std::array{0, 0, 0}, - py::arg("controller") = std::array{0, 0, 0, 0, 0}, + py::arg("controller") = std::array{0, 0, 0, 0, 0, 0}, py::arg("thermal") = std::array{0, 0, 0, 0, 0, 0}, - py::arg("lugre") = std::array{0, 0, 0, 0, 0, 0}, + py::arg("lugre") = std::array{0, 0, 0, 0, 0}, py::arg("input_mode") = 0); // ============================= MJSTENDONPATH =============================== diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 8d320463..e2277bfd 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -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++; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 307f1a53..2b73c12e 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -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 ---------------------------------------------------- diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 352790b5..df10509f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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 } diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index f3acf0aa..3c9a05d4 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1416,7 +1416,7 @@ TEST_F(DCMotorTest, LuGreViscousFriction) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; @@ -1929,7 +1929,7 @@ TEST_F(DCMotorTest, CurrentRateLimit) { + inductance="0.01 0" saturation="0 0 100"/> )"; @@ -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"( + + + + + + + + + + + + )"; + 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"( + + + )"; + 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"( @@ -1975,7 +2075,7 @@ TEST_F(DCMotorTest, LuGreExactIntegration) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; @@ -2021,7 +2121,7 @@ TEST_F(DCMotorTest, LuGreSteadyState) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; @@ -2068,7 +2168,7 @@ TEST_F(DCMotorTest, LuGreBristleSpring) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml index d3c4b0ac..a1053bb6 100644 --- a/test/engine/testdata/derivative/dcmotor.xml +++ b/test/engine/testdata/derivative/dcmotor.xml @@ -30,6 +30,6 @@ + damping="0.001" lugre="1e4 100 0.005 0.008 0.1"/> diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index b36557aa..2a7d8a53 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -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"( diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index bd1b517b..5f6ecef7 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3108,7 +3108,55 @@ TEST_F(ActuatorParseTest, DCMotorSaturation) { mj_deleteModel(model); } -TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + +TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array 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"( @@ -3119,7 +3167,36 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + controller="1.0 2.0 3.0 4.0 5.0 6.0"/> + + + )"; + std::array 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"( + + + + + + + + + )"; @@ -3135,6 +3212,30 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { mj_deleteModel(model); } +TEST_F(ActuatorParseTest, DCMotorLuGreInheritedDefaults) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array 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"( @@ -3216,7 +3317,7 @@ TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) { + lugre="100 1 0.5 0.7 10"/> )"; @@ -3241,7 +3342,7 @@ TEST_F(ActuatorParseTest, DCMotorActdimAllThree) { + lugre="100 1 0.5 0.7 10"/> )";