diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index df6381f5..fa61555b 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4658,6 +4658,15 @@ Set actuator to muscle; return error if any.a Set actuator to active adhesion; return error if any. +.. _mjs_setToDCMotor: + +`mjs_setToDCMotor <#mjs_setToDCMotor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_setToDCMotor + +Set actuator to DC motor; return error if any. + .. _AddAssets: Assets diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 7e90fa14..2552f932 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6323,6 +6323,174 @@ This element has a subset of the common attributes and two custom attributes. to the target body. +.. _actuator-dcmotor: + +:el-prefix:`actuator/` |-| **dcmotor** |*| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This element creates a DC motor actuator. Note that :el:`dcmotor` is quite different from the :ref:`general actuation +model`. Unlike the general model where the components of force generation are independent affine functions +mapping from control to force, :el:`dcmotor` relies on highly coupled physical dynamics. See the `DC motor technical +note <_static/dcmotor.pdf>`__ for complete mathematical formulations and parameter semantics, but we include a few +important notes here: + +- Note that while :ref:`resistance`, :ref:`motorconst` and + :ref:`nominal` are each optional, some combination of them is required. + See Section 2.1 of the `technical note <_static/dcmotor.pdf>`__. +- The control :ref:`input` semantic is either the voltage applied to the motor terminals, or a + position or velocity target for a PID :ref:`controller`. +- Optional features include electrical dynamics (:ref:`inductance`), + :ref:`cogging torque`, :ref:`thermal resistance variation`, and + :ref:`LuGre` friction. + +The underlying :el:`general` attributes are set to the :el:`dcmotor` type, and their associated parameter arrays are +computed internally: + +========= ======= ========= ======== +Attribute Setting Attribute Setting +========= ======= ========= ======== +dyntype dcmotor dynprm computed +gaintype dcmotor gainprm computed +biastype dcmotor biasprm computed +========= ======= ========= ======== + +This element has the following custom attributes in addition to the common attributes: + +.. _actuator-dcmotor-name: + +.. _actuator-dcmotor-class: + +.. _actuator-dcmotor-group: + +.. _actuator-dcmotor-delay: + +.. _actuator-dcmotor-nsample: + +.. _actuator-dcmotor-interp: + +.. _actuator-dcmotor-ctrllimited: + +.. _actuator-dcmotor-ctrlrange: + +.. _actuator-dcmotor-lengthrange: + +.. _actuator-dcmotor-gear: + +.. _actuator-dcmotor-damping: + +.. _actuator-dcmotor-armature: + +.. _actuator-dcmotor-cranklength: + +.. _actuator-dcmotor-joint: + +.. _actuator-dcmotor-jointinparent: + +.. _actuator-dcmotor-tendon: + +.. _actuator-dcmotor-cranksite: + +.. _actuator-dcmotor-slidersite: + +.. _actuator-dcmotor-site: + +.. _actuator-dcmotor-refsite: + +.. _actuator-dcmotor-user: + +.. |actuator/dcmotor attrib list| replace:: + :at:`name`, :at:`class`, :at:`group`, :at:`nsample`, :at:`interp`, :at:`delay`, :at:`ctrllimited`, :at:`ctrlrange`, + :at:`lengthrange`, :at:`gear`, :at:`damping`, :at:`armature`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, + :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + +|actuator/dcmotor attrib list| + Same as in actuator/ :ref:`general `. + +.. _actuator-dcmotor-resistance: + +:at:`resistance`: :at-val:`real, optional` + Terminal resistance :math:`R` in Ohm. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-motorconst: + +:at:`motorconst`: :at-val:`real(2), optional` + Motor constants, defined as :at:`motorconst` = ":at-val:`Kt` :at-val:`Ke`" (N·m/A, equivalently V·s/rad). + :at-val:`Kt` is the torque constant and :at-val:`Ke` the back-EMF constant; they can differ when magnetic saturation + is present. If both are positive, the effective constant is :math:`K = \sqrt{K_t K_e}` (geometric mean). If only one + is positive, :math:`K` equals that value; a single value is interpreted as :math:`K_t = K_e`. If your datasheet gives + the speed constant :math:`K_v` in rad/(V·s), use :math:`K_e = 1/K_v`. (see `tech note <_static/dcmotor.pdf>`__ for + details) + +.. _actuator-dcmotor-nominal: + +:at:`nominal`: :at-val:`real(3), optional` + Nominal operating point, defined as :at:`nominal` = ":at-val:`voltage` :at-val:`stall_torque` + :at-val:`no_load_speed`". The compiler derives :math:`K =` :at-val:`voltage` / :at-val:`no_load_speed` and :math:`R = + K` · :at-val:`voltage` / :at-val:`stall_torque`. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-inductance: + +:at:`inductance`: :at-val:`real(2), "0 0"` + Electrical dynamics, defined as :at:`inductance` = ":at-val:`L` :at-val:`timeconst`" (Henry, seconds). These are + alternative specifications: :at-val:`L` is the winding inductance and :at-val:`timeconst` :math:`= L/R` is the + electrical time constant. Specify one; if both are given, :at-val:`L` takes precedence. If both are 0 (the default), + no electrical dynamics are modeled and the current is computed algebraically. Adds one activation variable for + armature current. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-thermal: + +:at:`thermal`: :at-val:`real(6), "0 0 0 0 0 0"` + Thermal model, defined as :at:`thermal` = ":at-val:`resistance` :at-val:`capacitance` :at-val:`timeconst` + :at-val:`tempcoef` :at-val:`reftemp` :at-val:`ambient`" (K/W, J/K, s, 1/K, °C, °C). The first three sub-values + specify the thermal time constant: :at-val:`timeconst` = :at-val:`resistance` :math:`\times` :at-val:`capacitance`. + Specify either :at-val:`timeconst` directly, or :at-val:`resistance` and :at-val:`capacitance`; if all three are + given, :at-val:`timeconst` takes precedence. If all are 0 (the default), thermal modeling is disabled. Adds one + activation variable for winding temperature. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _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-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>`__ for details) + +.. _actuator-dcmotor-cogging: + +:at:`cogging`: :at-val:`real(3), "0 0 0"` + Cogging torque, defined as :at:`cogging` = ":at-val:`amplitude` :at-val:`poles` :at-val:`phase`" (N·m, integer, rad). + Adds a position-dependent torque :math:`= \textsf{amplitude} \cdot \sin(\textsf{poles} \cdot \theta + + \textsf{phase})`. Disabled when :at-val:`amplitude` = 0 (the default). + (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _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-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. + (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _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 PID :ref:`controller` uses the control as a + reference setpoint relative to the joint trajectory. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-controller: + +:at:`controller`: :at-val:`real(5), "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, the controller 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>`__ for details) + .. _actuator-plugin: :el-prefix:`actuator/` |-| **plugin** |?| @@ -9887,6 +10055,57 @@ refsite, tendon, slidersite, cranksite. All :ref:`adhesion ` attributes are available here except: name, class, body. +.. _default-dcmotor: + +.. _default-dcmotor-ctrllimited: + +.. _default-dcmotor-ctrlrange: + +.. _default-dcmotor-gear: + +.. _default-dcmotor-damping: + +.. _default-dcmotor-armature: + +.. _default-dcmotor-cranklength: + +.. _default-dcmotor-user: + +.. _default-dcmotor-group: + +.. _default-dcmotor-delay: + +.. _default-dcmotor-nsample: + +.. _default-dcmotor-interp: + +.. _default-dcmotor-motorconst: + +.. _default-dcmotor-resistance: + +.. _default-dcmotor-nominal: + +.. _default-dcmotor-saturation: + +.. _default-dcmotor-inductance: + +.. _default-dcmotor-cogging: + +.. _default-dcmotor-controller: + +.. _default-dcmotor-input: + +.. _default-dcmotor-thermal: + +.. _default-dcmotor-lugre: + +:el-prefix:`default/` |-| **dcmotor** |?| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +All :ref:`dcmotor ` attributes are available here except: name, class, joint, jointinparent, site, +refsite, tendon, slidersite, cranksite. + + .. _custom: **custom** |*| diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 74f72561..4dbc6c29 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2984,6 +2984,105 @@ :ref:`gain` + .. dropdown:: :ref:`dcmotor` |*| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`class` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`ctrllimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`lengthrange` + + .. grid-item:: + :ref:`gear` + + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + + .. grid-item:: + :ref:`cranklength` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`joint` + + .. grid-item:: + :ref:`jointinparent` + + .. grid-item:: + :ref:`tendon` + + .. grid-item:: + :ref:`slidersite` + + .. grid-item:: + :ref:`cranksite` + + .. grid-item:: + :ref:`site` + + .. grid-item:: + :ref:`refsite` + + .. grid-item:: + :ref:`motorconst` + + .. grid-item:: + :ref:`resistance` + + .. grid-item:: + :ref:`nominal` + + .. grid-item:: + :ref:`saturation` + + .. grid-item:: + :ref:`inductance` + + .. grid-item:: + :ref:`cogging` + + .. grid-item:: + :ref:`controller` + + .. grid-item:: + :ref:`thermal` + + .. grid-item:: + :ref:`lugre` + + .. grid-item:: + :ref:`input` + + .. dropdown:: :ref:`plugin` |*| .. grid:: 2 3 4 4 @@ -6146,6 +6245,75 @@ :ref:`delay` + .. dropdown:: :ref:`dcmotor` :octicon:`dot` + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`ctrllimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`gear` + + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + + .. grid-item:: + :ref:`cranklength` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`motorconst` + + .. grid-item:: + :ref:`resistance` + + .. grid-item:: + :ref:`nominal` + + .. grid-item:: + :ref:`saturation` + + .. grid-item:: + :ref:`inductance` + + .. grid-item:: + :ref:`cogging` + + .. grid-item:: + :ref:`controller` + + .. grid-item:: + :ref:`input` + + .. grid-item:: + :ref:`thermal` + + .. grid-item:: + :ref:`lugre` + + .. dropdown:: :ref:`custom` |*| diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf new file mode 100644 index 00000000..caf8a36c Binary files /dev/null and b/doc/_static/dcmotor.pdf differ diff --git a/doc/changelog.rst b/doc/changelog.rst index 39316130..1bd56f67 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,9 @@ Upcoming version (not yet released) General ^^^^^^^ +- Added the :ref:`dcmotor` actuator for modeling DC motors. Supports optional + electrical dynamics (inductance), cogging torque, thermal resistance variation, and LuGre friction. See the + `technical note <_static/dcmotor.pdf>`__ for more details. - Actuators with joint or tendon transmissions can now contribute :ref:`damping` and :ref:`armature` to their transmission target. These are applied during the passive force and inertia computations, respectively, and are scaled by gear\ :sup:`2` diff --git a/doc/dcmotor/buildpdf.sh b/doc/dcmotor/buildpdf.sh new file mode 100755 index 00000000..ed73a053 --- /dev/null +++ b/doc/dcmotor/buildpdf.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +pdflatex -interaction=nonstopmode -jobname=dcmotor dcmotor.tex 2>&1 | grep -E '(Error|Output written)' && \ +bibtex dcmotor 2>&1 | grep -v '^$' && \ +pdflatex -interaction=nonstopmode -jobname=dcmotor dcmotor.tex 2>&1 | grep -E '(Error|Output written)' && \ +pdflatex -interaction=nonstopmode -jobname=dcmotor dcmotor.tex 2>&1 | grep -E '(Error|Output written)' && \ +rm -f *.{aux,log,out,bbl,blg} && \ +mv dcmotor.pdf ../_static/ diff --git a/doc/dcmotor/dcmotor.tex b/doc/dcmotor/dcmotor.tex new file mode 100644 index 00000000..c60e08b4 --- /dev/null +++ b/doc/dcmotor/dcmotor.tex @@ -0,0 +1,1416 @@ +% Copyright 2026 DeepMind Technologies Limited +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. + +\documentclass[10pt, a4paper, twocolumn]{article} +\usepackage[utf8]{inputenc} +\usepackage[T1]{fontenc} +\usepackage{roboto-mono} +\usepackage{relsize} +\let\oldtexttt\texttt +\renewcommand{\texttt}[1]{{\smaller\oldtexttt{#1}}} +\usepackage{amsmath, amssymb} +\usepackage{multicol} +\usepackage{geometry} +\geometry{margin=0.75in} +\usepackage{titlesec} +\titlespacing*{\section}{0pt}{1.5ex plus 0.5ex minus 0.2ex}{1ex plus 0.2ex} +\titlespacing*{\subsection}{0pt}{1.2ex plus 0.4ex minus 0.2ex}{0.8ex plus 0.2ex} +\setlength{\parskip}{0.4ex plus 0.1ex minus 0.1ex} +\usepackage{booktabs} +\usepackage{enumitem} +\setlist[itemize]{label=\scalebox{0.8}{$\bullet$}} +\usepackage{float} +\usepackage{titling} +\setlength{\droptitle}{-4em} +\usepackage{stfloats} +\usepackage{url} +\renewcommand{\UrlFont}{\small\ttfamily} +\usepackage{tikz} +\usepackage{pgfplots} +\pgfplotsset{compat=1.18} +\usepgfplotslibrary{fillbetween} +\usepackage{hyperref} +\hypersetup{colorlinks=true, linkcolor=blue, urlcolor=blue, citecolor=blue} +\usepackage{caption} +\usepackage{subcaption} +\captionsetup{font=footnotesize, labelfont=footnotesize} +\usepackage{xcolor} +\usepackage{listings} +\lstset{ + language=C, + basicstyle=\footnotesize\ttfamily, + keywordstyle=\bfseries\color{blue!70!black}, + commentstyle=\itshape\color{gray}, + stringstyle=\color{red!60!black}, + numbers=left, + numberstyle=\tiny\color{gray}, + numbersep=5pt, + frame=single, + framerule=0.4pt, + rulecolor=\color{gray!40}, + backgroundcolor=\color{gray!5}, + breaklines=true, + columns=fullflexible, + keepspaces=true, + showstringspaces=false, + tabsize=2, + xleftmargin=1.5em, + framexleftmargin=1.5em, + aboveskip=0.8em, + belowskip=0.5em, + morekeywords={mjtNum, mjModel, mjData, mjtByte}, +} + +\newcommand{\atR}{\texttt{resistance}} +\newcommand{\atK}{\texttt{motorconst}} +\newcommand{\atKt}{\texttt{motorconst:Kt}} +\newcommand{\atKe}{\texttt{motorconst:Ke}} +\newcommand{\atVM}{\texttt{nominal:voltage}} +\newcommand{\atSTALL}{\texttt{nominal:stall\_torque}} +\newcommand{\atNLS}{\texttt{nominal:no\_load\_speed}} +\newcommand{\atTMAX}{\texttt{saturation:torque}} +\newcommand{\atIMAX}{\texttt{saturation:current}} +\newcommand{\atVMAX}{\texttt{saturation:voltage}} +\newcommand{\atCRATE}{\texttt{saturation:current\_rate}} +\newcommand{\atKP}{\texttt{controller:kp}} +\newcommand{\atKI}{\texttt{controller:ki}} +\newcommand{\atKD}{\texttt{controller:kd}} +\newcommand{\atSLEW}{\texttt{controller:slewmax}} +\newcommand{\atIMAXINT}{\texttt{controller:Imax}} +\newcommand{\atL}{\texttt{inductance:L}} +\newcommand{\atTE}{\texttt{inductance:timeconst}} +\newcommand{\atCOGA}{\texttt{cogging:amplitude}} +\newcommand{\atCOGP}{\texttt{cogging:poles}} +\newcommand{\atCOGPH}{\texttt{cogging:phase}} +\newcommand{\atRT}{\texttt{thermal:resistance}} +\newcommand{\atTC}{\texttt{thermal:capacitance}} +\newcommand{\atTT}{\texttt{thermal:timeconst}} +\newcommand{\atALPHA}{\texttt{thermal:tempcoef}} +\newcommand{\atTREF}{\texttt{thermal:reftemp}} +\newcommand{\atTAMB}{\texttt{thermal:ambient}} +\newcommand{\atSIG}{\texttt{lugre:stiffness}} +\newcommand{\atSIGD}{\texttt{lugre:damping}} +\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} +\date{} + +\begin{document} + +\maketitle + +\noindent We review DC motors and describe MuJoCo's \texttt{dcmotor} actuator. The equations are derived for brushed motors but apply equally to brushless ones, where electronic commutation reduces to an equivalent circuit. + +%============================================================================= +% BACKGROUND +%============================================================================= +\section{Background} +\label{sec:background} + +We use SI units throughout, but any coherent system of units applies. We assume motion is rotational; for linear motion replace radians with meters as required. + +% --------------------------------------------------------------------------- +% Electromagnetic Model +% --------------------------------------------------------------------------- +\subsection{Electromagnetic Model} +\label{sec:electromagnetics} + +The key electro-mechanical variables are + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$v$ & Applied voltage & Volt \\ +$i$ & Current & Ampere \\ +$\omega$ & Angular velocity & radian/second \\ +$\tau$ & Output torque & Newton $\cdot$ meter \\ +\bottomrule +\end{tabular} +\end{table} + +\noindent and the key constants are + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$R$ & Resistance & Ohm \\ +$K_t$ & Torque constant & Newton $\cdot$ meter/Ampere \\ +$K_e$ & Back-EMF constant & Volt $\cdot$ second/radian \\ +\bottomrule +\end{tabular} +\end{table} + +\noindent The quasi-static model~\cite{hughes2019, maxon_formulas, simscape_dcmotor} assumes instantaneous electrical dynamics: current and torque are direct functions of voltage and velocity. The constitutive equations are the voltage balance \eqref{eq:voltage} and the torque law \eqref{eq:torque_law}: +\begin{subequations} +\label{eq:motor_laws} +\begin{align} + v &= i \, R + K_e \, \omega \label{eq:voltage} \\ + \tau &= K_t i \label{eq:torque_law} +\end{align} +\end{subequations} + +\noindent Solving for current and substituting, we have + +\begin{equation} + \tau = \frac{K_t}{R} (v - K_e \, \omega) + \label{eq:torque_speed} +\end{equation} + +\noindent Output torque is proportional to the difference between applied and back-EMF voltage $v_{\text{back}} = K_e \, \omega$ (Figure~\ref{fig:torque_speed}). + +\paragraph{Electrical constants.} +Fundamentally, both $K_t$ and $K_e$ arise from the same physical quantity: the magnetic flux $\Phi$ of the coil. Faraday's law gives $v_{\text{back}} = \Phi \, \omega$ and the Lorentz force gives $\tau = \Phi \, i$, so in SI units: + +\begin{equation*} + K_e = K_t + \label{eq:ke_eq_kt} +\end{equation*} + +\noindent This can also be seen from energy conservation: $P_e = i \, (K_e \, \omega) = (K_t \, i) \, \omega = P_m$. + +\pagebreak +\noindent Note the dimensions match: + +\vspace*{-\abovedisplayskip} +\begin{equation*} + \frac{\text{Volt}}{\text{radian}/\text{second}} + = \frac{\text{Joule}}{\text{Coulomb}/\text{second}} + = \frac{\text{Newton} \cdot \text{meter}}{\text{Ampere}} +\end{equation*} + +\noindent If these constants are the same, why have both? Two reasons. First, datasheets typically use mixed units ($K_e$ in RPM/V, $K_t$ in mN$\cdot$m/A), giving different values for the same physical quantity. Second, $K_t$ and $K_e$ are measured differently: $K_t$ by locking the rotor and measuring torque per Ampere; $K_e$ by spinning the rotor and measuring open-circuit Volts per radian/second. In the first case, high currents can lead to magnetic field saturation in the core, causing the effective $K_t$ to drop below $K_e$. The equality $K_e = K_t$ thus assumes $\Phi$ independent of $i$. We make this assumption for now and use a single motor constant $K \equiv K_t = K_e$ throughout the remainder of this document and internally in MuJoCo, but see note at end of \S\ref{sec:dcmotor}. + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.0} +\pgfmathsetmacro{\wz}{1.0} +\begin{axis}[ + width=0.9\columnwidth, height=0.55\columnwidth, + axis lines=left, + clip=false, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=0, xmax={\wz*1.15}, ymin=0, ymax={\taus*1.15}, + xtick={\wz}, xticklabels={$\omega_0$}, + ytick={\taus}, yticklabels={$\tau_0$}, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\addplot[thick, blue!15] coordinates {(0,\taus*0.7) (\wz*0.7,0)}; +\addplot[thick, blue!25] coordinates {(0,\taus*0.8) (\wz*0.8,0)}; +\addplot[thick, blue!50] coordinates {(0,\taus*0.9) (\wz*0.9,0)}; +\addplot[thick, blue] coordinates {(0,\taus) (\wz,0)}; +\node[font=\scriptsize, text=gray, align=center] + at (axis cs: \wz*0.25, \taus*0.2) + {decreasing\\ voltage}; +\draw[->, thick, gray] (axis cs: \wz*0.4, \taus*0.55) + -- (axis cs: \wz*0.4, \taus*0.15); +\node[font=\scriptsize] at (axis cs: \wz*0.4, -0.08) + {Speed}; +\node[font=\scriptsize, rotate=90] at (axis cs: -0.04, \taus*0.4) + {Torque}; +\end{axis} +\end{tikzpicture} +\caption{Torque-speed relationship \eqref{eq:torque_speed} at fixed voltage. As voltage decreases, the maximum torque and speed decrease linearly.} +\label{fig:torque_speed} +\end{figure} + +\paragraph{Current Saturation.} +A maximum current rating $i_{\max}$ limits the output torque: +\begin{equation} + \tau = \text{clip}\!\left(\frac{K}{R}(v - K \, \omega),\; + \pm K \, i_{\max} \right) + \label{eq:saturation} +\end{equation} +where the maximum torque $\tau_{\max} = K \, i_{\max}$. The feasible torque-speed envelope forms a parallelogram: + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.3} +\pgfmathsetmacro{\wz}{1.0} +\pgfmathsetmacro{\taumax}{0.7} +\pgfmathsetmacro{\slope}{\taus/\wz} +\pgfmathsetmacro{\wcu}{(\taus-\taumax)/\slope} +\pgfmathsetmacro{\wcl}{(\taus+\taumax)/\slope} +\pgfmathsetmacro{\wext}{1.5} +\pgfmathsetmacro{\dexthi}{\taus+\slope*\wext} +\pgfmathsetmacro{\dextlo}{\taus-\slope*\wext} +\begin{axis}[ + width=0.9\columnwidth, height=0.6\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=-1.6, xmax=1.6, ymin=-1.6, ymax=1.6, + xtick={-\wz, \wz}, xticklabels={$-\omega_0$, {}}, + ytick={-\taus, \taus}, + yticklabels={$-\tau_0$, $\tau_0$}, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\fill[blue, opacity=0.08] + (-\wcl, \taumax) -- (\wcu, \taumax) -- (\wcl, -\taumax) + -- (-\wcu, -\taumax) -- cycle; +\addplot[thick, dashed, gray] coordinates {(-\wext, \dexthi) (\wext, \dextlo)}; +\addplot[thick, dashed, gray] coordinates {(-\wext, -\dextlo) (\wext, -\dexthi)}; +\addplot[thick, dashed, gray] coordinates {(-1.55, \taumax) (1.55, \taumax)}; +\addplot[thick, dashed, gray] coordinates {(-1.55, -\taumax) (1.55, -\taumax)}; +\addplot[thick, blue] coordinates + {(-\wcl, \taumax) (\wcu, \taumax) (\wcl, -\taumax) (-\wcu, -\taumax) + (-\wcl, \taumax)}; +\node[font=\scriptsize, anchor=south] at (axis cs: \wz, \taumax) + {$\tau_{\max}$}; +\node[font=\scriptsize, anchor=north] at (axis cs: -\wz, -\taumax) + {$-\tau_{\max}$}; +\draw[thick, dashed, gray] (axis cs: \wz, 0) -- (axis cs: \wz, -\taumax); +\draw[thick, dashed, gray] (axis cs: -\wz, 0) -- (axis cs: -\wz, \taumax); +\node[font=\normalsize, anchor=south] at (axis cs: \wz, 0.05) {$\omega_0$}; +\end{axis} +\end{tikzpicture} +\caption{Torque-speed envelope with current saturation~\eqref{eq:saturation}.} +\label{fig:saturation} +\end{figure} + +\noindent Note that datasheets typically distinguish two current limits. The \emph{continuous} (or \emph{nominal}) current $i_{\max}$ is the thermal limit: the maximum current the motor can sustain indefinitely without exceeding its maximum winding temperature. The \emph{peak} current $i_{\text{peak}}$ is a higher short-term limit, typically 5--10$\times$ the continuous value, constrained by demagnetization or commutation limits. + +\begin{table}[H] +\centering +\footnotesize +\setlength{\tabcolsep}{3pt} +\renewcommand{\arraystretch}{1.2} +\begin{tabular}{@{}llll@{}} +\toprule +Symbol & Description & Condition & Formula/note\\ +\midrule +$\tau_0$ & Stall Torque & $\omega=0$ & $\tau_0 = Kv / R$ \\ +$\omega_0$ & No-Load Speed & $\tau_{\text{load}}=0$ & + $\omega_0 \approx v / K$ \\ +$\partial\omega / \partial\tau$ & Gradient & Slope & + $-R/K^2$ \\ +$i_{\max}$ & Maximum Current & Limit & Thermal limit \\ +$\tau_{\max}$ & Maximum Torque & Limit & $\tau_{\max} = K \, i_{\max}$ \\ +\bottomrule +\end{tabular} +\caption{Named constants derived from the motor equations.} +\label{tab:electromech_constants} +\end{table} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Inductance} +\label{sec:inductance} + +Including the effects of winding inductance $L$ (Henry) means treating the current $i$ as a state variable: +\begin{equation} + v = L \, \frac{di}{dt} + i \, R + K \, \omega + \label{eq:inductance} +\end{equation} +The change in current is proportional to the voltage and negatively proportional to both the instantaneous current and the rotation velocity. The time constant of this ODE is $t_e = L/R$. If $t_e \ll \Delta t$ (the simulation timestep), the current equilibrates within a single step and the quasi-static approximation~\eqref{eq:torque_speed} is adequate. + +Motor drivers often impose a hard limit on $di/dt$ to protect windings and electronics, bounding the torque ramp rate to $K \cdot (di/dt)_{\max}$. + +% --------------------------------------------------------------------------- +% Mechanical Model +% --------------------------------------------------------------------------- +\subsection{Mechanical Model} +\label{sec:mechanical} + +Several purely mechanical phenomena affect the motor's behavior and the effective delivered torque. + +\paragraph{Mechanical losses.} +These reduce the net torque available at the shaft: $\tau_{\text{net}} = \tau_{\text{elec}} - \tau_{\text{loss}}$. +\begin{itemize} + \item \textbf{Coulomb:} Constant torque opposing rotation (dry friction). + $\tau_{\text{loss}} = \tau_c \, \text{sgn}(\omega)$. Discontinuous; already available in MuJoCo as \texttt{frictionloss}. + \item \textbf{Viscous:} Drag is a smooth function of speed $\tau_{\text{loss}} = b(\omega)$. The simplest model is linear, with drag proportional to speed $\tau_{\text{loss}} = B \, \omega$, but higher order terms may be needed for higher-fidelity models e.g., $\tau_{\text{loss}} = B_1 \, \omega + B_2 \, \omega |\omega| + B_3 \, \omega^3 + \dots$. +\end{itemize} + +\noindent Datasheets report the \emph{no-load current} $i_0$: the current drawn when spinning freely at no-load speed $\omega_0$. At steady state, the electromagnetic torque balances all mechanical losses: +\begin{equation} + K \, i_0 = \tau_c + B \, \omega_0 + \label{eq:noload} +\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: +\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) + \label{eq:net_torque} +\end{equation*} +The clipping applies to the electrical torque (current limit), while both friction terms are mechanical losses applied to the output post-clipping. Viscous drag $-B\omega$ tilts the envelope; Coulomb friction $-\tau_c\,\text{sgn}(\omega)$ shifts the right half ($\omega > 0$) down and the left half ($\omega < 0$) up, creating a $2\tau_c$ discontinuity at $\omega = 0$ (Figure~\ref{fig:drag_linear}). + +\begin{figure}[H] +\centering +\begin{subfigure}[t]{\columnwidth} +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.3} +\pgfmathsetmacro{\wz}{1.0} +\pgfmathsetmacro{\taumax}{0.7} +\pgfmathsetmacro{\bvis}{0.3} +\pgfmathsetmacro{\tauf}{0.2} +\pgfmathsetmacro{\slope}{\taus/\wz} +\pgfmathsetmacro{\wcu}{(\taus-\taumax)/\slope} +\pgfmathsetmacro{\wcl}{(\taus+\taumax)/\slope} +\pgfmathsetmacro{\wext}{1.5} +\pgfmathsetmacro{\dragL}{\bvis*\wext} +\pgfmathsetmacro{\dragR}{-\bvis*\wext} +%% Right half vertices (ω ≥ 0): shifted down by τ_f +\pgfmathsetmacro{\Ra}{\taumax-\tauf} +\pgfmathsetmacro{\Rb}{\taumax-\bvis*\wcu-\tauf} +\pgfmathsetmacro{\Rc}{-\taumax-\bvis*\wcl-\tauf} +\pgfmathsetmacro{\Rd}{-\taumax-\tauf} +%% Left half vertices (ω ≤ 0): shifted up by τ_f +\pgfmathsetmacro{\La}{\taumax+\bvis*\wcl+\tauf} +\pgfmathsetmacro{\Lb}{\taumax+\tauf} +\pgfmathsetmacro{\Lc}{-\taumax+\tauf} +\pgfmathsetmacro{\Ld}{-\taumax+\bvis*\wcu+\tauf} +\begin{axis}[ + width=0.9\columnwidth, height=0.6\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=-1.8, xmax=1.8, ymin=-1.8, ymax=1.8, + xtick=\empty, ytick=\empty, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\fill[blue, opacity=0.08] + (0, \Ra) -- (\wcu, \Rb) -- (\wcl, \Rc) -- (0, \Rd) -- cycle; +\addplot[thick, blue] coordinates + {(0, \Ra) (\wcu, \Rb) (\wcl, \Rc) (0, \Rd)}; +\fill[blue, opacity=0.08] + (-\wcl, \La) -- (0, \Lb) -- (0, \Lc) -- (-\wcu, \Ld) -- cycle; +\addplot[thick, blue] coordinates + {(-\wcl, \La) (0, \Lb) (0, \Lc) (-\wcu, \Ld) (-\wcl, \La)}; +\addplot[thick, dashed, gray] coordinates {(0.01, {-\tauf-\bvis*0.01}) (\wext, {-\tauf+\dragR})}; +\addplot[thick, dashed, gray] coordinates {(-\wext, {\tauf+\dragL}) (-0.01, {\tauf+\bvis*0.01})}; +\node[font=\footnotesize, anchor=north west] at (axis cs: -1.75, -0.45) + {$-B\omega - \tau_c\,\text{sgn}(\omega)$}; +\draw[->, gray, thick] (axis cs: -1.4, -0.45) -- (axis cs: -1.3, {\tauf+\bvis}); +\pgfmathsetmacro{\gapmid}{(\Ra+\Lb)/2} +\draw[thick, <->, gray] (axis cs: 0.12, \Ra) -- (axis cs: 0.12, \Lb); +\node[font=\scriptsize, anchor=west] at (axis cs: 0.18, \gapmid) + {$2\tau_c$}; +\end{axis} +\end{tikzpicture} +\caption{Linear viscous drag and Coulomb friction.} +\label{fig:drag_linear} +\end{subfigure} + +\vspace{0.5em} + +\begin{subfigure}[t]{\columnwidth} +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.3} +\pgfmathsetmacro{\wz}{1.0} +\pgfmathsetmacro{\taumax}{0.7} +\pgfmathsetmacro{\Bone}{0.15} +\pgfmathsetmacro{\Btwo}{0.35} +\pgfmathsetmacro{\slope}{\taus/\wz} +\pgfmathsetmacro{\wcl}{(\taus+\taumax)/\slope} +\pgfmathsetmacro{\wext}{1.7} +\begin{axis}[ + width=0.9\columnwidth, height=0.6\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=-2.0, xmax=2.0, ymin=-2.0, ymax=2.0, + xtick=\empty, ytick=\empty, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, + samples=200, +] +\addplot[name path=upper, thick, blue, domain=-\wcl:\wcl] + {min(\taus - \slope*x, \taumax) - \Bone*x - \Btwo*x*abs(x)}; +\addplot[name path=lower, thick, blue, domain=-\wcl:\wcl] + {max(-\taus - \slope*x, -\taumax) - \Bone*x - \Btwo*x*abs(x)}; +\addplot[blue, opacity=0.08] fill between[of=upper and lower]; +\addplot[thick, dashed, gray, domain=-\wext:\wext] + {-\Bone*x - \Btwo*x*abs(x)}; +\node[font=\footnotesize, anchor=north west] at (axis cs: -1.9, -0.5) + {$-b(\omega)$}; +\draw[->, gray, thick] (axis cs: -1.5, -0.5) -- (axis cs: -1.3, {0.1 + 0.35*1.3*1.3}); +\end{axis} +\end{tikzpicture} +\caption{Nonlinear viscous drag $b(\omega) = B_1\omega + B_2\omega|\omega|$, no friction.} +\label{fig:drag_nonlinear} +\end{subfigure} + +\caption{Torque-speed envelopes with mechanical losses. The dashed gray line shows the drag function; the shaded region is the achievable torque at each speed. Note that datasheet torque-speed curves typically plot the first quadrant only.} +\label{fig:drag} +\end{figure} + +\paragraph{Rotor Inertia and Gearing.} +Every DC motor datasheet lists the rotor inertia $J_r$ (kg$\cdot$m$^2$). When a gear train with ratio $N$ is attached, the effective inertia reflected to the output shaft is $J_{\text{eff}} = J_r N^2$~\cite{tedrake2024}. Note that real gearboxes also introduce efficiency losses (typically 70--90\%), which reduce the transmitted torque by a multiplicative factor $\eta$, approximated by effectively reducing the motor constant $K_{\text{eff}} = \eta K$. + +\paragraph{Cogging Torque.} +Brushless DC motors exhibit \emph{cogging torque}: a position-dependent torque ripple caused by the interaction between permanent magnets and stator slots. It can be modeled as a periodic bias: +\begin{equation} + \tau_{\text{cog}}(\theta) = A \sin(N_p \, \theta + \phi) + \label{eq:cogging} +\end{equation} +where $A$ is the amplitude, $N_p$ is the number of pole pairs times the number of slots per pole, and $\phi$ is a phase offset. Cogging torque is significant primarily at low speeds. Datasheets sometimes report peak cogging as a percentage of rated torque (typically 1--5\%). + +\begin{table}[H] +\centering +\footnotesize +\setlength{\tabcolsep}{3pt} +\renewcommand{\arraystretch}{1.2} +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Formula / Note \\ +\midrule +$\tau_c$ & Coulomb friction & $\tau_c\,\text{sgn}(\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$ \\ +$N$ & Gear ratio & $J_{\text{eff}} = J_r N^2$ \\ +$\eta$ & Gearbox efficiency & $K' = \eta \, K$ \\ +$A$ & Cogging amplitude & $\tau_{\text{cog}} = A\sin(N_p\theta + \phi)$ \\ +$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).} +\label{tab:key_constants} +\end{table} + +\paragraph{Backlash.} +Gearboxes introduce backlash: a small angular deadband where the motor can turn without moving the output shaft. Datasheets report this in arcminutes. MuJoCo supports backlash modeling via a dual-joint decomposition; \href{https://mujoco.readthedocs.io/en/stable/modeling.html#backlash}{see here} for details. + +% --------------------------------------------------------------------------- +% Thermal Model +% --------------------------------------------------------------------------- +\subsection{Thermal Model} +\label{sec:thermal} + +Winding temperature affects motor performance primarily through increased copper resistance, and can be modeled as a single lumped thermal state. The thermal constants are + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$R_T$ & Thermal resistance & Kelvin/Watt \\ +$C$ & Thermal capacitance & Joule/Kelvin \\ +$t_T = R_T C$ & Thermal time constant & second \\ +$\alpha$ & Resistance temp.\ coefficient & 1/Kelvin \\ +$T_0$ & Reference temperature & degree Celsius \\ +$T_a$ & Ambient temperature & degree Celsius \\ +\bottomrule +\end{tabular} +\caption{Thermal model constants. Units involving temperature differences use Kelvin (equivalent to Celsius for differences); absolute temperatures use degree Celsius, following datasheet convention.} +\end{table} + +\noindent Note that some manufacturers specify two thermal resistances: $R_{\text{th1}}$ (winding-to-housing) and $R_{\text{th2}}$ (housing-to-ambient), which sum to give the total winding-to-ambient thermal resistance $R_T = R_{\text{th1}} + R_{\text{th2}}$. The single-node model above uses $R_T$ directly. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Lumped Thermal ODE} +\label{sec:thermal_ode} + +The winding temperature $T$ evolves according to a first-order lumped model driven by the power dissipation $P$ (Watt, detailed in \S\ref{sec:thermal_losses}): +\begin{equation} + \frac{\partial T}{\partial t} = \frac{1}{C} P - \frac{T - T_a}{t_T} + \label{eq:thermal_ode} +\end{equation} +where $t_T = R_T C$ is the thermal time constant. This produces exponential rise/decay toward a steady-state temperature $T_{ss} = T_a + R_T P$ (Figure~\ref{fig:thermal_response}). + +\begin{figure}[ht] +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\Tss}{1.0} +\pgfmathsetmacro{\ttau}{1.0} +\pgfmathsetmacro{\xmax}{4.5} +\begin{axis}[ + width=0.9\columnwidth, height=0.45\columnwidth, + axis lines=left, + clip=false, + xlabel={$t$}, ylabel={$T - T_a$}, + xmin=0, xmax=\xmax, ymin=0, ymax={\Tss*1.25}, + xtick={\ttau}, xticklabels={$t_T$}, + ytick={{\Tss*(1-exp(-1))}, \Tss}, + yticklabels={$(1{-}1/e)\,R_T P$, $R_T P$}, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\addplot[thick, blue, domain=0:\xmax, samples=100] + {\Tss*(1 - exp(-x/\ttau))}; +\addplot[thick, dashed, gray] coordinates {(0,\Tss) (\xmax,\Tss)}; +\draw[thick, dashed, gray] (axis cs:\ttau, 0) -- (axis cs:\ttau, {\Tss*(1-exp(-1))}); +\draw[thick, dashed, gray] (axis cs:0, {\Tss*(1-exp(-1))}) -- (axis cs:\ttau, {\Tss*(1-exp(-1))}); +\node[font=\scriptsize, anchor=south] at (axis cs:\xmax*0.5, \Tss) + {$T_{ss} = T_a + R_T P$}; +\end{axis} +\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} +\end{figure} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Losses} +\label{sec:thermal_losses} + +The dominant loss is copper (Joule) heating: +\begin{equation*} + P = i^2 R(T) + \label{eq:copper_loss} +\end{equation*} +Optionally, speed-dependent iron losses (eddy-current and hysteresis losses in the stator laminations) can be included~\cite{hughes2019}: +\begin{equation*} + P = i^2 R(T) + K_{\text{fe}} \omega^2 + \label{eq:total_loss} +\end{equation*} +The iron loss coefficient $K_{\text{fe}}$ is not typically listed on datasheets and must be identified from efficiency curves or manufacturer simulation tools. For most hobby and robotics motors, iron losses are small compared to copper losses and can be neglected. They become significant at high speeds in large industrial motors. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Temperature-Dependent Resistance} +\label{sec:resistance_temperature} + +Copper resistance increases approximately linearly with temperature: +\begin{equation} + R(T) = R_0 \left(1 + \alpha (T - T_0)\right) + \label{eq:resistance_temperature} +\end{equation} +where $R_0$ is resistance at reference temperature $T_0$ and $\alpha \approx 0.0039 \, \text{K}^{-1}$ for copper. This is the dominant thermal feedback: as $T$ rises, $R$ increases, so for a given voltage the current $i = (v - K \omega) / R(T)$ drops, reducing torque. + +Note that $R(T)$ also increases heating for a given current ($P = i^2 R(T)$), creating mild positive feedback under current control. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Magnet Flux Derating} +\label{sec:magnet_derating} + +Permanent magnet flux weakens with temperature, reducing $K$: +\begin{equation*} + K(T) = K_0 \left(1 + \alpha_m (T - T_0)\right) + \label{eq:kt_temperature} +\end{equation*} +with $\alpha_m < 0$ (motor-dependent). This effect is often small over normal operating ranges and can be ignored. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Thermal Derating} +\label{sec:thermal_derating} + +Real actuators limit current as winding temperature approaches a maximum: +\begin{equation*} + i_{\max}(T) = \begin{cases} + i_{\text{rated}} & T \le T_1 \\ + i_{\text{safe}} + (i_{\text{rated}} - i_{\text{safe}}) \, s(T) & T_1 < T < T_2 \\ + i_{\text{safe}} & T \ge T_2 + \end{cases} +\end{equation*} +where $s(T)$ is a smooth interpolant between $T_1$ and $T_2$. This reduces the maximum available torque as the motor heats up. + +% --------------------------------------------------------------------------- +% Micro-Friction Models +% --------------------------------------------------------------------------- +\subsection{Micro-Friction Models} +\label{sec:micro_friction} + +Simple macroscopic friction models (Coulomb, viscous) cannot capture complex mechanical phenomena common in real motors with gear trains, such as pre-sliding hysteresis and stick-slip limit cycles. To capture these behaviors, a richer dynamic model is required. At the microscopic level, two surfaces in contact touch at many asperities which deform elastically under tangential load. This can be modeled as an average bristle deflection $z$, governed by a first-order ODE driven by the relative velocity $\omega$. Friction torque is then a function of $z$, $\dot{z}$, and $\omega$. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Dahl Model} +\label{sec:dahl} + +The simplest stateful model~\cite{dahl68} treats friction as a rate-independent hysteresis operator derived from the stress-strain curve: +\begin{equation*} + \dot{z} = \omega - \frac{\sigma_0}{\tau_c} |\omega| \, z + \label{eq:dahl_state} +\end{equation*} +with output $\tau = -\sigma_0 z$. In steady state ($\dot{z}=0$), $z_{ss} = \tau_c \, \text{sgn}(\omega) / \sigma_0$ so $\tau_{ss} = -\tau_c \, \text{sgn}(\omega)$: pure Coulomb friction opposing motion. The two parameters are the bristle stiffness $\sigma_0$ (torque/radian) and the Coulomb friction torque $\tau_c$. +For small displacements the model is approximately linear ($\tau \approx -\sigma_0 \theta$), giving spring-like pre-sliding behavior with hysteresis during direction reversals (Figure~\ref{fig:hysteresis}). The Dahl model does not capture the Stribeck effect~\cite{stribeck1902} (the drop in friction at low velocity) and thus cannot predict stick-slip motion. + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\begin{axis}[ + width=0.9\columnwidth, height=0.55\columnwidth, + axis lines=middle, + xlabel={$\theta$}, ylabel={$\tau$}, + xmin=-1.4, xmax=1.4, ymin=-1.4, ymax=1.4, + xtick=\empty, ytick=\empty, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, + clip=false, +] +\pgfmathsetmacro{\sig}{2.5} +\pgfmathsetmacro{\Fc}{1.0} +\pgfmathsetmacro{\xm}{1.0} +\pgfmathsetmacro{\ch}{(exp(\sig*\xm)+exp(-\sig*\xm))/2} +\addplot[thick, blue, domain=-\xm:\xm, samples=150, name path=lower] + {-\Fc*(1 - exp(-\sig*x)/\ch)}; +\addplot[thick, blue, domain=-\xm:\xm, samples=150, name path=upper] + {\Fc*(1 - exp(\sig*x)/\ch)}; +\addplot[blue, opacity=0.08] fill between[of=lower and upper]; +\draw[thick, dotted] (axis cs:-1.4, -\Fc) -- (axis cs:1.4, -\Fc) + node[right, font=\scriptsize] {$-\tau_c$}; +\draw[thick, dotted] (axis cs:-1.4, \Fc) -- (axis cs:1.4, \Fc) + node[right, font=\scriptsize] {$\tau_c$}; +\draw[->, thick, gray] (axis cs:0.05, -0.78) -- (axis cs:0.25, -0.83); +\draw[->, thick, gray] (axis cs:-0.05, 0.78) -- (axis cs:-0.25, 0.83); +\end{axis} +\end{tikzpicture} +\caption{Hysteresis loop: friction torque $\tau$ vs.\ displacement $\theta$ under slow + periodic loading (Dahl model). + The loop area represents energy dissipated per cycle. + Unlike memoryless Coulomb friction, the torque is continuous.} +\label{fig:hysteresis} +\end{figure} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{LuGre Model} +\label{sec:lugre} + +The LuGre model~\cite{dewit95, lugre_revisited} extends Dahl by making the bristle saturation velocity-dependent and adding micro-damping and viscous terms: +\begin{subequations} +\label{eq:lugre} +\begin{align} + \dot{z} &= \omega - \sigma_0 \frac{|\omega|}{g(\omega)} z + \label{eq:lugre_state} \\ + \tau &= -(\sigma_0 z + \sigma_1 \dot{z} + \sigma_2 \omega) + \label{eq:lugre_force} +\end{align} +\end{subequations} +where the function $g(\omega)$ captures the Stribeck effect: +\begin{equation} + g(\omega) = \tau_c + (\tau_s - \tau_c) \, e^{-(\omega/\omega_s)^\gamma} + \label{eq:stribeck} +\end{equation} +with exponent $\gamma$ typically 1 or 2. In steady state, $\tau_{ss}(\omega) = -g(\omega) \, \text{sgn}(\omega) - \sigma_2 \omega$: the classic Stribeck curve (Figure~\ref{fig:stribeck}). + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\begin{axis}[ + width=0.9\columnwidth, height=0.55\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau_{ss}$}, + xmin=-3, xmax=3, ymin=-2.5, ymax=2.5, + xtick=\empty, ytick=\empty, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, + clip=false, +] +\pgfmathsetmacro{\Fc}{1.0} +\pgfmathsetmacro{\Fs}{1.8} +\pgfmathsetmacro{\vs}{0.5} +\pgfmathsetmacro{\sigtwo}{0.15} +\addplot[thick, blue, domain=0.01:3, samples=200] + {-(\Fc + (\Fs-\Fc)*exp(-(x/\vs)^2)) - \sigtwo*x}; +\addplot[thick, blue, domain=-3:-0.01, samples=200] + {(\Fc + (\Fs-\Fc)*exp(-(-x/\vs)^2)) - \sigtwo*x}; +\addplot[thick, dashed, gray, domain=-3:3, samples=2] {-\sigtwo*x}; +\draw[thick, dotted] (axis cs:0,-\Fs) -- (axis cs:3,-\Fs) + node[right, font=\scriptsize] {$-\tau_s$}; +\draw[thick, dotted] (axis cs:0,-\Fc) -- (axis cs:3,-\Fc) + node[right, font=\scriptsize] {$-\tau_c$}; +\draw[thick, dotted] (axis cs:0,\Fs) -- (axis cs:-3,\Fs) + node[left, font=\scriptsize] {$\tau_s$}; +\draw[thick, dotted] (axis cs:0,\Fc) -- (axis cs:-3,\Fc) + node[left, font=\scriptsize] {$\tau_c$}; +\node[font=\footnotesize, anchor=north east] at (axis cs:-0.5, -0.1) + {$-\sigma_2\omega$}; +\end{axis} +\end{tikzpicture} +\caption{Steady-state friction $\tau_{ss}(\omega) = -g(\omega)\,\text{sgn}(\omega) - \sigma_2 \omega$. + Stiction torque $\tau_s$ at $\omega\!=\!0$ drops to Coulomb level $\tau_c$ + over velocity scale $\omega_s$ (Stribeck effect). Friction opposes motion.} +\label{fig:stribeck} +\end{figure} + +\noindent The Dahl model is recovered by setting $g(\omega) = \tau_c$ and $\sigma_1 = \sigma_2 = 0$. Linearizing around $\omega = z = 0$ gives second-order dynamics $J\ddot{\theta} - (\sigma_1 + \sigma_2)\dot{\theta} - \sigma_0 \theta = \tau$ (applied torque): a spring-damper with natural frequency $\omega_n = \sqrt{\sigma_0/J}$, critically damped when $\sigma_1 = 2\sqrt{J\sigma_0}$. + +The LuGre model can be shown to be input-strictly-passive (the map $\omega \mapsto \tau$ dissipates energy) provided $\sigma_2 > \sigma_1 (\tau_s - \tau_c)/\tau_c$. This passivity condition limits $\sigma_1$ and can lead to underdamped micro-dynamics, motivating the following extension. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Velocity-Dependent Damping} +\label{sec:vel_damping} + +The passivity constraint on $\sigma_1$ can be relaxed by making the micro-damping decrease with velocity: +\begin{equation*} + \sigma_1(\omega) = \bar{\sigma}_1\, e^{-(\omega/\omega_s)^\beta} + \label{eq:sigma1_vel} +\end{equation*} +This allows large damping in the stiction regime (good for numerical stability and physical fidelity) while satisfying passivity at higher velocities where $\sigma_1 \to 0$. + +Together with $\tau_c$ from \S\ref{sec:mechanical}, the LuGre model with velocity-dependent damping adds six parameters: +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$\sigma_0$ & Bristle stiffness, pre-sliding slope & N$\cdot$m/rad \\ +$\bar{\sigma}_1$ & Peak bristle damping at $\omega = 0$ & N$\cdot$m$\cdot$s/rad \\ +$\sigma_2$ & Viscous damping coefficient & N$\cdot$m$\cdot$s/rad \\ +$\tau_s$ & Stiction torque, $\tau_s \ge \tau_c$ & N$\cdot$m \\ +$\omega_s$ & Stribeck velocity & rad/s \\ +$\beta$ & Damping decay exponent & dimensionless \\ +\bottomrule +\end{tabular} +\caption{Parameters of the LuGre friction model (\S\ref{sec:lugre}--\ref{sec:vel_damping}).} +\label{tab:lugre_params} +\end{table} + + +\newpage +%============================================================================= +% IMPLEMENTATION +%============================================================================= +\section{Implementation} +\label{sec:implementation} + +Here we describe MuJoCo's \texttt{dcmotor} actuator. Some scalars are grouped into vectors; we use a colon to denote such scalar sub-attributes, e.g.\ \texttt{cogging:phase} refers to the third element of the \texttt{cogging} attribute (see Tables \ref{tab:mjcf_attributes} and \ref{tab:cogging_impl}). + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Size & Description \\ +\midrule +\texttt{resistance} & 1 & Terminal resistance $R$ \\ +\texttt{motorconst} & 2 & Motor constants ($K_t, K_e$; see below) \\ +\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}$) \\ +\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{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}$) \\ +\bottomrule +\end{tabular} +\caption{MJCF attributes for the \texttt{dcmotor} actuator, split into electrical, mechanical and control groupings.} +\label{tab:mjcf_attributes} +\end{table} + +% --------------------------------------------------------------------------- +% Stateless dcmotor +% --------------------------------------------------------------------------- +\subsection{Stateless DC Motor} +\label{sec:dcmotor} + +The output torque of the stateless motor follows Eq.~\eqref{eq:saturation}, mapping physical parameters to the underlying affine model. The three core parameters are the effective motor constant $K$, resistance $R$, and maximum torque $\tau_{\max}$. +They are stored in \texttt{mjModel} as follows: + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & \texttt{mjModel} storage \\ +\midrule +$R$ & Resistance & \texttt{gainprm[0]} \\ +$K$ & Effective motor constant & \texttt{gainprm[1]} \\ +$\tau_{\max}$ & Maximum torque & \texttt{forcerange} \\ +\bottomrule +\end{tabular} +\caption{Stateless DC motor core parameters.} +\end{table} + +\noindent The gain $G = K/R$ and back-EMF bias $-GK\omega$ are computed at runtime. Storing $R$ separately allows temperature-dependent resistance $R(T)$, Eq.~\eqref{eq:resistance_temperature}, to be applied. These three core parameters can be specified with a combination of eight sub-attributes + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atR{} & $R$ & Ohm \\ +\atKt{} & $K_t$ & N$\cdot$m/A \\ +\atKe{} & $K_e$ & V$\cdot$s/rad \\ +\atVM{} & $v_n$ & Volt \\ +\atSTALL{} & $\tau_0\!=\!Kv_n/R$ & N$\cdot$m \\ +\atNLS{} & $\omega_0\!\approx\!v_n/K$ & rad/s \\ +\atTMAX{} & $\tau_{\max}$ & N$\cdot$m \\ +\atIMAX{} & $i_{\max}$ & Ampere \\ +\bottomrule +\end{tabular} +\caption{Stateless DC motor basic attributes.} +\end{table} + +\noindent The attribute \atK{} has two sub-attributes, \texttt{Kt} and \texttt{Ke}. If both are positive, $K = \sqrt{K_t K_e}$ (preserving power balance $K^2 = K_t K_e$). If only one is positive, $K$ equals that value. + +\pagebreak +\noindent The following attribute combinations are supported: +\begin{enumerate}[itemsep=2pt, parsep=0pt, topsep=2pt] + \item Effective motor constant $K$, one of: + \begin{itemize}[itemsep=1pt, parsep=0pt, topsep=1pt] + \item \atKt{} \emph{and/or} \atKe{} + \item \atNLS{} \emph{and} \atVM{} + \end{itemize} + \item Resistance $R$, one of: + \begin{itemize}[itemsep=1pt, parsep=0pt, topsep=1pt] + \item \atR{} + \item \atSTALL{} \emph{and} \atVM{} + \end{itemize} + \item Maximum torque $\tau_{\max}$, one of: + \begin{itemize}[itemsep=1pt, parsep=0pt, topsep=1pt] + \item \atTMAX{} + \item \atIMAX{} + \end{itemize} +\end{enumerate} + +\noindent \atIMAX{} corresponds to the continuous (thermal) current limit. Peak current behavior can be approximated with the thermal model (\S\ref{sec:temperature_impl}). + +\paragraph{Rotor Inertia and Gearing.} To model rotor inertia with a gear train, set the actuator's \texttt{armature} $= J_r$ and \texttt{gear} $= N$. Actuator-level \texttt{armature} automatically scales the inertia by $N^2$ to reflect $J_{\text{eff}}$ to the output shaft. + +\paragraph{Mechanical Drag.} The full torque-speed envelope applies viscous drag \emph{outside} the current clamp: +\begin{equation*} + \tau_{\text{net}} = \text{clip}\!\left( \frac{K}{R}(v - K \, \omega),\; + \pm \tau_{\max} \right) - b(\omega) + \label{eq:drag} +\end{equation*} +Actuator-level \texttt{damping} reproduces this post-clamp behavior. It accepts an array of polynomial drag coefficients (\texttt{damping[0]} $= B_1$, \texttt{damping[1]} $= B_2$, $\dots$). As with \texttt{armature}, actuator-level \texttt{damping} is scaled by $N^2$. + +\paragraph{Cogging Torque.} The magnetic torque ripple of Eq.~\eqref{eq:cogging} is modeled as a periodic bias added to the actuator force, where $\theta$ is the \texttt{actuator\_length}, i.e.\ the transmission-transformed joint angle. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atCOGA{} & $A$ & N$\cdot$m \\ +\atCOGP{} & $N_p$ & dimensionless \\ +\atCOGPH{} & $\phi$ & radian \\ +\bottomrule +\end{tabular} +\caption{Cogging torque attributes.} +\label{tab:cogging_impl} +\end{table} + +\paragraph{Mapping to Isaac Lab.} +Isaac Lab~\cite{isaaclab2025} implements the stateless DC motor model. Table~\ref{tab:mapping} maps its attributes to the constants defined in this document and to the \texttt{dcmotor} attributes. + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & MuJoCo & Isaac Lab \\ +\midrule +$\tau_0$ & \atSTALL{} & \texttt{saturation\_effort} \\ +$\omega_0$ & \atNLS{} & \texttt{velocity\_limit} \\ +$\tau_{\max}$ & \atTMAX{} & \texttt{effort\_limit} \\ +\bottomrule +\end{tabular} +\caption{Mapping of attributes to Isaac Lab.} +\label{tab:mapping} +\end{table} + +\noindent Isaac Lab does not expose electrical parameters. To reproduce the same torque-speed envelope, set \atVM{} to any positive value (e.g.,~\texttt{1}) and \texttt{ctrlrange} to $\pm$\,that value (e.g., \texttt{"-1 1"}). + +\paragraph{Gearbox Efficiency.} Gearbox efficiency $\eta$ is not a separate attribute. To account for transmission losses, reduce the motor constant: $K \rightarrow \eta K$. This correctly reduces forward torque transmission. + +\paragraph{Computed parameters.} +Several derived quantities that appear on datasheets can be computed and used to cross-check the parameterization. The torque-speed gradient $\partial\omega/\partial\tau = -R/K^2$ gives the slope of the torque-speed line (Table~\ref{tab:electromech_constants}). The mechanical time constant $t_m = R\,J/K^2$ is the time for the motor to reach 63\% of its no-load speed under a voltage step, where $J$ is the rotor inertia (\texttt{armature}). The nominal (continuous) torque is $\tau_n = K \cdot i_{\max}$. The no-load current $i_0$ can be computed from Eq.~\eqref{eq:noload} given known friction parameters. See Table~\ref{tab:datasheet}. + +\paragraph{Not modeled:} +Nonlinear torque constant $K_t(i)$. Separate $K_t$ and $K_e$ values are accepted via \atKt{} and \atKe{} but collapsed to a single effective $K = \sqrt{K_t K_e}$. + + +% --------------------------------------------------------------------------- +% Stateful Current +% --------------------------------------------------------------------------- +\subsection{Stateful Current} +\label{sec:current_impl} + +A winding current state variable governed by Eq.~\eqref{eq:inductance} is added if the electrical time constant $t_e > 0$ (derived from inductance $L > 0$ or specified directly). When enabled, the state is integrated by \texttt{mjDYN\_DCMOTOR}, and the gain switches from $K/R$ (stateless) to $K$ (stateful). +The time constant $t_e$ can be determined by either: + +\begin{itemize}[itemsep=0pt, parsep=0pt, topsep=2pt] + \item \atTE{} + \item \atL{} \emph{and} \atR{} (via $t_e = L/R$) +\end{itemize} + +\paragraph{Current rate limiting.} When the sub-attribute \atCRATE{} is set ($(di/dt)_{\max} > 0$) and the current state is enabled ($t_e > 0$), the rate of change of current is clamped: +\begin{equation*} + \frac{di}{dt} \leftarrow \text{clip}\!\left(\frac{di}{dt},\; \pm(di/dt)_{\max}\right) +\end{equation*} +This limits the torque ramp rate to $K \cdot (di/dt)_{\max}$ (N$\cdot$m/s) without requiring any additional state variables, since the current $i$ is already an activation variable and we are simply clamping its rate of change. This attribute has no effect when $t_e = 0$ (stateless current). + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atL{} & $L$ & Henry \\ +\atTE{} & $t_e\!=\!L/R$ & second \\ +\atCRATE{} & $(di{/}dt)_{\max}$ & Ampere/second \\ +\bottomrule +\end{tabular} +\caption{Stateful current attributes.} +\end{table} + +% --------------------------------------------------------------------------- +% Temperature +% --------------------------------------------------------------------------- +\subsection{Temperature} +\label{sec:temperature_impl} + +A winding temperature state governed by the lumped ODE~\eqref{eq:thermal_ode} is added if any of the thermal attributes ($R_T, C, t_T$) are specified. The state $T$ is the temperature rise above ambient ($T = T_{\text{winding}} - T_a$), so the absolute temperature is $T + T_a$. Temperature modifies the winding resistance via Eq.~\eqref{eq:resistance_temperature}, which feeds back into the motor equation: higher temperature increases resistance, leading to reduced current for a given voltage. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atRT{} & $R_T$ & K/W \\ +\atTC{} & $C$ & J/K \\ +\atTT{} & $t_T\!=\!R_T C$ & s \\ +\atALPHA{} & $\alpha$ & 1/K \\ +\atTREF{} & $T_0$ & \textdegree C \\ +\atTAMB{} & $T_a$ & \textdegree C \\ +\bottomrule +\end{tabular} +\caption{Thermal model attributes.} +\end{table} + +\noindent The time constant $t_T$ can be determined by either: +\begin{itemize}[itemsep=0pt, parsep=0pt, topsep=2pt] + \item \atTT{} + \item \atRT{} \emph{and} \atTC{} +\end{itemize} + +\paragraph{Not modeled:} +Iron losses (\S\ref{sec:thermal_losses}), magnet flux derating (\S\ref{sec:magnet_derating}), and thermal current derating (\S\ref{sec:thermal_derating}). Only copper losses ($i^2 R$) drive the thermal model; $K$ is treated as temperature-independent. + +% --------------------------------------------------------------------------- +% Stateful Friction +% --------------------------------------------------------------------------- +\subsection{Stateful Friction} +\label{sec:friction_impl} + +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. +\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: +\begin{equation} + z_{k+1} = e^{a \Delta t} z_k + + \frac{b(e^{a \Delta t} - 1)}{a} \, \omega + \label{eq:zoh} +\end{equation} +reducing to $z_{k+1} = z_k + b\omega\Delta t$ in the limit $a \to 0$. This integration is unconditionally stable for any $\Delta t$. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +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 \\ +\bottomrule +\end{tabular} +\caption{LuGre friction attributes.} +\end{table} + +\noindent\textbf{Not modeled:} +Velocity-dependent bristle damping $\sigma_1(\omega)$ (\S\ref{sec:vel_damping}), a constant $\sigma_1$ is used. The Stribeck exponent is not exposed and fixed at $\gamma = 2$. + +\newpage +% --------------------------------------------------------------------------- +% PID Controller +% --------------------------------------------------------------------------- +\subsection{PID Controller} +\label{sec:controller} + +Many actuators embed an on-board controller computing drive voltage from position or velocity commands. To model such actuators, \texttt{dcmotor} supports an optional controller layer upstream of the motor physics. Two attributes control this behavior: + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Type & Description \\ +\midrule +\texttt{input} & keyword & \texttt{voltage}, \texttt{position}, \texttt{velocity} \\ +\texttt{controller} & vector & Gains (mode-dependent) \\ +\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. 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$. + +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 +\begin{tikzpicture}[ + block/.style={draw, rounded corners=2pt, minimum height=1.6em, + font=\scriptsize, fill=blue!5}, + mode/.style={font=\scriptsize, text=blue!70!black}, + arr/.style={->, thick, >=stealth}, + every node/.style={inner sep=2pt}, +] +% ctrl input +\node[font=\small] (ctrl) at (0, 3.5) {Input $u = {}$\texttt{ctrl}}; + +% Mode selector box +\node[block, minimum width=5.5cm, minimum height=6.5em, align=center] + (sel) at (0, 1.8) {}; +\node[font=\footnotesize\bfseries, anchor=north] at (0, 2.7) + {Controller mode}; +\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) +\end{aligned}$}; + +% arrow ctrl to mode +\draw[arr] (ctrl.south) -- (sel.north); + +% Motor block +\node[block, font=\footnotesize\bfseries, minimum width=5.5cm, minimum height=2.2em, align=center] + (motor) at (0, -0.8) {DC Motor physics}; + +% single arrow with v label +\draw[arr] (sel.south) -- (motor.north) + node[midway, fill=white, font=\small, inner sep=2pt] {Voltage $v$}; + +% output +\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.} +\label{fig:controller_pipeline} +\end{figure} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Position Mode} +\label{sec:position_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. + +\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. + +\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 + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Setpoint Slew Rate} +\label{sec:setpoint_slew} + +The user command $u = {}$\texttt{ctrl} can change discontinuously between timesteps. When \atSLEW{} is set ($s > 0$), the effective setpoint is rate-limited: +\begin{equation*} + u \leftarrow \text{clip}(u, \; u_{\text{prev}} \pm s \cdot \Delta t) +\end{equation*} +where $u_{\text{prev}}$ is the previous effective setpoint and $\Delta t$ is the timestep. This smoothly ramps the reference trajectory instead of allowing instantaneous jumps. + +\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. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Anti-windup} +\label{sec:anti_windup} + +When $k_i > 0$, the integrator state $x_I$ provides steady-state error correction. However, sustained saturation or large setpoint changes can cause $x_I$ to grow excessively, leading to overshoot (integral windup). To prevent this, when \atIMAXINT{} is set ($I_{\max} > 0$), the state is bounded each step: +\begin{equation*} + x_I \leftarrow \text{clip}(x_I, \pm I_{\max}) +\end{equation*} +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: +\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. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atKP{} & $k_p$ & mode-dependent \\ +\atKI{} & $k_i$ & mode-dependent \\ +\atKD{} & $k_d$ & V$\cdot$s/rad \\ +\atSLEW{} & $s$ & ctrl-units/s \\ +\atIMAXINT{} & $I_{\max}$ & mode-dependent \\ +\atVMAX{} & $v_{\max}$ & Volt \\ +\bottomrule +\end{tabular} +\caption{Controller attributes.} +\label{tab:controller_attrs} +\end{table} + +% --------------------------------------------------------------------------- +% Low-Level Semantics +% --------------------------------------------------------------------------- +\subsection{Low-Level Semantics} +\label{sec:array_semantics} + +The \texttt{dcmotor} actuator uses the enum value types \texttt{mjGAIN\_DCMOTOR}, \texttt{mjDYN\_DCMOTOR}, \texttt{mjBIAS\_DCMOTOR}, and populates the rows of several \texttt{mjModel} arrays (all \texttt{actuator\_*}), as follows: + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}llll@{}} +\toprule +Array & Index & Symbol & Description \\ +\midrule +\texttt{gainprm} & 0 & $R$ & Resistance ($\Omega$) \\ +& 1 & $K$ & Motor constant (N$\cdot$m/A) \\ +& 2 & $\alpha$ & Resistance coeff.\ ($\text{K}^{-1}$) \\ +& 3 & $T_0$ & Reference temperature (\textdegree C) \\ +& 4 & $k_p$ & Controller proportional gain \\ +& 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$) \\ +\midrule +\texttt{dynprm} & 0 & $t_e$ & Electrical time constant (s) \\ +& 1 & $(di{/}dt)_{\max}$ & Current rate limit (A/s) \\ +& 2 & $R_T$ & Thermal resistance ($\text{K}$/W) \\ +& 3 & $C$ & Thermal capacitance (J/$\text{K}$) \\ +& 4 & $T_a$ & Ambient temperature (\textdegree C) \\ +& 5 & $\sigma_0$ & LuGre bristle stiffness \\ +& 6 & $\sigma_1$ & LuGre bristle damping \\ +& 7 & $s$ & Controller slew rate \\ +& 8 & $I_{\max}$ & Integral limit (anti-windup) \\ +\midrule +\texttt{biasprm} & 0 & $A$ & Cogging amplitude (N$\cdot$m) \\ +& 1 & $N_p$ & Cogging periodicity \\ +& 2 & $\phi$ & Cogging phase (rad) \\ +& 3 & $\tau_c$ & LuGre Coulomb fric. (N$\cdot$m) \\ +& 4 & $\tau_s$ & LuGre static fric. (N$\cdot$m) \\ +& 5 & $\omega_s$ & Stribeck velocity (rad/s) \\ +\midrule +\texttt{forcerange} & 0 & $-\tau_{\max}$ & Minimum torque (N$\cdot$m) \\ +& 1 & $\tau_{\max}$ & Maximum torque (N$\cdot$m) \\ +\midrule +\texttt{damping} & 0 & $B_1 (+\sigma_2)$ & Linear (+ LuGre viscous) \\ +& 1 & $B_2$ & Quadratic \\ +& 2 & $B_3$ & Cubic \\ +\midrule +\texttt{armature} & 0 & $J_r$ & Actuator armature \\ +\midrule +\texttt{gear} & 0 & $N$ & Gear ratio \\ +\bottomrule +\end{tabular} +\caption{\texttt{mjModel} array semantics for the \texttt{dcmotor} actuator.} +\label{tab:array_semantics} +\end{table} + +\paragraph{Runtime mutability.} +Most \texttt{mjModel} parameters listed above may be freely modified at runtime for system identification or gain tuning. However, five parameters control the \emph{number} of activation states, which is determined at compile time and cannot change during simulation. Toggling any of the following parameters between zero and positive after compilation is an error: +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}llcl@{}} +\toprule +Parameter & Storage & State & Semantics \\ +\midrule +$s$ & \texttt{dynprm[7]} & $u_{\text{prev}}$ & previous control \\ +$k_i$ & \texttt{gainprm[5]} & $x_I$ & controller integral \\ +$R_T, C$ & \texttt{dynprm[2,3]} & $T$ & temperature rise \\ +$\sigma_0$ & \texttt{dynprm[5]} & $z$ & bristle deflection \\ +$t_e$ & \texttt{dynprm[0]} & $i$ & winding current \\ +\bottomrule +\end{tabular} +\caption{Compile-time state switches, allocated in \texttt{act} in the order shown. Do not toggle between zero and positive at runtime.} +\end{table} + + +\onecolumn +%============================================================================= +% DATASHEET MAPPING +%============================================================================= +\section{Datasheet Mapping} +\label{sec:datasheet} + +Table~\ref{tab:datasheet} maps commercial motor datasheet specifications to attributes. The left column shows the datasheet entry as typically labeled by motor manufacturers; the right column shows the corresponding \texttt{dcmotor} MJCF attribute, when one exists. Derived quantities that are not direct attributes (gradient, mechanical time constant) are included for completeness. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lllll@{}} +\toprule +Specification & Symbol & Formula / Note & Datasheet Symbol & Attribute \\ +\midrule +Resistance & $R$ & Terminal resistance & R, Ra & \atR{} \\ +Torque Constant & $K_t$ & $\tau = K_t i$ & kt, km & \atKt{} \\ +Back-EMF Constant & $K_e$ & $v_{\text{back}} = K_e \omega$ & ke & \atKe{} \\ +Speed Constant & $K_v$ & $K_v = 1/K_e$ & kn, kv & $1/$\atKe{} \\ +Nominal Voltage & $v_n$ & Rated voltage (e.g., 24V) & Un, VDC & \atVM{} \\ +No-load Speed & $\omega_0$ & $\omega_0 \approx v_n / K$ & n0 & \atNLS{} \\ +Stall Torque & $\tau_0$ & $\tau_0 = K v_n / R$ & MH, Ts & \atSTALL{} \\ +Stall Current & $i_s$ & Max.\ possible: $i_s = v_n / R$ & IA & \\ +Nominal Current & $i_{\max}$ & Thermal limit (continuous) & Ic, IN & \atIMAX{} \\ +Peak Current & $i_{\text{peak}}$ & Short-term limit & Ipk & \\ +\midrule +Coulomb Friction & $\tau_c$ & Dry friction opposing motion & $T_f$ & \texttt{frictionloss} (joint)\\ +Viscous Friction & $B$ & Drag $\propto \omega$ & $C_v$ & \texttt{damping} \\ +Rotor Inertia & $J_r$ & Reflected: $J_{\text{eff}} = J_r N^2$ & J, Jm & \texttt{armature} \\ +Gear Ratio & $N$ & Reduction ratio & $N$, $i$ & \texttt{gear} \\ +Gearbox Efficiency & $\eta$ & Fold into $K$: use $\eta K$ & $\eta$ & \\ +Cogging Amplitude & $A$ & Peak cogging torque & --- & \atCOGA{} \\ +Cogging Periodicity & $N_p$ & Poles $\times$ slots/pole & --- & \atCOGP{} \\ +\midrule +Nominal Torque & $\tau_n$ & $\tau_n = K \cdot i_{\max}$ & $M_N$, $T_c$ & \\ +No-load Current & $i_0$ & Friction: Eq.~\eqref{eq:noload} & $I_0$ & \\ +Gradient & $\partial\omega/\partial\tau$ & $-R / K^2$ & $\Delta n / \Delta M$ & \\ +Mech.\ Time Const. & $t_m$ & $t_m = R\,J / K^2$ & $\tau_m$ & \\ +\midrule +Inductance & $L$ & Terminal inductance & L & \atL{} \\ +Elec.\ Time Const. & $t_e$ & $t_e = L / R$ & $\tau_e$ & \atTE{} \\ +\midrule +Thermal Resistance & $R_T$ & Winding-to-ambient & Rth & \atRT{} \\ +Thermal Capacitance & $C$ & $C = t_T / R_T$ & $C_{\text{th}}$ & \atTC{} \\ +Thermal Time Const. & $t_T$ & $t_T = R_T C$ & $\tau_{\text{th}}$ & \atTT{} \\ +Ref.\ Temperature & $T_0$ & Temperature at which $R$ is specified & $T_{\text{ref}}$ & \atTREF{} \\ +Ambient Temperature & $T_a$ & Operating environment & --- & \atTAMB{} \\ +Max.\ Winding Temp. & $T_{\max}$ & Absolute limit & $T_{\max}$ & \\ +Res.\ Temp.\ Coeff. & $\alpha$ & $\approx 0.0039\, \text{K}^{-1}$ (copper) & $\alpha_{\text{Cu}}$ & \atALPHA{} \\ +\bottomrule +\end{tabular} +\caption{Datasheet parameters and their relation to model constants. Groups: electrical, mechanical, derived, inductance, thermal.} +\label{tab:datasheet} +\end{table} + +\small +\bibliographystyle{ieeetr} +\bibliography{refs} + +\end{document} diff --git a/doc/dcmotor/refs.bib b/doc/dcmotor/refs.bib new file mode 100644 index 00000000..29708bc7 --- /dev/null +++ b/doc/dcmotor/refs.bib @@ -0,0 +1,82 @@ +@article{dewit95, + author = {Canudas de Wit, C. and Olsson, H. and {\AA}str{\"o}m, K. J. and Lischinsky, P.}, + title = {{A New Model for Control of Systems with Friction}}, + journal = {IEEE Transactions on Automatic Control}, + volume = {40}, + number = {3}, + pages = {419--425}, + year = {1995}, + month = mar, +} + +@article{lugre_revisited, + author = {{\AA}str{\"o}m, K. J. and Canudas de Wit, C.}, + title = {{Revisiting the LuGre Friction Model}}, + journal = {IEEE Control Systems Magazine}, + volume = {28}, + number = {6}, + pages = {101--114}, + year = {2008}, + month = dec, +} + +@techreport{dahl68, + author = {Dahl, P.}, + title = {{A Solid Friction Model}}, + institution = {The Aerospace Corporation}, + address = {El Segundo, CA}, + number = {TOR-0158(3107-18)-1}, + year = {1968}, +} + +@book{hughes2019, + author = {Hughes, Austin and Drury, Bill}, + title = {{Electric Motors and Drives: Fundamentals, Types and Applications}}, + edition = {5th}, + publisher = {Newnes}, + year = {2019}, +} + +@book{tedrake2024, + author = {Tedrake, Russ}, + title = {{Underactuated Robotics: Algorithms for Walking, Running, + Swimming, Flying, and Manipulation}}, + publisher = {MIT}, + year = {2024}, + note = {Course notes for MIT 6.832, \url{https://underactuated.mit.edu}}, +} + +@article{isaaclab2025, + author = {Mittal, Mayank and Yu, Calvin and Yu, Qinxi and Liu, Jingzhou + and Rudin, Nikita and Hoeller, David and Yuan, Jia Lin + and Singh, Ritvik and Guo, Yunrong and Mazhar, Hammad + and Mandlekar, Ajay and Babich, Buck and State, Gavriel + and Hutter, Marco and Garg, Animesh}, + title = {{Isaac Lab: A Unified and Modular Framework for Robot Learning}}, + journal = {arXiv preprint arXiv:2502.11048}, + year = {2025}, +} + +@article{stribeck1902, + author = {Stribeck, R.}, + title = {{Die wesentlichen Eigenschaften der Gleit- und Rollenlager}}, + journal = {Zeitschrift des Vereines Deutscher Ingenieure}, + volume = {46}, + pages = {1341--1348, 1432--1438, 1463--1470}, + year = {1902}, +} + +@misc{maxon_formulas, + author = {{Maxon Motor AG}}, + title = {{Key Information on Maxon DC Motors and Maxon EC Motors}}, + howpublished = {\url{https://www.maxongroup.com}}, + year = {2024}, + note = {{Maxon} Academy Technical Notes}, +} + +@misc{simscape_dcmotor, + author = {{MathWorks}}, + title = {{DC Motor --- Simscape Electrical Block Reference}}, + howpublished = {\url{https://www.mathworks.com/help/sps/ref/dcmotor.html}}, + year = {2024}, +} diff --git a/doc/includes/references.h b/doc/includes/references.h index bc8a548d..fd855a1c 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -635,19 +635,22 @@ typedef enum mjtDyn_ { // type of actuator dynamics mjDYN_INTEGRATOR, // integrator: da/dt = u mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration - mjDYN_MUSCLE, // piece-wise linear filter with two time constants + mjDYN_MUSCLE, // piecewise linear filter with two time constants + mjDYN_DCMOTOR, // DC motor electrical dynamics mjDYN_USER // user-defined dynamics type } mjtDyn; typedef enum mjtGain_ { // type of actuator gain mjGAIN_FIXED = 0, // fixed gain mjGAIN_AFFINE, // const + kp*length + kv*velocity mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() + mjGAIN_DCMOTOR, // DC motor gain: K or K/R mjGAIN_USER // user-defined gain type } mjtGain; typedef enum mjtBias_ { // type of actuator bias mjBIAS_NONE = 0, // no bias mjBIAS_AFFINE, // const + kp*length + kv*velocity mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias() + mjBIAS_DCMOTOR, // DC motor bias: back-EMF, cogging, LuGre friction mjBIAS_USER // user-defined bias type } mjtBias; typedef enum mjtObj_ { // type of MujoCo object @@ -3659,6 +3662,10 @@ const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double t double range[2], double force, double scale, double lmin, 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); mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 2f9cdd35..49cfee0b 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -244,7 +244,8 @@ typedef enum mjtDyn_ { // type of actuator dynamics mjDYN_INTEGRATOR, // integrator: da/dt = u mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration - mjDYN_MUSCLE, // piece-wise linear filter with two time constants + mjDYN_MUSCLE, // piecewise linear filter with two time constants + mjDYN_DCMOTOR, // DC motor electrical dynamics mjDYN_USER // user-defined dynamics type } mjtDyn; @@ -253,6 +254,7 @@ typedef enum mjtGain_ { // type of actuator gain mjGAIN_FIXED = 0, // fixed gain mjGAIN_AFFINE, // const + kp*length + kv*velocity mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() + mjGAIN_DCMOTOR, // DC motor gain: K or K/R mjGAIN_USER // user-defined gain type } mjtGain; @@ -261,6 +263,7 @@ typedef enum mjtBias_ { // type of actuator bias mjBIAS_NONE = 0, // no bias mjBIAS_AFFINE, // const + kp*length + kv*velocity mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias() + mjBIAS_DCMOTOR, // DC motor bias: back-EMF, cogging, LuGre friction mjBIAS_USER // user-defined bias type } mjtBias; diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 362f16e9..ba496360 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1726,6 +1726,12 @@ MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], do // Set actuator to active adhesion; return error if any. MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +// Set actuator to DC motor; return error if any. +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); + //---------------------------------- Assets -------------------------------------------------------- diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index d4dcdf34..bc28f78d 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -263,7 +263,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDYN_FILTER', 2), ('mjDYN_FILTEREXACT', 3), ('mjDYN_MUSCLE', 4), - ('mjDYN_USER', 5), + ('mjDYN_DCMOTOR', 5), + ('mjDYN_USER', 6), ]), )), ('mjtGain', @@ -274,7 +275,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGAIN_FIXED', 0), ('mjGAIN_AFFINE', 1), ('mjGAIN_MUSCLE', 2), - ('mjGAIN_USER', 3), + ('mjGAIN_DCMOTOR', 3), + ('mjGAIN_USER', 4), ]), )), ('mjtBias', @@ -285,7 +287,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjBIAS_NONE', 0), ('mjBIAS_AFFINE', 1), ('mjBIAS_MUSCLE', 2), - ('mjBIAS_USER', 3), + ('mjBIAS_DCMOTOR', 3), + ('mjBIAS_USER', 4), ]), )), ('mjtObj', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index ff1bcf12..0ab6df20 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -10786,6 +10786,86 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Set actuator to active adhesion; return error if any.', )), + ('mjs_setToDCMotor', + FunctionDecl( + name='mjs_setToDCMotor', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='motorconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='resistance', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='nominal', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='saturation', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(4,), + ), + ), + FunctionParameterDecl( + name='inductance', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='cogging', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='controller', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(5,), + ), + ), + FunctionParameterDecl( + name='thermal', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(6,), + ), + ), + FunctionParameterDecl( + name='lugre', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(6,), + ), + ), + FunctionParameterDecl( + name='input_mode', + type=ValueType(name='int'), + ), + ), + doc='Set actuator to DC motor; return error if any.', + )), ('mjs_addMesh', FunctionDecl( name='mjs_addMesh', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index e3bfc7ef..28af8af1 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1303,6 +1303,31 @@ PYBIND11_MODULE(_specs, m) { } }, py::arg("gain")); + mjsActuator.def( + "set_to_dcmotor", + [](raw::MjsActuator* self, std::array motorconst, + double resistance, + 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::string err = mjs_setToDCMotor( + self, motorconst.data(), resistance, nominal.data(), + saturation.data(), inductance.data(), cogging.data(), + controller.data(), thermal.data(), lugre.data(), input_mode); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + 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("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("thermal") = std::array{0, 0, 0, 0, 0, 0}, + py::arg("lugre") = std::array{0, 0, 0, 0, 0, 0}, + py::arg("input_mode") = 0); // ============================= MJSTENDONPATH =============================== // helper struct for tendon path indexing diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index f1c99d82..a82646dd 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1557,6 +1557,13 @@ class SpecsTest(absltest.TestCase): self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + actuator.set_to_dcmotor(motorconst=[0.05, 0.05], resistance=2.0) + self.assertEqual(actuator.gainprm[0], 2.0) + self.assertEqual(actuator.gainprm[1], 0.05) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_DCMOTOR) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_DCMOTOR) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_DCMOTOR) + def test_bad_contact_sensor(self): test_cases = [ dict( diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 3f9b467a..00267a3e 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1107,6 +1107,17 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2]; } + // DC motor bias (back-EMF) + else if (m->actuator_biastype[i] == mjBIAS_DCMOTOR) { + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + if (dynprm[0] <= 0) { + mjtNum R = mju_max(mjMINVAL, gainprm[0]); + mjtNum K = gainprm[1]; + bias_vel -= K * K / R; + } + } + // affine gain if (m->actuator_gaintype[i] == mjGAIN_AFFINE) { // extract bias info: prm = [const, kp, kv] @@ -1122,6 +1133,28 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { m->actuator_gainprm + mjNGAIN*i); } + // DC motor controller damping and LuGre micro-damping + else if (m->actuator_gaintype[i] == mjGAIN_DCMOTOR) { + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + int input_mode = (int)gainprm[8]; + if (input_mode > 0) { + mjtNum R = gainprm[0]; + mjtNum K = gainprm[1]; + mjtNum gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R); + mjtNum kp = gainprm[4]; + mjtNum kd = gainprm[6]; + bias_vel -= gain * (input_mode == 1 ? kd : kp); + } + + // LuGre: force includes -sigma1*z_dot, z_dot = a*z + v + // d(sigma1*z_dot)/dv = sigma1*(da/dv*z + 1), ignoring higher-order da/dv*z + mjtNum sigma1 = dynprm[6]; + if (sigma1 > 0) { + bias_vel -= sigma1; + } + } + // force = gain .* [ctrl/act] if (gain_vel != 0) { if (m->actuator_dyntype[i] == mjDYN_NONE) { diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b28c342f..e8e08f00 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -257,6 +257,36 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { } +// helper for DC motor: computes control voltage from PID state +static mjtNum dcmotorVoltage(mjtNum ctrl, mjtNum length, mjtNum velocity, + mjtNum x_I, const mjtNum* gainprm) { + int input_mode = (int)gainprm[8]; + mjtNum Vmax = gainprm[7]; + mjtNum voltage; + + // get voltage + if (input_mode > 0) { + mjtNum kp = gainprm[4]; // proportional gain + mjtNum ki = gainprm[5]; // integral gain + mjtNum kd = gainprm[6]; // derivative gain + + 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; + } + + // clip voltage + if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax); + + return voltage; +} + // clamp vector to range static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n, @@ -275,7 +305,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; int nv = m->nv, nu = m->nu, ntendon = m->ntendon; mjtNum gain, bias, tau; - mjtNum *prm, *force = d->actuator_force; + mjtNum *force = d->actuator_force; // clear actuator_force mju_zero(force, nu); @@ -327,37 +357,136 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // zero act_dot for actuator plugins - if (m->actuator_actnum[i]) { - mju_zero(d->act_dot + act_first, m->actuator_actnum[i]); + int actnum = m->actuator_actnum[i]; + if (actnum) { + mju_zero(d->act_dot + act_first, actnum); } // extract info - prm = m->actuator_dynprm + i*mjNDYN; + const mjtNum* dynprm = m->actuator_dynprm + i*mjNDYN; + mjtDyn dyntype = m->actuator_dyntype[i]; // index into the last element in act. For most actuators it's also the - // first element, but actuator plugins might store their own state in act. - int act_last = act_first + m->actuator_actnum[i] - 1; + // first element, but actuator plugins might store their own state in act + int act_last = act_first + actnum - 1; // compute act_dot according to dynamics type - switch ((mjtDyn) m->actuator_dyntype[i]) { + switch (dyntype) { case mjDYN_INTEGRATOR: // simple integrator d->act_dot[act_last] = ctrl[i]; break; - case mjDYN_FILTER: // linear filter: prm = tau + case mjDYN_FILTER: // linear filter: dynprm = tau case mjDYN_FILTEREXACT: - tau = mju_max(mjMINVAL, prm[0]); + tau = mju_max(mjMINVAL, dynprm[0]); d->act_dot[act_last] = (ctrl[i] - d->act[act_last]) / tau; break; - case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact) - d->act_dot[act_last] = mju_muscleDynamics( - ctrl[i], d->act[act_last], prm); + case mjDYN_MUSCLE: // muscle model: dynprm = (tau_act, tau_deact) + d->act_dot[act_last] = mju_muscleDynamics(ctrl[i], d->act[act_last], dynprm); break; + case mjDYN_DCMOTOR: { // DC motor: up to 5 optional states + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + + // verify allocated state size matches parameters; SHOULD NOT OCCUR + if (mj_dcmotorSlots(dynprm, gainprm).num_slots != actnum) { + mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i); + } + + int adr = act_first; + mjtNum velocity = d->actuator_velocity[i]; + mjtNum R = gainprm[0]; // resistance + mjtNum K = gainprm[1]; // motor constant + mjtNum ki = gainprm[5]; // integral gain + mjtNum te = dynprm[0]; // electrical time constant + + // slot order: slew, integral, temperature, bristle, current + + // 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[i], u_prev - slew, u_prev + slew); + d->act_dot[adr] = (u_eff - u_prev) / m->opt.timestep; + ctrl[i] = u_eff; + adr++; + } + + // controller state: integral state + 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[i]; // default raw accumulator for voltage and velocity modes + + // position mode + if (input_mode == 1) { + act_dot = ctrl[i] - d->actuator_length[i]; + } + + // clamp act_dot based on integral state + if (Imax > 0) { + if (x_I >= Imax) { + act_dot = mju_min(act_dot, 0); + } else if (x_I <= -Imax) { + act_dot = mju_max(act_dot, 0); + } + } + d->act_dot[adr] = act_dot; + adr++; + } + + // compute physical voltage to feed into current and temperature equations + mjtNum V = dcmotorVoltage(ctrl[i], d->actuator_length[i], velocity, x_I, gainprm); + + // temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient + mjtNum RT = dynprm[2]; // thermal resistance + if (RT > 0) { + mjtNum C = dynprm[3]; // thermal capacitance + mjtNum Ta = dynprm[4]; // ambient temperature + mjtNum alpha = gainprm[2]; // temperature coefficient + mjtNum T0 = gainprm[3]; // reference temperature + mjtNum T = d->act[adr]; // temperature rise above ambient + R *= 1 + alpha * (T + Ta - T0); + + // get current: from act_last if stateful, from (V - K*omega)/R if stateless + mjtNum current = (te > 0) ? d->act[act_last] : (V - K * velocity) / R; + d->act_dot[adr] = (R*current*current - T / RT) / C; + adr++; + } + + // LuGre bristle state: dz/dt = v - sigma0 * |v| / g(v) * z + mjtNum sigma0 = dynprm[5]; // bristle stiffness + if (sigma0 > 0) { + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; + mjtNum F_C = biasprm[3]; // Coulomb friction + mjtNum F_S = biasprm[4]; // static friction + mjtNum v_S = biasprm[5]; // Stribeck velocity + mjtNum z = d->act[adr]; // bristle state + mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S); + mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g); + d->act_dot[adr] = a * z + velocity; + adr++; + } + + // current state: di/dt = (V/R - K/R*omega - i) / te + if (te > 0) { + mjtNum dimax = dynprm[1]; // current rate limit (di/dt)_max + mjtNum i_dot = (V/R - K/R*velocity - d->act[act_last]) / te; + if (dimax > 0) { + i_dot = mju_clip(i_dot, -dimax, dimax); + } + d->act_dot[act_last] = i_dot; + } + break; + } + default: // user dynamics if (mjcb_act_dyn) { - if (m->actuator_actnum[i] == 1) { + if (actnum == 1) { // scalar activation dynamics, get act_dot d->act_dot[act_last] = mjcb_act_dyn(m, d, i); } else { @@ -407,17 +536,20 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]]; } - // extract gain info - prm = m->actuator_gainprm + mjNGAIN*i; + // extract info + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + mjtGain gaintype = m->actuator_gaintype[i]; + int actnum = m->actuator_actnum[i]; // handle according to gain type - switch ((mjtGain) m->actuator_gaintype[i]) { + switch (gaintype) { case mjGAIN_FIXED: // fixed gain: prm = gain - gain = prm[0]; + gain = gainprm[0]; break; case mjGAIN_AFFINE: // affine: prm = [const, kp, kv] - gain = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i]; + gain = gainprm[0] + gainprm[1]*d->actuator_length[i] + gainprm[2]*d->actuator_velocity[i]; break; case mjGAIN_MUSCLE: // muscle gain @@ -425,9 +557,43 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { d->actuator_velocity[i], m->actuator_lengthrange+2*i, m->actuator_acc0[i], - prm); + gainprm); break; + case mjGAIN_DCMOTOR: { // DC motor: gain = K or K/R + mjtNum R = gainprm[0]; // resistance + mjtNum K = gainprm[1]; // motor constant + mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm); + + // verify allocated state size matches parameters; SHOULD NOT OCCUR + if (slots.num_slots != actnum) { + mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i); + } + + int adr = m->actuator_actadr[i]; + + // adjust R for temperature if enabled + if (slots.temperature >= 0) { + mjtNum T = d->act[adr + slots.temperature]; + mjtNum alpha = gainprm[2]; // temperature coefficient + mjtNum T0 = gainprm[3]; // reference temperature + mjtNum Ta = dynprm[4]; // ambient temperature + R *= 1 + alpha * (T + Ta - T0); + } + + // stateful current: gain = K, force = K * act[last] (generic path) + // stateless: gain = K/R, force = K/R * ctrl (condition below) + gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R); + + // controller: compute voltage, override ctrl[i] for force computation + if ((int)gainprm[8] > 0) { + mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0; + ctrl[i] = dcmotorVoltage(ctrl[i], d->actuator_length[i], + d->actuator_velocity[i], x_I, gainprm); + } + break; + } + default: // user gain if (mjcb_act_gain) { gain = mjcb_act_gain(m, d, i); @@ -437,11 +603,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // set force = gain .* [ctrl/act] - if (m->actuator_actadr[i] == -1) { + + // DC motor without current state: use ctrl even if other activations exist + int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0); + if (actnum == 0 || dcmotor_no_current) { force[i] = gain * ctrl[i]; } else { // use last activation variable associated with actuator i - int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1; + int act_adr = m->actuator_actadr[i] + actnum - 1; mjtNum act; if (m->actuator_actearly[i]) { @@ -453,25 +622,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // extract bias info - prm = m->actuator_biasprm + mjNBIAS*i; + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; + mjtBias biastype = m->actuator_biastype[i]; // handle according to bias type - switch ((mjtBias) m->actuator_biastype[i]) { + switch (biastype) { case mjBIAS_NONE: // none bias = 0.0; break; - case mjBIAS_AFFINE: // affine: prm = [const, kp, kv] - bias = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i]; + case mjBIAS_AFFINE: // affine: biasprm = [const, kp, kv] + bias = biasprm[0] + biasprm[1]*d->actuator_length[i] + biasprm[2]*d->actuator_velocity[i]; break; case mjBIAS_MUSCLE: // muscle passive force bias = mju_muscleBias(d->actuator_length[i], m->actuator_lengthrange+2*i, m->actuator_acc0[i], - prm); + biasprm); break; + case mjBIAS_DCMOTOR: { // DC motor: back-EMF only (current-limited) + bias = 0; + + // back-EMF (stateless only; for stateful current it's in the ODE) + mjtNum te = m->actuator_dynprm[mjNDYN*i]; // electrical time constant + if (te <= 0) { + mjtNum K = gainprm[1]; // motor constant + bias -= gain * K * d->actuator_velocity[i]; + } + break; + } + default: // user bias if (mjcb_act_bias) { bias = mjcb_act_bias(m, d, i); @@ -537,6 +719,41 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // clamp actuator_force clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL); + // add DC motor mechanical forces (not subject to current limits) + for (int i=0; i < nu; i++) { + if (m->actuator_biastype[i] != mjBIAS_DCMOTOR) { + continue; + } + if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) { + continue; + } + if (mj_actuatorDisabled(m, i) || m->actuator_plugin[i] >= 0) { + continue; + } + + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + + // cogging torque + mjtNum A = biasprm[0]; + if (A != 0) { + mjtNum Np = biasprm[1]; + mjtNum phi = biasprm[2]; + force[i] += A * mju_sin(Np*d->actuator_length[i] + phi); + } + + // LuGre friction + mjtNum sigma0 = dynprm[5]; + if (sigma0 > 0) { + mjtNum sigma1 = dynprm[6]; + mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, m->actuator_gainprm + mjNGAIN*i); + int adr = m->actuator_actadr[i] + slots.bristle; + mjtNum z = d->act[adr]; + mjtNum z_dot = d->act_dot[adr]; + force[i] -= sigma0 * z + sigma1 * z_dot; + } + } + // qfrc_actuator = moment' * force mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv, d->moment_rownnz, d->moment_rowadr, d->moment_colind); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a4827ddb..81c3b6ca 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -709,22 +709,69 @@ int mj_actuatorDisabled(const mjModel* m, int i) { mjtNum mj_nextActivation(const mjModel* m, const mjData* d, int actuator_id, int act_adr, mjtNum act_dot) { mjtNum act = d->act[act_adr]; + int dyntype = m->actuator_dyntype[actuator_id]; - if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { + if (dyntype == mjDYN_FILTEREXACT) { // exact filter integration // act_dot(0) = (ctrl-act(0)) / tau // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau)) // = act(0) + act_dot(0) * tau * (1 - exp(-h / tau)) mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id*mjNDYN]); act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau)); - } else { - // Euler integration + } else if (dyntype == mjDYN_DCMOTOR) { + const mjtNum* dynprm = m->actuator_dynprm + actuator_id * mjNDYN; + const mjtNum* gainprm = m->actuator_gainprm + actuator_id * mjNGAIN; + mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm); + + int offset = act_adr - m->actuator_actadr[actuator_id]; + + // current filter: exact integration + if (offset == slots.current) { + mjtNum te = mju_max(mjMINVAL, dynprm[0]); + act = act + act_dot * te * (1 - mju_exp(-m->opt.timestep / te)); + } + + // LuGre bristle: dz/dt = a*z + v where a = -sigma0*|v|/g(v) + else if (offset == slots.bristle) { + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*actuator_id; + mjtNum F_C = biasprm[3]; // Coulomb friction + mjtNum F_S = biasprm[4]; // static friction + mjtNum v_S = biasprm[5]; // Stribeck velocity + mjtNum sigma0 = dynprm[5]; // bristle stiffness + mjtNum velocity = d->actuator_velocity[actuator_id]; + mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S); + + // ZOH exact ZOH integration: z(h) = exp(ah)*z(0) + ((exp(ah)-1)/a)*v + mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g); // decay rate + mjtNum h = m->opt.timestep; + mjtNum exp_ah = mju_exp(a * h); // state transition + mjtNum int_h = mju_abs(a) > mjMINVAL ? (exp_ah - 1) / a : h; // input integral + act = exp_ah * act + int_h * velocity; + } + + // integral state: Euler integration with anti-windup clamp + else if (offset == slots.integral) { + act = act + act_dot * m->opt.timestep; + mjtNum Imax = dynprm[8]; + if (Imax > 0) { + act = mju_clip(act, -Imax, Imax); + } + } + + // temperature and slew: Euler integration + else { + act = act + act_dot * m->opt.timestep; + } + } + + // otherwise Euler integration + else { act = act + act_dot * m->opt.timestep; } - // clamp to actrange - if (m->actuator_actlimited[actuator_id]) { - mjtNum* actrange = m->actuator_actrange + 2*actuator_id; + // clamp to actrange unless DC motor + if (dyntype != mjDYN_DCMOTOR && m->actuator_actlimited[actuator_id]) { + const mjtNum* actrange = m->actuator_actrange + 2*actuator_id; act = mju_clip(act, actrange[0], actrange[1]); } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 0dd2ca97..65057a51 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -769,6 +769,26 @@ mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) { } +// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2) +mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S) { + mjtNum ratio = velocity / mju_max(mjMINVAL, v_S); + return F_C + (F_S - F_C) * mju_exp(-ratio*ratio); +} + + +// compute DC motor activation slot indices from parameter arrays +mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm) { + mjDCMotorSlots s = {-1, -1, -1, -1, -1, 0}; + if (dynprm[7] > 0) s.slew = s.num_slots++; // slew rate limiting + if (gainprm[5] > 0) s.integral = s.num_slots++; // PI integral + if (dynprm[2] > 0) s.temperature = s.num_slots++; // thermal model + if (dynprm[5] > 0) s.bristle = s.num_slots++; // LuGre bristle + if (dynprm[0] > 0) s.current = s.num_slots++; // current filter + + return s; +} + + //---------------------------------------- Base64 -------------------------------------------------- // decoding function for Base64 diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 756b3b8e..cac5bed5 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -50,6 +50,23 @@ MJAPI mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum ta // muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width) MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); +// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2) +mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S); + +// DC motor activation slot indices (-1 = slot not active) +typedef struct { + int slew; // slew rate state + int integral; // integral state + int temperature; // temperature state + int bristle; // LuGre bristle state + int current; // current state + int num_slots; // number of DC motor states +} mjDCMotorSlots; + +// compute activation slot indices for a DC motor actuator +// dynprm = actuator_dynprm row, gainprm = actuator_gainprm row +mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm); + // all 3 semi-axes of a geom MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type); diff --git a/src/user/user_api.cc b/src/user/user_api.cc index f5b569d9..2317cc3a 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -15,6 +15,7 @@ #include "user/user_api.h" #include +#include #include #include #include @@ -1120,6 +1121,166 @@ 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 Kt = motorconst[0]; // torque constant + double Ke = motorconst[1]; // back-EMF constant + double R = resistance; // electrical resistance + double vn = nominal[0]; // nominal voltage + double tau0 = nominal[1]; // stall torque + double omega0 = nominal[2]; // no-load speed + + // 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 + double B = actuator->damping[0]; + if (lugre[0] > 0) B += lugre[2]; + + if (B > 0 && R > 0) { + // R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0 + double disc = vn*vn - 4*R*B*omega0*omega0; + Ke = disc > 0 ? (vn + sqrt(disc)) / (2*omega0) : vn / omega0; + } else if (B > 0 && tau0 > 0) { + // R from nominal (tau0 = Ke*vn/R, so R = Ke*vn/tau0) + // substituting into omega0 = vn*Ke/(Ke^2 + R*B): + // omega0 = vn/(Ke + vn*B/tau0) => Ke = vn/omega0 - vn*B/tau0 + double Ke_exact = vn / omega0 - vn*B / tau0; + Ke = Ke_exact > 0 ? Ke_exact : vn / omega0; + } else { + // B = 0 or insufficient data for B-correction: omega0 = vn*Ke/Ke^2 = vn/Ke + Ke = vn / omega0; + } + } + + // resolve effective motor constant K from [Kt, Ke] + double K = (Kt > 0 && Ke > 0) ? sqrt(Kt * Ke) : + (Kt > 0) ? Kt : Ke; + + // derive R from nominal: tau0 = K*vn/R + if (R == 0 && vn > 0 && tau0 > 0 && K > 0) { + R = K * vn / tau0; + } + + if (K <= 0) return "DC motor: motor constant K must be positive"; + if (R <= 0) return "DC motor: resistance R must be positive"; + + // set types + actuator->dyntype = mjDYN_DCMOTOR; + actuator->gaintype = mjGAIN_DCMOTOR; + actuator->biastype = mjBIAS_DCMOTOR; + + // gainprm: [R, K, alpha, T0] + actuator->gainprm[0] = R; + actuator->gainprm[1] = K; + + // controller parameters: gainprm[4:6] for kp, ki, kd + actuator->gainprm[4] = controller[0]; // kp + actuator->gainprm[5] = controller[1]; // ki + actuator->gainprm[6] = controller[2]; // kd + + // controller parameters: dynprm[7,8] for slewmax, Imax + actuator->dynprm[7] = controller[3]; // slewmax + actuator->dynprm[8] = controller[4]; // Imax + + // saturation: [tau_max, i_max, (di/dt)_max, v_max] + if (saturation[2] > 0) { + actuator->dynprm[1] = saturation[2]; // (di/dt)_max + } + if (saturation[3] > 0) { + actuator->gainprm[7] = saturation[3]; // v_max + } + + // saturation -> forcerange + if (saturation[0] > 0 || saturation[1] > 0) { + double tau_max = saturation[0]; + if (tau_max == 0 && saturation[1] > 0) { + tau_max = K * saturation[1]; // tau_max = K * i_max + } + actuator->forcerange[0] = -tau_max; + actuator->forcerange[1] = tau_max; + actuator->forcelimited = 1; + } + + // cogging: [amplitude, periodicity, phase] -> biasprm[0:3] + actuator->biasprm[0] = cogging[0]; // amplitude + actuator->biasprm[1] = cogging[1]; // periodicity + actuator->biasprm[2] = cogging[2]; // phase + + // count activation variables: slot order is slew, integral, temperature, bristle, current + int actdim = 0; + + // inductance: [L, te] + if (inductance[0] < 0) return "DC motor: inductance must be non-negative"; + if (inductance[1] < 0) return "DC motor: electrical time constant must be non-negative"; + double te = inductance[0] > 0 ? inductance[0] / R : inductance[1]; + actuator->dynprm[0] = te; + if (te > 0) { + actdim++; + } + + // controller states: slew rate limiting + if (controller[3] > 0) { // slewmax + actdim++; + } + + // controller states: integral + if (controller[1] > 0) { // ki + actdim++; + } + + // thermal -> temperature activation + if (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0) { + double RT = thermal[0]; // thermal resistance + double C = thermal[1]; // thermal capacitance + double tth = thermal[2]; // thermal time constant + double alpha = thermal[3]; // temperature coefficient + double T0 = thermal[4]; // reference temperature + double Ta = thermal[5]; // ambient temperature + + if (tth > 0 && RT > 0 && C == 0) { + C = tth / RT; + } else if (tth > 0 && C > 0 && RT == 0) { + RT = tth / C; + } else if (tth == 0 && RT > 0 && C > 0) { + tth = RT * C; + } + + if (RT <= 0) return "DC motor: thermal resistance must be positive"; + if (C <= 0) return "DC motor: thermal capacitance must be positive"; + + actuator->dynprm[2] = RT; + actuator->dynprm[3] = C; + actuator->dynprm[4] = Ta; + actuator->gainprm[2] = alpha; + actuator->gainprm[3] = T0; + actdim++; + } + + // lugre: {stiffness, damping, viscous, coulomb, static, stribeck} + if (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 + actdim++; + } + + // set input mode and activation dimension + actuator->gainprm[8] = input_mode; + actuator->actdim = actdim; + + // enforce actlimited = 0; homogeneous bounds are invalid across DC motor states + actuator->actlimited = 0; + + return ""; +} + + + // get spec from body mjSpec* mjs_getSpec(mjsElement* element) { return &(static_cast(element)->model->spec); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index c8395157..5198cc20 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7222,20 +7222,20 @@ void mjCActuator::Compile(void) { // check and set actdim if (!plugin.active) { - if (actdim > 1 && dyntype != mjDYN_USER) { - throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator"); + if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR) { + throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' and 'dcmotor'"); } if (actdim == 1 && dyntype == mjDYN_NONE) { throw mjCError(this, "invalid actdim 1 in stateless actuator"); } - if (actdim == 0 && dyntype != mjDYN_NONE) { + if (actdim == 0 && dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR) { throw mjCError(this, "invalid actdim 0 in stateful actuator"); } } - // set actdim + // set actdim to 1 if it is unset and type is standard one-activation dyntype if (actdim < 0) { - actdim = (dyntype != mjDYN_NONE); + actdim = (dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR); } // check muscle parameters diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d40e6a9a..352790b5 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -206,6 +206,10 @@ std::vector MJCF[nMJCF] = { "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "?", "forcelimited", "ctrlrange", "forcerange", "gain", "user", "group", "nsample", "interp", "delay"}, + {"dcmotor", "?", "ctrllimited", "ctrlrange", + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", + "motorconst", "resistance", "nominal", "saturation", + "inductance", "cogging", "controller", "input", "thermal", "lugre"}, {">"}, {"extension", "*"}, @@ -436,6 +440,12 @@ std::vector MJCF[nMJCF] = { "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay", "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"}, + {"dcmotor", "*", "name", "class", "group", "nsample", "interp", "delay", + "ctrllimited", "ctrlrange", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", + "motorconst", "resistance", "nominal", "saturation", + "inductance", "cogging", "controller", "thermal", "lugre", "input"}, {"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "damping", "armature", "cranklength", "joint", "jointinparent", @@ -724,33 +734,45 @@ const mjMap mark_map[mark_sz] = { // dyn type -const int dyn_sz = 6; +const int dyn_sz = 7; const mjMap dyn_map[dyn_sz] = { {"none", mjDYN_NONE}, {"integrator", mjDYN_INTEGRATOR}, {"filter", mjDYN_FILTER}, {"filterexact", mjDYN_FILTEREXACT}, {"muscle", mjDYN_MUSCLE}, + {"dcmotor", mjDYN_DCMOTOR}, {"user", mjDYN_USER} }; +// dcmotor controller input mode +const int dcmotorinput_sz = 3; +const mjMap dcmotorinput_map[dcmotorinput_sz] = { + {"voltage", 0}, + {"position", 1}, + {"velocity", 2} +}; + + // gain type -const int gain_sz = 4; +const int gain_sz = 5; const mjMap gain_map[gain_sz] = { {"fixed", mjGAIN_FIXED}, {"affine", mjGAIN_AFFINE}, {"muscle", mjGAIN_MUSCLE}, + {"dcmotor", mjGAIN_DCMOTOR}, {"user", mjGAIN_USER} }; // bias type -const int bias_sz = 4; +const int bias_sz = 5; const mjMap bias_map[bias_sz] = { {"none", mjBIAS_NONE}, {"affine", mjBIAS_AFFINE}, {"muscle", mjBIAS_MUSCLE}, + {"dcmotor", mjBIAS_DCMOTOR}, {"user", mjBIAS_USER} }; @@ -2498,6 +2520,54 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { err = mjs_setToAdhesion(actuator, gain); } + // DC motor + else if (type == "dcmotor") { + bool inherited = (actuator->gaintype == mjGAIN_DCMOTOR); + 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, + inherited ? actuator->gainprm[6] : 0, + inherited ? actuator->gainprm[7] : 0, + inherited ? actuator->dynprm[7] : 0, + inherited ? actuator->dynprm[8] : 0}; + double inductance[2] = {0, inherited ? actuator->dynprm[0] : 0}; + double cogging[3] = {inherited ? actuator->biasprm[0] : 0, + inherited ? actuator->biasprm[1] : 0, + inherited ? actuator->biasprm[2] : 0}; + double thermal[6] = {inherited ? actuator->dynprm[2] : 0, + inherited ? actuator->dynprm[3] : 0, + 0, + inherited ? actuator->gainprm[2] : 0, + inherited ? actuator->gainprm[3] : 0, + inherited ? actuator->dynprm[4] : 0}; + double lugre[6] = {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; + 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, "inductance", 2, inductance, text, false, false); + ReadAttr(elem, "cogging", 3, cogging, text, false, false); + ReadAttr(elem, "controller", 5, controller, text, false, false); + ReadAttr(elem, "thermal", 6, thermal, text, false, false); + ReadAttr(elem, "lugre", 6, lugre, text, false, false); + if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) { + // successfully parsed + } + err = mjs_setToDCMotor(actuator, motorconst, resistance, + nominal, saturation, inductance, + cogging, controller, thermal, lugre, input_mode); + } + else if (type == "plugin") { OnePlugin(elem, &actuator->plugin); int n; @@ -2962,7 +3032,8 @@ void mjXReader::Default(XMLElement* section, const mjsDefault* def, const mjVFS* name == "intvelocity" || name == "cylinder" || name == "muscle" || - name == "adhesion") { + name == "adhesion" || + name == "dcmotor") { OneActuator(elem, def->actuator); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 4e0369ce..8c568b22 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -102,7 +102,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 246 +#define nMJCF 248 extern std::vector MJCF[nMJCF]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 144ffe65..e9aa9ea7 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -871,7 +871,7 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe if (writingdefaults) { WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim); } else { - int default_actdim = actuator->dyntype == mjDYN_NONE ? 0 : 1; + int default_actdim = (actuator->dyntype != mjDYN_NONE && actuator->dyntype != mjDYN_DCMOTOR); WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim); } WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, actuator->dyntype, def->Actuator().dyntype); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index df8f863b..c32e6b1e 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -91,6 +91,8 @@ static const char* const kDampedPendulumPath = "engine/testdata/derivative/damped_pendulum.xml"; static const char* const kLinearPath = "engine/testdata/derivative/linear.xml"; +static const char* const kDCMotorPath = + "engine/testdata/derivative/dcmotor.xml"; static const char* const kModelPath = "testdata/model.xml"; // compare analytic and finite-difference d_smooth/d_qvel @@ -99,9 +101,12 @@ TEST_F(DerivativeTest, SmoothDvel) { for (const char* local_path : {kEnergyConservingPendulumPath, kTumblingThinObjectPath, kDampedActuatorsPath, - kDamperActuatorsPath}) { + kDamperActuatorsPath, + kDCMotorPath}) { const std::string xml_path = GetTestDataFilePath(local_path); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + char error[1024] = ""; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error; int nD = model->nD; mjData* data = mj_makeData(model); @@ -758,9 +763,12 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) { for (const char* local_path : {kEnergyConservingPendulumPath, kTumblingThinObjectPath, kDampedActuatorsPath, - kDamperActuatorsPath}) { + kDamperActuatorsPath, + kDCMotorPath}) { const std::string xml_path = GetTestDataFilePath(local_path); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + char error[1024] = ""; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error; int nD = model->nD; mjtNum* qDeriv = (mjtNum*) mju_malloc(sizeof(mjtNum)*nD); mjData* data = mj_makeData(model); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index cee67e29..f3acf0aa 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1148,6 +1148,949 @@ TEST_F(ActuatorTest, DampRatioTendon) { mj_deleteModel(model); } +// ----------------------- DC motor actuators ---------------------------------- + +using DCMotorTest = MujocoTest; + +TEST_F(DCMotorTest, IntVelocityEquivalence) { + 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); + + // Apply a time-varying velocity command + while (data->time < 1.0) { + data->ctrl[0] = mju_sin(20 * data->time); + data->ctrl[1] = mju_sin(20 * data->time); + mj_step(model, data); + + // Both actuators should integrate identical states + EXPECT_MJTNUM_EQ(data->act[0], data->act[1]); + + // 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 + EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1], + MjTol(1e-14, 1e-6)); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatelessSteadyState) { + 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); + + double K = 0.05; + double R = 2.0; + double V = 12.0; + double omega = 3.0; + + data->ctrl[0] = V; + data->qvel[0] = omega; + mj_forward(model, data); + + double expected_force = K / R * (V - K * omega); + EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-12, 1e-5)); + EXPECT_EQ(model->actuator_actnum[0], 0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentFilterConverges) { + 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); + + ASSERT_EQ(model->actuator_actnum[0], 1); + + double K = 0.05; + double R = 2.0; + double V = 12.0; + + data->ctrl[0] = V; + for (int i = 0; i < 10000; i++) { + mj_step(model, data); + } + + double omega = data->qvel[0]; + double i_ss = V / R - K / R * omega; + double expected_force = K * i_ss; + + EXPECT_NEAR(data->act[0], i_ss, MjTol(1e-6, 1e-4)); + EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-6, 1e-4)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentFilterExactIntegration) { + 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); + + double R = 2.0; + double te = 0.01 / R; + double V = 12.0; + + data->ctrl[0] = V; + mj_step(model, data); + + double h = model->opt.timestep; + double exact_current = V / R * (1 - mju_exp(-h / te)); + EXPECT_NEAR(data->act[0], exact_current, MjTol(1e-10, 1e-4)); + + double euler_current = V / R * h / te; + EXPECT_GT(std::abs(data->act[0] - euler_current), + std::abs(data->act[0] - exact_current)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CoggingTorque) { + 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); + + double A = 0.1, Np = 6, phi = 0; + double K = 0.05, R = 2.0; + double V = 5.0; + double pos = 1.0; + + data->ctrl[0] = V; + data->qpos[0] = pos; + mj_forward(model, data); + + double electrical_force = K / R * V; + double cogging = A * mju_sin(Np * pos + phi); + EXPECT_NEAR(data->actuator_force[0], electrical_force + cogging, + MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CoggingBypassesSaturation) { + 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); + + double A = 0.1, Np = 6, phi = 0; + double pos = 1.0; + + data->ctrl[0] = 100.0; + data->qpos[0] = pos; + mj_forward(model, data); + + double cogging = A * mju_sin(Np * pos + phi); + EXPECT_NEAR(model->actuator_forcerange[1], 0.001, MjTol(1e-12, 1e-5)); + EXPECT_GT(mju_abs(data->actuator_force[0]), 0.001); + EXPECT_NEAR(data->actuator_force[0], 0.001 + cogging, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreViscousFriction) { + 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); + + ASSERT_EQ(model->actuator_actnum[0], 1); + + double sigma1 = 1, sigma2 = 0.01; + double K = 0.05, R = 2.0; + double omega = 2.0; + + data->ctrl[0] = 0; + data->qvel[0] = omega; + mj_forward(model, data); + + EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2); + 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)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, ThermalRiseAndFall) { + 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); + + int adr = model->actuator_actadr[0]; + ASSERT_EQ(model->actuator_actnum[0], 1); + EXPECT_EQ(data->act[adr], 0); + + double R = 2.0, V = 10.0; + double RT = 10.0, C = 5.0; + double h = model->opt.timestep; + double P = V * V / R; + + data->ctrl[0] = V; + + mj_step(model, data); + double dT1 = h * P / C; + EXPECT_NEAR(data->act[adr], dT1, MjTol(1e-11, 1e-4)); + + mj_step(model, data); + double dT2 = dT1 + h * (P - dT1 / RT) / C; + EXPECT_NEAR(data->act[adr], dT2, MjTol(1e-11, 1e-4)); + + data->ctrl[0] = 0; + mj_step(model, data); + double dT3 = dT2 + h * (0 - dT2 / RT) / C; + EXPECT_NEAR(data->act[adr], dT3, MjTol(1e-11, 1e-4)); + EXPECT_LT(data->act[adr], dT2); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, ThermalSteadyState) { + 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); + + double R = 2.0, V = 10.0; + double RT = 0.1; + double dT_ss = RT * V * V / R; + + data->ctrl[0] = V; + for (int i = 0; i < 10000; i++) { + mj_step(model, data); + } + + int adr = model->actuator_actadr[0]; + EXPECT_NEAR(data->act[adr], dT_ss, 1e-4); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, ThermalAffectsForce) { + 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); + + double K = 0.05, R = 2.0, V = 10.0; + double alpha = 0.004; + int adr = model->actuator_actadr[0]; + + data->ctrl[0] = V; + data->act[adr] = 0; + mj_forward(model, data); + double force_cold = data->actuator_force[0]; + EXPECT_NEAR(force_cold, K / R * V, MjTol(1e-12, 1e-5)); + + double dT = 50; + data->act[adr] = dT; + mj_forward(model, data); + double R_hot = R * (1 + alpha * dT); + double force_hot = data->actuator_force[0]; + EXPECT_NEAR(force_hot, K / R_hot * V, MjTol(1e-12, 1e-5)); + EXPECT_LT(force_hot, force_cold); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// Temperature slot must be correctly offset past slew and integral states. +TEST_F(DCMotorTest, ThermalAffectsForceWithController) { + 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); + + // slot order: slew(0), integral(1), temperature(2) + ASSERT_EQ(model->actuator_actnum[0], 3); + 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 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 + data->act[temp_adr] = dT; // temperature rise above ambient + data->ctrl[0] = 1.0; // position setpoint = 1.0, qpos = 0, error = 1.0 + mj_forward(model, data); + + // 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 + double R_hot = R * (1 + alpha * dT); + EXPECT_NEAR(data->actuator_force[0], K / R_hot * 1.0, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatelessPositionMode) { + 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); + + // Position target 5.0, current pos 0.0, current vel 0.0 + data->ctrl[0] = 5.0; + mj_forward(model, data); + + // 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)); + + // Velocity penalty + data->qvel[0] = 2.0; + mj_forward(model, data); + // 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)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatelessVelocityMode) { + 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); + + // Velocity target 4.0, current vel 1.0 + data->ctrl[0] = 4.0; + data->qvel[0] = 1.0; + mj_forward(model, data); + + // 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)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatefulPositionMode) { + 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); + + // Controller states: 1 for slew, 1 for ki -> actnum = 2 + ASSERT_EQ(model->actuator_actnum[0], 2); + int adr = model->actuator_actadr[0]; + + // Current states + double u_prev = 1.0; + double x_I = 2.0; + data->act[adr] = u_prev; + data->act[adr+1] = x_I; + + // target 5.0 position, current 0.0 + data->ctrl[0] = 5.0; + data->qvel[0] = 0.5; + mj_forward(model, data); + + // slew bounding: s = 10.0, dt = 0.001. max_change = 0.01 + // Target = 5.0. It is upper bounded by u_prev + 0.01 = 1.01 + EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5)); + + // 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)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { + 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); + + // Controller states: slew (0), ki (1), current (2). actnum = 3 + ASSERT_EQ(model->actuator_actnum[0], 3); + int adr = model->actuator_actadr[0]; + + double u_prev = 1.0; + double x_I = 2.0; + double current = 0.5; + data->act[adr] = u_prev; + data->act[adr+1] = x_I; + data->act[adr+2] = current; + + // Target 5.0 position, velocity 0.5 + data->ctrl[0] = 5.0; + data->qvel[0] = 0.5; + mj_forward(model, data); + + // Slew bounding: max_change = 0.01, u_eff = 1.01 + EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5)); + + // 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 + + // 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)); + + // Force is just K * current since current is stateful + EXPECT_NEAR(data->actuator_force[0], 0.05 * 0.5, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatefulVelocityMode) { + 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); + + // Controller states: 1 for ki (no slew) + ASSERT_EQ(model->actuator_actnum[0], 1); + int adr = model->actuator_actadr[0]; + + 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 + data->ctrl[0] = 4.0; + data->qvel[0] = 1.0; + mj_forward(model, data); + + // 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 + 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)); + + // repeat with non-zero joint position + data->qpos[0] = 1.5; + mj_forward(model, data); + + // 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)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentPlusThermal) { + 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); + + ASSERT_EQ(model->actuator_actnum[0], 2); + int adr = model->actuator_actadr[0]; + + double K = 0.05, R = 2.0, V = 12.0; + double te = 0.01 / R; + double RT = 10.0, C = 5.0; + + double current = 3.0; + double dT = 10.0; + data->act[adr] = dT; + data->act[adr+1] = current; + data->ctrl[0] = V; + mj_forward(model, data); + + EXPECT_NEAR(data->actuator_force[0], K * current, MjTol(1e-12, 1e-5)); + + double R_hot = R * (1 + 0.004 * dT); + double T_dot = (R_hot * current * current - dT / RT) / C; + EXPECT_NEAR(data->act_dot[adr], T_dot, MjTol(1e-10, 1e-4)); + + double omega = data->qvel[0]; + double i_dot = (V/R_hot - K/R_hot*omega - current) / te; + EXPECT_NEAR(data->act_dot[adr+1], i_dot, MjTol(1e-10, 1e-3)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentRateLimit) { + // Verifies that saturation:current_rate clamps di/dt. + 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); + + ASSERT_EQ(model->actuator_actnum[0], 1); + int adr = model->actuator_actadr[0]; + + double V = 12.0; + double dimax = 100.0; // A/s rate limit + + // unclamped: i_dot = (V/R - 0 - 0) / te = 6 / 0.005 = 1200 A/s >> dimax + data->act[adr] = 0; // current = 0 + data->ctrl[0] = V; + mj_forward(model, data); + + // i_dot should be clipped to +dimax + EXPECT_NEAR(data->act_dot[adr], dimax, MjTol(1e-12, 1e-5)); + + // reverse: large negative drive + data->ctrl[0] = -V; + mj_forward(model, data); + + // i_dot should be clipped to -dimax + EXPECT_NEAR(data->act_dot[adr], -dimax, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreExactIntegration) { + 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); + + ASSERT_EQ(model->actuator_actnum[0], 1); + int adr = model->actuator_actadr[0]; + + double sigma0 = 100, F_C = 0.5, F_S = 0.7, v_S = 10; + double z0 = 0.002; + double v = 0.5; + double h = model->opt.timestep; + + data->act[adr] = z0; + data->qvel[0] = v; + + double ratio = v / v_S; + double g_v = F_C + (F_S - F_C) * mju_exp(-ratio*ratio); + double a = -sigma0 * std::abs(v) / g_v; + double exp_ah = mju_exp(a * h); + double int_h = (exp_ah - 1) / a; + double z_new = exp_ah * z0 + int_h * v; + + mj_step(model, data); + EXPECT_NEAR(data->act[adr], z_new, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreSteadyState) { + 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); + + int adr = model->actuator_actadr[0]; + + double sigma0 = 100, sigma2 = 0.01; + double F_C = 0.5, F_S = 0.7, v_S = 10; + double K = 0.05, R = 2.0; + double v = 0.5; + + data->qvel[0] = v; + data->ctrl[0] = 0; + for (int i = 0; i < 10000; i++) { + mj_step(model, data); + } + + double ratio = v / v_S; + double g_v = F_C + (F_S - F_C) * mju_exp(-ratio*ratio); + double z_ss = g_v / sigma0; + EXPECT_NEAR(data->act[adr], z_ss, 1e-4); + + EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2); + double back_emf = K * K / R * data->qvel[0]; + double lugre_ss = g_v; + EXPECT_NEAR(data->actuator_force[0], -back_emf - lugre_ss, 1e-3); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreBristleSpring) { + 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); + + int adr = model->actuator_actadr[0]; + double sigma0 = 100; + double X = 0.01; + + data->act[adr] = X; + data->ctrl[0] = 0; + mj_forward(model, data); + + EXPECT_NEAR(data->actuator_force[0], -sigma0 * X, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + // ----------------------- filterexact actuators ------------------------------- using FilterExactTest = MujocoTest; diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml new file mode 100644 index 00000000..d3c4b0ac --- /dev/null +++ b/test/engine/testdata/derivative/dcmotor.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 45ae2393..bd1b517b 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3003,6 +3003,325 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { mj_deleteModel(model); } +TEST_F(ActuatorParseTest, DCMotorBasicParsing) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_dyntype[0], mjDYN_DCMOTOR); + EXPECT_EQ(model->actuator_gaintype[0], mjGAIN_DCMOTOR); + EXPECT_EQ(model->actuator_biastype[0], mjBIAS_DCMOTOR); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05); + EXPECT_MJTNUM_EQ(model->actuator_damping[0], 1.0); + EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[0], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[1], 3.0); + EXPECT_MJTNUM_EQ(model->actuator_armature[0], 0.1); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorNominalDerivation) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // actuator 0: B = 0, Ke = vn/omega0 + { + double K = 12.0 / 600.0; + double R = K * 12.0 / 0.6; + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0*mjNGAIN + 0], R); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0*mjNGAIN + 1], K); + } + + // actuator 1: B > 0, R given, quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0 + { + double B = 0.0001, R = 0.4, vn = 12.0, omega0 = 600.0; + double disc = vn*vn - 4*R*B*omega0*omega0; + double Ke = (vn + sqrt(disc)) / (2*omega0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1*mjNGAIN + 0], R); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1*mjNGAIN + 1], Ke); + } + + // actuator 2: B > 0, R from nominal, Ke = vn/omega0 - vn*B/tau0 + { + double B = 0.0001, vn = 12.0, tau0 = 0.6, omega0 = 600.0; + double Ke = vn / omega0 - vn*B / tau0; + double R = Ke * vn / tau0; + EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 0], R); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 1], Ke); + } + + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorSaturation) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_forcelimited[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_forcerange[0], -1.5); + EXPECT_MJTNUM_EQ(model->actuator_forcerange[1], 1.5); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + 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_dynprm[5], 100); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[6], 1); + EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01); + EXPECT_MJTNUM_EQ(model->actuator_biasprm[3], 0.5); + EXPECT_MJTNUM_EQ(model->actuator_biasprm[4], 0.7); + EXPECT_MJTNUM_EQ(model->actuator_biasprm[5], 10); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimStateless) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 0); + EXPECT_EQ(model->actuator_actadr[0], -1); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimCurrentOnly) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.001 / 2.0); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimThermalOnly) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[2], 10); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[3], 5); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[4], 25); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimAllThree) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 3); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorMissingKError) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("motor constant K must be positive")); +} + +TEST_F(ActuatorParseTest, DCMotorDefaultsPropagate) { + 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_gainprm[0], 1.5); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.03); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorMotorconstGeometricMean) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + double K = std::sqrt(0.03 * 0.05); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], K); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[mjNGAIN + 1], 0.03); + mj_deleteModel(model); +} + TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index ea8ae869..1e6168a9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -269,19 +269,22 @@ public enum mjtDyn : int{ mjDYN_FILTER = 2, mjDYN_FILTEREXACT = 3, mjDYN_MUSCLE = 4, - mjDYN_USER = 5, + mjDYN_DCMOTOR = 5, + mjDYN_USER = 6, } public enum mjtGain : int{ mjGAIN_FIXED = 0, mjGAIN_AFFINE = 1, mjGAIN_MUSCLE = 2, - mjGAIN_USER = 3, + mjGAIN_DCMOTOR = 3, + mjGAIN_USER = 4, } public enum mjtBias : int{ mjBIAS_NONE = 0, mjBIAS_AFFINE = 1, mjBIAS_MUSCLE = 2, - mjBIAS_USER = 3, + mjBIAS_DCMOTOR = 3, + mjBIAS_USER = 4, } public enum mjtObj : int{ mjOBJ_UNKNOWN = 0, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 53013ccd..cfa83b40 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -9876,6 +9876,18 @@ 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) { + UNPACK_VALUE(double, motorconst); + UNPACK_VALUE(double, nominal); + UNPACK_VALUE(double, saturation); + UNPACK_VALUE(double, inductance); + UNPACK_VALUE(double, cogging); + UNPACK_VALUE(double, controller); + UNPACK_VALUE(double, thermal); + UNPACK_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)); +} + std::string mjs_setToDamper_wrapper(MjsActuator& actuator, double kv) { return std::string(mjs_setToDamper(actuator.get(), kv)); } @@ -10812,6 +10824,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjBIAS_NONE", mjBIAS_NONE) .value("mjBIAS_AFFINE", mjBIAS_AFFINE) .value("mjBIAS_MUSCLE", mjBIAS_MUSCLE) + .value("mjBIAS_DCMOTOR", mjBIAS_DCMOTOR) .value("mjBIAS_USER", mjBIAS_USER); enum_("mjtBuiltin") .value("mjBUILTIN_NONE", mjBUILTIN_NONE) @@ -10912,6 +10925,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjDYN_FILTER", mjDYN_FILTER) .value("mjDYN_FILTEREXACT", mjDYN_FILTEREXACT) .value("mjDYN_MUSCLE", mjDYN_MUSCLE) + .value("mjDYN_DCMOTOR", mjDYN_DCMOTOR) .value("mjDYN_USER", mjDYN_USER); enum_("mjtEnableBit") .value("mjENBL_OVERRIDE", mjENBL_OVERRIDE) @@ -10974,6 +10988,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjGAIN_FIXED", mjGAIN_FIXED) .value("mjGAIN_AFFINE", mjGAIN_AFFINE) .value("mjGAIN_MUSCLE", mjGAIN_MUSCLE) + .value("mjGAIN_DCMOTOR", mjGAIN_DCMOTOR) .value("mjGAIN_USER", mjGAIN_USER); enum_("mjtGeom") .value("mjGEOM_PLANE", mjGEOM_PLANE) @@ -13295,6 +13310,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mjs_setName", &mjs_setName_wrapper); function("mjs_setToAdhesion", &mjs_setToAdhesion_wrapper); function("mjs_setToCylinder", &mjs_setToCylinder_wrapper); + function("mjs_setToDCMotor", &mjs_setToDCMotor_wrapper); function("mjs_setToDamper", &mjs_setToDamper_wrapper); function("mjs_setToIntVelocity", &mjs_setToIntVelocity_wrapper); function("mjs_setToMotor", &mjs_setToMotor_wrapper);