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

Add <dcmotor> actuator and related docs and tests.

Browse Source 
PiperOrigin-RevId: 892927987
Change-Id: I38ed6412801341ba03ddf5fe7b93a6081df24d37
This commit is contained in:
Yuval Tassa
2026-04-01 07:49:53 -07:00
committed by Copybara-Service
parent 6da210c794
commit 70a7647ad9
31 changed files with 3994 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+9
View File
@@ -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
doc/XMLreference.rst
+219
View File
@@ -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<geActuation>`. 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<actuator-dcmotor-resistance>`, :ref:`motorconst<actuator-dcmotor-motorconst>` and
:ref:`nominal<actuator-dcmotor-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<actuator-dcmotor-input>` semantic is either the voltage applied to the motor terminals, or a
position or velocity target for a PID :ref:`controller<actuator-dcmotor-controller>`.
- Optional features include electrical dynamics (:ref:`inductance<actuator-dcmotor-inductance>`),
:ref:`cogging torque<actuator-dcmotor-cogging>`, :ref:`thermal resistance variation<actuator-dcmotor-thermal>`, and
:ref:`LuGre<actuator-dcmotor-lugre>` friction.
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-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<actuator-dcmotor-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<actuator-general-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<actuator-dcmotor-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 <actuator-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 <actuator-dcmotor>` attributes are available here except: name, class, joint, jointinparent, site,
refsite, tendon, slidersite, cranksite.
.. _custom:
**custom** |*|
doc/XMLschema.rst
+168
View File
@@ -2984,6 +2984,105 @@
:ref:`gain<actuator-adhesion-gain>`
.. dropdown:: :ref:`dcmotor<actuator-dcmotor>` |*|
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`name<actuator-dcmotor-name>`
.. grid-item::
:ref:`class<actuator-dcmotor-class>`
.. grid-item::
:ref:`group<actuator-dcmotor-group>`
.. grid-item::
:ref:`nsample<actuator-dcmotor-nsample>`
.. grid-item::
:ref:`interp<actuator-dcmotor-interp>`
.. grid-item::
:ref:`delay<actuator-dcmotor-delay>`
.. grid-item::
:ref:`ctrllimited<actuator-dcmotor-ctrllimited>`
.. grid-item::
:ref:`ctrlrange<actuator-dcmotor-ctrlrange>`
.. grid-item::
:ref:`lengthrange<actuator-dcmotor-lengthrange>`
.. grid-item::
:ref:`gear<actuator-dcmotor-gear>`
.. grid-item::
:ref:`damping<actuator-dcmotor-damping>`
.. grid-item::
:ref:`armature<actuator-dcmotor-armature>`
.. grid-item::
:ref:`cranklength<actuator-dcmotor-cranklength>`
.. grid-item::
:ref:`user<actuator-dcmotor-user>`
.. grid-item::
:ref:`joint<actuator-dcmotor-joint>`
.. grid-item::
:ref:`jointinparent<actuator-dcmotor-jointinparent>`
.. grid-item::
:ref:`tendon<actuator-dcmotor-tendon>`
.. grid-item::
:ref:`slidersite<actuator-dcmotor-slidersite>`
.. grid-item::
:ref:`cranksite<actuator-dcmotor-cranksite>`
.. grid-item::
:ref:`site<actuator-dcmotor-site>`
.. grid-item::
:ref:`refsite<actuator-dcmotor-refsite>`
.. grid-item::
:ref:`motorconst<actuator-dcmotor-motorconst>`
.. grid-item::
:ref:`resistance<actuator-dcmotor-resistance>`
.. grid-item::
:ref:`nominal<actuator-dcmotor-nominal>`
.. grid-item::
:ref:`saturation<actuator-dcmotor-saturation>`
.. grid-item::
:ref:`inductance<actuator-dcmotor-inductance>`
.. grid-item::
:ref:`cogging<actuator-dcmotor-cogging>`
.. grid-item::
:ref:`controller<actuator-dcmotor-controller>`
.. grid-item::
:ref:`thermal<actuator-dcmotor-thermal>`
.. grid-item::
:ref:`lugre<actuator-dcmotor-lugre>`
.. grid-item::
:ref:`input<actuator-dcmotor-input>`
.. dropdown:: :ref:`plugin<actuator-plugin>` |*|
.. grid:: 2 3 4 4
@@ -6146,6 +6245,75 @@
:ref:`delay<default-adhesion-delay>`
.. dropdown:: :ref:`dcmotor<default-dcmotor>` :octicon:`dot`
.. grid:: 2 3 4 4
:gutter: 0
.. grid-item::
:ref:`ctrllimited<default-dcmotor-ctrllimited>`
.. grid-item::
:ref:`ctrlrange<default-dcmotor-ctrlrange>`
.. grid-item::
:ref:`gear<default-dcmotor-gear>`
.. grid-item::
:ref:`damping<default-dcmotor-damping>`
.. grid-item::
:ref:`armature<default-dcmotor-armature>`
.. grid-item::
:ref:`cranklength<default-dcmotor-cranklength>`
.. grid-item::
:ref:`user<default-dcmotor-user>`
.. grid-item::
:ref:`group<default-dcmotor-group>`
.. grid-item::
:ref:`nsample<default-dcmotor-nsample>`
.. grid-item::
:ref:`interp<default-dcmotor-interp>`
.. grid-item::
:ref:`delay<default-dcmotor-delay>`
.. grid-item::
:ref:`motorconst<default-dcmotor-motorconst>`
.. grid-item::
:ref:`resistance<default-dcmotor-resistance>`
.. grid-item::
:ref:`nominal<default-dcmotor-nominal>`
.. grid-item::
:ref:`saturation<default-dcmotor-saturation>`
.. grid-item::
:ref:`inductance<default-dcmotor-inductance>`
.. grid-item::
:ref:`cogging<default-dcmotor-cogging>`
.. grid-item::
:ref:`controller<default-dcmotor-controller>`
.. grid-item::
:ref:`input<default-dcmotor-input>`
.. grid-item::
:ref:`thermal<default-dcmotor-thermal>`
.. grid-item::
:ref:`lugre<default-dcmotor-lugre>`
.. dropdown:: :ref:`custom<custom>` |*|
doc/_static/dcmotor.pdf
Vendored
BIN
View File
Binary file not shown.
doc/changelog.rst
+3
View File
@@ -8,6 +8,9 @@ Upcoming version (not yet released)
General
^^^^^^^
- Added the :ref:`dcmotor<actuator-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<actuator-general-damping>` and :ref:`armature<actuator-general-armature>` to their transmission target.
These are applied during the passive force and inertia computations, respectively, and are scaled by gear\ :sup:`2`
doc/dcmotor/buildpdf.sh
Executable
+21
View File
@@ -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/
doc/dcmotor/dcmotor.tex
+1416
View File
File diff suppressed because it is too large Load Diff
doc/dcmotor/refs.bib
+82
View File
@@ -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},
}
doc/includes/references.h
+8 -1
View File
@@ -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);
include/mujoco/mjmodel.h
+4 -1
View File
@@ -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;
include/mujoco/mujoco.h
+6
View File
@@ -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 --------------------------------------------------------
python/mujoco/introspect/enums.py
+6 -3
View File
@@ -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',
python/mujoco/introspect/functions.py
+80
View File
@@ -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',
python/mujoco/specs.cc
+25
View File
@@ -1303,6 +1303,31 @@ PYBIND11_MODULE(_specs, m) {
}
},
py::arg("gain"));
mjsActuator.def(
"set_to_dcmotor",
[](raw::MjsActuator* self, std::array<double, 2> motorconst,
double resistance,
std::array<double, 3> nominal, std::array<double, 4> saturation,
std::array<double, 2> inductance, std::array<double, 3> cogging,
std::array<double, 5> controller, std::array<double, 6> thermal,
std::array<double, 6> lugre, int input_mode) {
std::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<double, 3>{0, 0, 0},
py::arg("saturation") = std::array<double, 4>{0, 0, 0, 0},
py::arg("inductance") = std::array<double, 2>{0, 0},
py::arg("cogging") = std::array<double, 3>{0, 0, 0},
py::arg("controller") = std::array<double, 5>{0, 0, 0, 0, 0},
py::arg("thermal") = std::array<double, 6>{0, 0, 0, 0, 0, 0},
py::arg("lugre") = std::array<double, 6>{0, 0, 0, 0, 0, 0},
py::arg("input_mode") = 0);
// ============================= MJSTENDONPATH ===============================
// helper struct for tendon path indexing
python/mujoco/specs_test.py
+7
View File
@@ -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(
src/engine/engine_derivative.c
+33
View File
@@ -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) {
src/engine/engine_forward.c
+243 -26
View File
@@ -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);
src/engine/engine_support.c
+53 -6
View File
@@ -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]);
}
src/engine/engine_util_misc.c
+20
View File
@@ -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
src/engine/engine_util_misc.h
+17
View File
@@ -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);
src/user/user_api.cc
+161
View File
@@ -15,6 +15,7 @@
#include "user/user_api.h"
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
@@ -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<mjCBase*>(element)->model->spec);
src/user/user_objects.cc
+5 -5
View File
@@ -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
src/xml/xml_native_reader.cc
+75 -4
View File
@@ -206,6 +206,10 @@ std::vector<const char*> 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<const char*> 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);
}
src/xml/xml_native_reader.h
+1 -1
View File
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 246
#define nMJCF 248
extern std::vector<const char*> MJCF[nMJCF];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+1 -1
View File
@@ -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);
test/engine/engine_derivative_test.cc
+12 -4
View File
@@ -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);
test/engine/engine_forward_test.cc
+943
View File
@@ -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"(
<mujoco>
<option integrator="implicit"/>
<worldbody>
<body pos="0 0 0">
<joint name="slide1" type="slide" axis="1 0 0"/>
<geom size=".1"/>
</body>
<body pos="0 1 0">
<joint name="slide2" type="slide" axis="1 0 0"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<!--
Equivalence mapping:
intvelocity force: F = kp * \int(ctrl - v) - kv * v
dcmotor force: F = (V*K - K^2*v) / R
where V = ki * \int(ctrl - v) (since kp=0, kd=0)
Setting K=1, R=0.2, ki=2 yields:
F = (2 * \int(ctrl - v) - v) / 0.2
= 10 * \int(ctrl - v) - 5 * v
This perfectly matches intvelocity with kp=10, kv=5.
-->
<intvelocity name="intvel" joint="slide1" kp="10" kv="5" actrange="-0.01 0.01"/>
<dcmotor name="dcmotor" joint="slide2" motorconst="1" resistance="0.2" input="velocity" controller="0 2 0 0 0.01"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
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"(
<mujoco>
<option timestep="0.0001"/>
<worldbody>
<body>
<joint name="joint" damping="1000"/>
<geom size="1" mass="100"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
cogging="0.1 6 0"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
saturation="0.001 0" cogging="0.1 6 0"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
thermal="10 5 0 0 25 25"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
thermal="0.1 0.1 0 0 25 25"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
thermal="0.1 0.1 0 0.004 25 25"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
input="position" controller="1.0 1.0 0 5.0 0"
thermal="0.1 0.1 0 0.004 25 25"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0 0.5 0 0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// 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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="velocity" controller="3.0 0 0 0 0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// 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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0.1 10.0 5.0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// 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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="position" controller="2.0 0.5 0.1 10.0 5.0"
motorconst="0.05" resistance="2.0" inductance="1.0"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" input="velocity" controller="3.0 1.0 0 0 2.0"
motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// 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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0" thermal="10 5 0 0.004 25 25"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint" damping="10000"/>
<geom size="1" mass="10000"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
inductance="0.01 0" saturation="0 0 100 0"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1" mass="1e6"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<option timestep="0.001"/>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1" mass="1e6"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
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"(
<mujoco>
<worldbody>
<body>
<joint name="joint"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="joint" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
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;
test/engine/testdata/derivative/dcmotor.xml
Vendored
+35
View File
@@ -0,0 +1,35 @@
<mujoco>
<worldbody>
<body name="motor1" pos="0 0.1 0">
<joint name="slide1" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor2" pos="0 0.2 0">
<joint name="slide2" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor3" pos="0 0.3 0">
<joint name="slide3" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor4" pos="0 0.4 0">
<joint name="joint4"/>
<geom size=".03"/>
</body>
</worldbody>
<actuator>
<!-- DC motor back-EMF stateless damping -->
<dcmotor name="dc_bias" joint="slide1" motorconst="2.0" resistance="0.5"/>
<!-- DC motor velocity controller damping -->
<dcmotor name="dc_vel" joint="slide2" motorconst="1.0" resistance="1.0" input="velocity" controller="0 5"/>
<!-- DC motor position controller damping -->
<dcmotor name="dc_pos" joint="slide3" motorconst="1.0" resistance="1.0" input="position" controller="10 0 5"/>
<!-- DC motor with LuGre friction (sigma1 micro-damping) -->
<dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0"
lugre="1e4 100 0.001 0.005 0.008 0.1"/>
</actuator>
</mujoco>
test/xml/xml_native_reader_test.cc
+319
View File
@@ -3003,6 +3003,325 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) {
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DCMotorBasicParsing) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0" damping="1 2 3" armature="0.1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<!-- no damping: Ke = vn/omega0 -->
<dcmotor joint="jnt" nominal="12 0.6 600"/>
<!-- B > 0, R given: quadratic -->
<dcmotor joint="jnt" nominal="12 0 600" resistance="0.4" damping="0.0001"/>
<!-- B > 0, R from nominal: linear -->
<dcmotor joint="jnt" nominal="12 0.6 600" damping="0.0001"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
saturation="1.5 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
inductance="0.001 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
thermal="10 5 0 0 0 25"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.05" resistance="2.0"
inductance="0.001 0"
thermal="10 5 0 0 0 25"
lugre="100 1 0.01 0.5 0.7 10"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
<default>
<dcmotor motorconst="0.03" resistance="1.5"/>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt"/>
</body>
</worldbody>
<actuator>
<dcmotor joint="jnt" motorconst="0.03 0.05" resistance="2.0"/>
<dcmotor joint="jnt" motorconst="0.03" resistance="2.0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> 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"(
<mujoco>
unity/Runtime/Bindings/MjBindings.cs
+6 -3
View File
@@ -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,
wasm/codegen/generated/bindings.cc
+16
View File
@@ -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>("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>("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>("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);