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

Improvements related to models where joint-actuator relationship is not one-to-one:

Browse Source 
- Add `joint-actuatorforcerange` for clamping total actuator force at joints. Add `sensor-jointactuatorfrc` for sensing total actuator forces on a single joint.
See [documentation](https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping) for justification and use cases.
- Add simple car model to `model/`.
- Move actuation-related test models into `engine/testdata/actuation/`.

PiperOrigin-RevId: 549355941
Change-Id: I27f6c1f80426d73a2811ef5ae74684a228b2fbd1
This commit is contained in:
Yuval Tassa
2023-07-19 10:28:24 -07:00
committed by Copybara-Service
parent 7603b07a20
commit 51aa375af0
29 changed files with 453 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+52 -1
View File
@@ -1421,6 +1421,8 @@ if omitted.
.. _default-joint-limited:
.. _default-joint-actuatorforcelimited:
.. _default-joint-solreflimit:
.. _default-joint-solimplimit:
@@ -1433,6 +1435,8 @@ if omitted.
.. _default-joint-range:
.. _default-joint-actuatorforcerange:
.. _default-joint-margin:
.. _default-joint-ref:
@@ -3053,6 +3057,16 @@ unit quaternions.
attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, joint limits will be enabled
if range is defined.
.. _body-joint-actuatorforcelimited:
:at:`actuatorforcelimited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies whether actuator forces acting on the joint should be clamped. See :ref:`CForceRange` for
details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| This
attribute interacts with the actuatorforcerange attribute below. If this attribute is "false", actuator force
clamping is disabled. If it is "true", actuator force clamping is enabled. If this attribute is "auto", and
:at:`autolimits` is set in :ref:`compiler <compiler>`, actuator force clamping will be enabled if actuatorforcerange
is defined.
.. _body-joint-solreflimit:
.. _body-joint-solimplimit:
@@ -3084,6 +3098,14 @@ unit quaternions.
|br| Setting this attribute without specifying :at:`limited` is an error, unless :at:`autolimits` is set in
:ref:`compiler <compiler>`.
.. _body-joint-actuatorforcerange:
:at:`actuatorforcerange`: :at-val:`real(2), "0 0"`
Range for clamping total actuator forces acting on this joint. See :ref:`CForceRange` for details. It is available
only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| The compiler expects the first
value to be smaller than the second value. |br| Setting this attribute without specifying :at:`actuatorforcelimited`
is an error, unless :at:`compiler-autolimits` is set.
.. _body-joint-margin:
:at:`margin`: :at-val:`real, "0"`
@@ -5009,7 +5031,8 @@ specify them independently.
:at:`refsite`: :at-val:`string, optional`
When using a :at:`site` transmission, measure the translation and rotation w.r.t the frame of the :at:`refsite`. In
this case the actuator *does* have length and :el:`position` actuators can be used to directly control an end
effector, see `refsite.xml <https://github.com/deepmind/mujoco/tree/main/test/engine/testdata/refsite.xml>`_ example
effector, see `refsite.xml
<https://github.com/deepmind/mujoco/tree/main/test/engine/testdata/actuation/refsite.xml>`__ example
model. As above, the length is the dot product of the :at:`gear` vector and the frame difference. So ``gear="0 1 0 0
0 0"`` means "Y-offset of :at:`site` in the :at:`refsite` frame", while ``gear="0 0 0 0 0 1"`` means rotation "Z-
rotation of :at:`site` in the :at:`refsite` frame". It is recommended to use a normalized :at:`gear` vector with
@@ -6217,6 +6240,34 @@ arms determined by the transmission). This sensor can be attached to any actuato
The actuator whose scalar force output will be sensed. The sensor output is copied from mjData.actuator_force.
.. _sensor-jointactuatorfrc:
:el-prefix:`sensor/` |-| **jointactuatorfrc** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
This element creates an actuator force sensor, measured at a joint. The quantity being sensed is the
generalized force contributed by all actuators to a single scalar joint (hinge or slider). This type of sensor is
important when multiple actuators act on a single joint or when a single actuator act on multiple joints. See
:ref:`CForceRange` for details.
.. _sensor-jointactuatorfrc-name:
.. _sensor-jointactuatorfrc-noise:
.. _sensor-jointactuatorfrc-cutoff:
.. _sensor-jointactuatorfrc-user:
:at:`name`, :at:`noise`, :at:`cutoff`, :at:`user`
See :ref:`CSensor`.
.. _sensor-jointactuatorfrc-joint:
:at:`joint`: :at-val:`string, required`
The joint where actuator forces will be sensed. The sensor output is copied from ``mjData.qfrc_actuator``.
.. _sensor-ballquat:
:el-prefix:`sensor/` |-| **ballquat** (*)
doc/XMLschema.rst
+21 -10
View File
@@ -220,15 +220,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`axis<default-joint-axis>` | :ref:`springdamper<default-joint-springdamper>` | :ref:`limited<default-joint-limited>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solreflimit<default-joint-solreflimit>` | :ref:`solimplimit<default-joint-solimplimit>` | :ref:`solreffriction<default-joint-solreffriction>` | |
| | | | :ref:`actuatorforcelimited<default-joint-actuatorforcelimited>` | :ref:`solreflimit<default-joint-solreflimit>` | :ref:`solimplimit<default-joint-solimplimit>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimpfriction<default-joint-solimpfriction>` | :ref:`stiffness<default-joint-stiffness>` | :ref:`range<default-joint-range>` | |
| | | | :ref:`solreffriction<default-joint-solreffriction>` | :ref:`solimpfriction<default-joint-solimpfriction>` | :ref:`stiffness<default-joint-stiffness>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`margin<default-joint-margin>` | :ref:`ref<default-joint-ref>` | :ref:`springref<default-joint-springref>` | |
| | | | :ref:`range<default-joint-range>` | :ref:`actuatorforcerange<default-joint-actuatorforcerange>` | :ref:`margin<default-joint-margin>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`armature<default-joint-armature>` | :ref:`damping<default-joint-damping>` | :ref:`frictionloss<default-joint-frictionloss>` | |
| | | | :ref:`ref<default-joint-ref>` | :ref:`springref<default-joint-springref>` | :ref:`armature<default-joint-armature>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<default-joint-user>` | | | |
| | | | :ref:`damping<default-joint-damping>` | :ref:`frictionloss<default-joint-frictionloss>` | :ref:`user<default-joint-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
@@ -627,15 +627,17 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`group<body-joint-group>` | :ref:`pos<body-joint-pos>` | :ref:`axis<body-joint-axis>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`springdamper<body-joint-springdamper>` | :ref:`limited<body-joint-limited>` | :ref:`solreflimit<body-joint-solreflimit>` | |
| | | | :ref:`springdamper<body-joint-springdamper>` | :ref:`limited<body-joint-limited>` | :ref:`actuatorforcelimited<body-joint-actuatorforcelimited>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimplimit<body-joint-solimplimit>` | :ref:`solreffriction<body-joint-solreffriction>` | :ref:`solimpfriction<body-joint-solimpfriction>` | |
| | | | :ref:`solreflimit<body-joint-solreflimit>` | :ref:`solimplimit<body-joint-solimplimit>` | :ref:`solreffriction<body-joint-solreffriction>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`stiffness<body-joint-stiffness>` | :ref:`range<body-joint-range>` | :ref:`margin<body-joint-margin>` | |
| | | | :ref:`solimpfriction<body-joint-solimpfriction>` | :ref:`stiffness<body-joint-stiffness>` | :ref:`range<body-joint-range>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ref<body-joint-ref>` | :ref:`springref<body-joint-springref>` | :ref:`armature<body-joint-armature>` | |
| | | | :ref:`actuatorforcerange<body-joint-actuatorforcerange>` | :ref:`margin<body-joint-margin>` | :ref:`ref<body-joint-ref>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`damping<body-joint-damping>` | :ref:`frictionloss<body-joint-frictionloss>` | :ref:`user<body-joint-user>` | |
| | | | :ref:`springref<body-joint-springref>` | :ref:`armature<body-joint-armature>` | :ref:`damping<body-joint-damping>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`frictionloss<body-joint-frictionloss>` | :ref:`user<body-joint-user>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| body |br| |_| |L| | | .. table:: |
@@ -1294,6 +1296,15 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`jointactuatorfrc | \* | :class: mjcf-attributes |
| <sensor-jointactuatorfrc>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<sensor-jointactuatorfrc-name>` | :ref:`joint<sensor-jointactuatorfrc-joint>` | :ref:`cutoff<sensor-jointactuatorfrc-cutoff>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`noise<sensor-jointactuatorfrc-noise>` | :ref:`user<sensor-jointactuatorfrc-user>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| sensor |br| |_| |L| | | .. table:: |
| :ref:`ballquat | \* | :class: mjcf-attributes |
| <sensor-ballquat>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
doc/changelog.rst
+11 -1
View File
@@ -9,6 +9,11 @@ General
^^^^^^^
- Added primitive collider for sphere-cylinder contacts, previously this pair used the generic convex-convex collider.
- Added :ref:`joint-actuatorforcerange<body-joint-actuatorforcerange>` for clamping total actuator force at joints and
:ref:`sensor-jointactuatorfrc<sensor-jointactuatorfrc>` for measuring total actuation force applied at a joint. The
most important use case for joint-level actuator force clamping is to ensure that
:ref:`Cartesian actuator<actuator-general-refsite>` forces are realizable by individual motors at the joints.
See :ref:`CForceRange` for details.
- Added an optional ``content_type`` attribute to hfield, texture, and mesh assets. This attribute supports a formatted
`MIME <https://en.wikipedia.org/wiki/MIME>`_ string used to determine the type of the asset file without resorting to
pulling the type from the file extension.
@@ -26,6 +31,10 @@ Python bindings
(`#812 <https://github.com/deepmind/mujoco/issues/812>`_, `#958 <https://github.com/deepmind/mujoco/issues/958>`_,
`#965 <https://github.com/deepmind/mujoco/issues/965>`_)
Models
^^^^^^
- Added simple `car <https://github.com/deepmind/mujoco/blob/main/model/car/car.xml>`__ example model.
Version 2.3.6 (June 20, 2023)
-----------------------------
@@ -551,7 +560,8 @@ General
#. Cartesian 6D end-effector control is now possible by adding a reference site to actuators with :at:`site`
transmission. See description of new :at:`refsite` attribute in the :ref:`actuator<actuator-general>` documentation
and `refsite.xml <https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/refsite.xml>`_ example model.
and `refsite.xml <https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`_ example
model.
#. Added :at:`autolimits` compiler option. If ``true``, joint and tendon :at:`limited` attributes and actuator
:at:`ctrllimited`, :at:`forcelimited` and :at:`actlimited` attributes will automatically be set to ``true`` if the
doc/computation.rst
+2 -2
View File
@@ -293,8 +293,8 @@ is attached; the possible attachment object types are :at:`joint`, :at:`tendon`,
If a :at:`site` transmission target is defined with the optional :at:`refsite` attribute, forces and torques are
applied in the frame of the reference site rather than the site's own frame. If a reference site is defined then
the length of the actuator is nonzero and corresponds to the pose difference of the two sites. This length can then
be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the :at:`refsite`
documentation in :ref:`actuator<actuator-general>` reference for more details.
be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the
:ref:`refsite<actuator-general-refsite>` documentation for more details.
.. _geActivation:
doc/includes/references.h
+3
View File
@@ -559,6 +559,7 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_ACTUATORPOS, // scalar actuator position
mjSENS_ACTUATORVEL, // scalar actuator velocity
mjSENS_ACTUATORFRC, // scalar actuator force
mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint
// sensors related to ball joints
mjSENS_BALLQUAT, // 4D ball joint quaternion
@@ -915,12 +916,14 @@ struct mjModel_ {
int* jnt_bodyid; // id of joint's body (njnt x 1)
int* jnt_group; // group for visibility (njnt x 1)
mjtByte* jnt_limited; // does joint have limits (njnt x 1)
mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1)
mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF)
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3)
mjtNum* jnt_axis; // local joint axis (njnt x 3)
mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1)
mjtNum* jnt_range; // joint limits (njnt x 2)
mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2)
mjtNum* jnt_margin; // min distance for limit detection (njnt x 1)
mjtNum* jnt_user; // user data (njnt x nuser_jnt)
doc/modeling.rst
+45 -3
View File
@@ -591,6 +591,48 @@ defaults class and in the creation of actual model elements. If a given model re
create multiple defaults classes, or avoid using defaults for actuators and instead specify all their attributes
explicitly.
.. _CForceRange:
Actuator force clamping
~~~~~~~~~~~~~~~~~~~~~~~
Actuator forces are usually limited between lower and upper bounds. These limits can be enforced in three ways:
Control clamping with :ref:`ctrlrange<actuator-general-ctrlrange>`:
If this actuator attribute is set, the input control value will be clamped. For simple :ref:`motors<actuator-motor>`,
clamping the control input is equivalent to clamping the force output.
Force clamping at actuator output with :ref:`forcerange<actuator-general-forcerange>`:
If this actuator attribute is set, the actuator's output force will be clamped. This attribute is useful for e.g.
:ref:`position actuators<actuator-position>`, to keep the forces within bounds. Note that position actuators
usually also require control range clamping to avoid hitting joint limits.
Force clamping at joint input with :ref:`joint/actuatorforcerange<body-joint-actuatorforcerange>`:
This joint attribute clamps input forces from all actuators acting on the joint, after passing through the
:ref:`transmission<geTransmission>`. Clamping actuator forces at the joint is equivalent to clamping them at the
actuator if the transmission is trivial (there is a one-to-one relationship between the actuator and the joint).
However, in situations where multiple actuators act on one joint or one actuator acts on multiple joints---yet the
actual torque is applied by a single physical actuator at the joint---it is desirable to clamp the forces at the joint
itself. Below are three examples where it is desirable to clamp actuator forces at the joint, rather than the
actuator:
- In `this example model
<https://github.com/deepmind/mujoco/blob/main/test/engine/testdata/actuation/joint_force_clamp.xml>`__ ,
two actuators, a :ref:`motor<actuator-motor>` and a :ref:`damper<actuator-damper>`, act on a single joint.
- In `this example model <https://github.com/deepmind/mujoco/blob/main/model/car/car.xml>`__ (similar to a "Dubin's
Car"), two actuators act on two wheels via a ref:`fixed tendon<tendon-fixed>` transmission in order to apply
symmetric (roll forward/back) and antisymmetric (turn right/left) torques.
- In `this example model <https://github.com/deepmind/mujoco/tree/main/test/engine/testdata/actuation/refsite.xml>`__,
a :ref:`site transmission<actuator-general-refsite>` implements a Cartesian controller of an arm end-effector.
In order for the computed torques to be realisable by individual, torque-limited joint motors, they need to be
clamped at the joints.
Note that in this case, where forces/torques are combined by the transmission, one should use the
:ref:`jointactuatorfrc<sensor-jointactuatorfrc>` sensor to report the total actuator force acting on a joint.
The standard :ref:`actuatorfrc<sensor-actuatorfrc>` sensor will continue to report the pre-clamped actuator force.
The three clamping options above are non-exclusive and can be combined as required.
.. _CActRange:
Activation clamping
@@ -601,9 +643,9 @@ with internal dynamics whose states are called "activations". One useful applica
"integrated-velocity" actuator, implemented by the :ref:`intvelocity<actuator-intvelocity>` shortcut. Different from the
:ref:`pure velocity<actuator-velocity>` actuators, which implement direct feedback on transmission target's velocity,
*integrated-velocity* actuators couple an *integrator* with a *position-feedback* actuator. In this case the semantics
of the activation state are "the target of the position actuator", and the semantics of the control signal are "the
velocity of the target of the position actuator". Note that in real robotic systems this integrated-velocity actuator is
the most common implementation of actuators with velocity semantics, rather than pure feedback on velocity which is
of the activation state are "the setpoint of the position actuator", and the semantics of the control signal are "the
velocity of the setpoint of the position actuator". Note that in real robotic systems this integrated-velocity actuator
is the most common implementation of actuators with velocity semantics, rather than pure feedback on velocity which is
often quite unstable (both in real life and in simulation).
In the case of integrated-velocity actuators, it is often desirable to *clamp* the activation state, since otherwise the
include/mujoco/mjmodel.h
+3
View File
@@ -284,6 +284,7 @@ typedef enum mjtSensor_ { // type of sensor
mjSENS_ACTUATORPOS, // scalar actuator position
mjSENS_ACTUATORVEL, // scalar actuator velocity
mjSENS_ACTUATORFRC, // scalar actuator force
mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint
// sensors related to ball joints
mjSENS_BALLQUAT, // 4D ball joint quaternion
@@ -674,12 +675,14 @@ struct mjModel_ {
int* jnt_bodyid; // id of joint's body (njnt x 1)
int* jnt_group; // group for visibility (njnt x 1)
mjtByte* jnt_limited; // does joint have limits (njnt x 1)
mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1)
mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF)
mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP)
mjtNum* jnt_pos; // local anchor position (njnt x 3)
mjtNum* jnt_axis; // local joint axis (njnt x 3)
mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1)
mjtNum* jnt_range; // joint limits (njnt x 2)
mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2)
mjtNum* jnt_margin; // min distance for limit detection (njnt x 1)
mjtNum* jnt_user; // user data (njnt x nuser_jnt)
include/mujoco/mjxmacro.h
+2
View File
@@ -191,12 +191,14 @@
XMJV( int, jnt_bodyid, njnt, 1 ) \
XMJV( int, jnt_group, njnt, 1 ) \
X ( mjtByte, jnt_limited, njnt, 1 ) \
X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \
X ( mjtNum, jnt_solref, njnt, mjNREF ) \
X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \
X ( mjtNum, jnt_pos, njnt, 3 ) \
X ( mjtNum, jnt_axis, njnt, 3 ) \
X ( mjtNum, jnt_stiffness, njnt, 1 ) \
X ( mjtNum, jnt_range, njnt, 2 ) \
X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \
X ( mjtNum, jnt_margin, njnt, 1 ) \
X ( mjtNum, jnt_user, njnt, MJ_M(nuser_jnt) ) \
X ( int, dof_bodyid, nv, 1 ) \
introspect/enums.py
+24 -23
View File
@@ -315,29 +315,30 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjSENS_ACTUATORPOS', 12),
('mjSENS_ACTUATORVEL', 13),
('mjSENS_ACTUATORFRC', 14),
('mjSENS_BALLQUAT', 15),
('mjSENS_BALLANGVEL', 16),
('mjSENS_JOINTLIMITPOS', 17),
('mjSENS_JOINTLIMITVEL', 18),
('mjSENS_JOINTLIMITFRC', 19),
('mjSENS_TENDONLIMITPOS', 20),
('mjSENS_TENDONLIMITVEL', 21),
('mjSENS_TENDONLIMITFRC', 22),
('mjSENS_FRAMEPOS', 23),
('mjSENS_FRAMEQUAT', 24),
('mjSENS_FRAMEXAXIS', 25),
('mjSENS_FRAMEYAXIS', 26),
('mjSENS_FRAMEZAXIS', 27),
('mjSENS_FRAMELINVEL', 28),
('mjSENS_FRAMEANGVEL', 29),
('mjSENS_FRAMELINACC', 30),
('mjSENS_FRAMEANGACC', 31),
('mjSENS_SUBTREECOM', 32),
('mjSENS_SUBTREELINVEL', 33),
('mjSENS_SUBTREEANGMOM', 34),
('mjSENS_CLOCK', 35),
('mjSENS_PLUGIN', 36),
('mjSENS_USER', 37),
('mjSENS_JOINTACTFRC', 15),
('mjSENS_BALLQUAT', 16),
('mjSENS_BALLANGVEL', 17),
('mjSENS_JOINTLIMITPOS', 18),
('mjSENS_JOINTLIMITVEL', 19),
('mjSENS_JOINTLIMITFRC', 20),
('mjSENS_TENDONLIMITPOS', 21),
('mjSENS_TENDONLIMITVEL', 22),
('mjSENS_TENDONLIMITFRC', 23),
('mjSENS_FRAMEPOS', 24),
('mjSENS_FRAMEQUAT', 25),
('mjSENS_FRAMEXAXIS', 26),
('mjSENS_FRAMEYAXIS', 27),
('mjSENS_FRAMEZAXIS', 28),
('mjSENS_FRAMELINVEL', 29),
('mjSENS_FRAMEANGVEL', 30),
('mjSENS_FRAMELINACC', 31),
('mjSENS_FRAMEANGACC', 32),
('mjSENS_SUBTREECOM', 33),
('mjSENS_SUBTREELINVEL', 34),
('mjSENS_SUBTREEANGMOM', 35),
('mjSENS_CLOCK', 36),
('mjSENS_PLUGIN', 37),
('mjSENS_USER', 38),
]),
)),
('mjtStage',
introspect/structs.py
+14
View File
@@ -1391,6 +1391,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='does joint have limits (njnt x 1)',
),
StructFieldDecl(
name='jnt_actfrclimited',
type=PointerType(
inner_type=ValueType(name='mjtByte'),
),
doc='does joint have actuator force limits (njnt x 1)',
),
StructFieldDecl(
name='jnt_solref',
type=PointerType(
@@ -1433,6 +1440,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='joint limits (njnt x 2)',
),
StructFieldDecl(
name='jnt_actfrcrange',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='range of total actuator force (njnt x 2)',
),
StructFieldDecl(
name='jnt_margin',
type=PointerType(
model/car/car.xml
+72
View File
@@ -0,0 +1,72 @@
<mujoco>
<compiler autolimits="true"/>
<asset>
<texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
<mesh name="chasis" scale=".01 .006 .0015"
vertex=" 9 2 0
-10 10 10
9 -2 0
10 3 -10
10 -3 -10
-8 10 -10
-10 -10 10
-8 -10 -10
-5 0 20"/>
</asset>
<default>
<joint damping=".03" actuatorforcerange="-0.5 0.5"/>
<default class="wheel">
<geom type="cylinder" size=".03 .01" rgba=".5 .5 1 1"/>
</default>
<default class="decor">
<site type="box" rgba=".5 1 .5 1"/>
</default>
</default>
<worldbody>
<geom type="plane" size="3 3 .01" material="grid"/>
<body name="car" pos="0 0 .03">
<freejoint/>
<light name="top light" pos="0 0 2" mode="trackcom" diffuse=".4 .4 .4"/>
<geom name="chasis" type="mesh" mesh="chasis"/>
<geom name="front wheel" pos=".08 0 -.015" type="sphere" size=".015" condim="1" priority="1"/>
<light name="front light" pos=".1 0 .02" dir="2 0 -1" diffuse="1 1 1"/>
<body name="left wheel" pos="-.07 .06 0" zaxis="0 1 0">
<joint name="left"/>
<geom class="wheel"/>
<site class="decor" size=".006 .025 .012"/>
<site class="decor" size=".025 .006 .012"/>
</body>
<body name="right wheel" pos="-.07 -.06 0" zaxis="0 1 0">
<joint name="right"/>
<geom class="wheel"/>
<site class="decor" size=".006 .025 .012"/>
<site class="decor" size=".025 .006 .012"/>
</body>
</body>
</worldbody>
<tendon>
<fixed name="forward">
<joint joint="left" coef=".5"/>
<joint joint="right" coef=".5"/>
</fixed>
<fixed name="turn">
<joint joint="left" coef="-.5"/>
<joint joint="right" coef=".5"/>
</fixed>
</tendon>
<actuator>
<motor name="forward" tendon="forward" ctrlrange="-1 1"/>
<motor name="turn" tendon="turn" ctrlrange="-1 1"/>
</actuator>
<sensor>
<jointactuatorfrc name="right" joint="right"/>
<jointactuatorfrc name="left" joint="left"/>
</sensor>
</mujoco>
src/engine/engine_forward.c
+10
View File
@@ -301,6 +301,16 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// qfrc_actuator = moment' * force
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// clamp qfrc_actuator
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
if (m->jnt_actfrclimited[i]) {
mjtNum *forcerange = m->jnt_actfrcrange + 2*i;
mjtNum *qfrc = d->qfrc_actuator + m->jnt_dofadr[i];
qfrc[0] = mju_clip(qfrc[0], forcerange[0], forcerange[1]);
}
}
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
if (m->actuator_plugin[i] >= 0) {
src/engine/engine_io.c
+1
View File
@@ -1471,6 +1471,7 @@ static int sensorSize(mjtSensor sensor_type, int sensor_dim) {
case mjSENS_ACTUATORPOS:
case mjSENS_ACTUATORVEL:
case mjSENS_ACTUATORFRC:
case mjSENS_JOINTACTFRC:
case mjSENS_JOINTLIMITPOS:
case mjSENS_JOINTLIMITVEL:
case mjSENS_JOINTLIMITFRC:
src/engine/engine_sensor.c
+5
View File
@@ -593,6 +593,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
if (rnePost == 0 &&
type != mjSENS_TOUCH &&
type != mjSENS_ACTUATORFRC &&
type != mjSENS_JOINTACTFRC &&
type != mjSENS_JOINTLIMITFRC &&
type != mjSENS_TENDONLIMITFRC) {
// compute cacc, cfrc_int, cfrc_ext
@@ -686,6 +687,10 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
d->sensordata[adr] = d->actuator_force[objid];
break;
case mjSENS_JOINTACTFRC: // jointactfrc
d->sensordata[adr] = d->qfrc_actuator[m->jnt_dofadr[objid]];
break;
case mjSENS_JOINTLIMITFRC: // jointlimitfrc
d->sensordata[adr] = 0;
for (int j=ne+nf; j < nefc; j++) {
src/user/user_model.cc
+2
View File
@@ -1407,6 +1407,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_type[jid] = pj->type;
m->jnt_group[jid] = pj->group;
m->jnt_limited[jid] = pj->limited;
m->jnt_actfrclimited[jid] = pj->actfrclimited;
m->jnt_qposadr[jid] = qposadr;
m->jnt_dofadr[jid] = dofadr;
m->jnt_bodyid[jid] = pj->body->id;
@@ -1414,6 +1415,7 @@ void mjCModel::CopyTree(mjModel* m) {
copyvec(m->jnt_axis+3*jid, pj->locaxis, 3);
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness;
copyvec(m->jnt_range+2*jid, pj->range, 2);
copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF);
copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP);
m->jnt_margin[jid] = (mjtNum)pj->margin;
src/user/user_objects.cc
+28 -1
View File
@@ -1059,9 +1059,12 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) {
mjuu_setvec(pos, 0, 0, 0);
mjuu_setvec(axis, 0, 0, 1);
limited = 2;
actfrclimited = 2;
stiffness = 0;
range[0] = 0;
range[1] = 0;
actfrcrange[0] = 0;
actfrcrange[1] = 0;
springdamper[0] = 0;
springdamper[1] = 0;
mj_defaultSolRefImp(solref_limit, solimp_limit);
@@ -1146,6 +1149,27 @@ int mjCJoint::Compile(void) {
}
}
// actuator force range: none for free or ball joints
if (type==mjJNT_FREE || type==mjJNT_BALL) {
actfrclimited = 0;
}
// otherwise if actfrclimited is auto, set according to whether actfrcrange is specified
else if (actfrclimited==2) {
bool hasrange = !(actfrcrange[0]==0 && actfrcrange[1]==0);
checklimited(this, model->autolimits, "joint", "", actfrclimited, hasrange);
actfrclimited = hasrange ? 1 : 0;
}
// resolve actuator force range limits
if (actfrclimited) {
// check data
if (actfrcrange[0]>=actfrcrange[1]) {
throw mjCError(this,
"actfrcrange[0] should be smaller than actfrcrange[1] in joint '%s' (id = %d)",
name.c_str(), id);
}
}
// FREE or BALL: set axis to (0,0,1)
if (type==mjJNT_FREE || type==mjJNT_BALL) {
axis[0] = axis[1] = 0;
@@ -4024,6 +4048,7 @@ void mjCSensor::Compile(void) {
case mjSENS_JOINTPOS:
case mjSENS_JOINTVEL:
case mjSENS_JOINTACTFRC:
// must be attached to joint
if (objtype!=mjOBJ_JOINT) {
throw mjCError(this,
@@ -4041,8 +4066,10 @@ void mjCSensor::Compile(void) {
datatype = mjDATATYPE_REAL;
if (type==mjSENS_JOINTPOS) {
needstage = mjSTAGE_POS;
} else {
} else if (type==mjSENS_JOINTVEL) {
needstage = mjSTAGE_VEL;
} else if (type==mjSENS_JOINTACTFRC) {
needstage = mjSTAGE_ACC;
}
break;
src/user/user_objects.h
+2
View File
@@ -296,11 +296,13 @@ class mjCJoint : public mjCBase {
mjtJoint type; // type of Joint
int group; // used for rendering
int limited; // does joint have limits: 0 false, 1 true, 2 auto
int actfrclimited; // are actuator forces on joints limited: 0 false, 1 true, 2 auto
double pos[3]; // anchor position
double axis[3]; // joint axis
double stiffness; // stiffness coefficient
double springdamper[2]; // timeconst, dampratio
double range[2]; // joint limits
double actfrcrange[2]; // actuator force limits
mjtNum solref_limit[mjNREF]; // solver reference: joint limits
mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits
mjtNum solref_friction[mjNREF]; // solver reference: dof friction
src/xml/xml_native_reader.cc
+16 -9
View File
@@ -79,7 +79,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 190;
static const int nMJCF = 191;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -138,10 +138,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mesh", "?", "1", "scale"},
{"material", "?", "8", "texture", "emission", "specular", "shininess",
"reflectance", "rgba", "texrepeat", "texuniform"},
{"joint", "?", "19", "type", "group", "pos", "axis", "springdamper",
"limited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "margin",
"ref", "springref", "armature", "damping", "frictionloss", "user"},
{"joint", "?", "21", "type", "group", "pos", "axis", "springdamper",
"limited", "actuatorforcelimited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "actuatorforcerange",
"margin", "ref", "springref", "armature", "damping", "frictionloss", "user"},
{"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
"shellinertia", "solmix", "solref", "solimp",
@@ -237,11 +237,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{">"},
{"inertial", "?", "9", "pos", "quat", "mass", "diaginertia",
"axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
{"joint", "*", "21", "name", "class", "type", "group", "pos", "axis",
"springdamper", "limited",
{"joint", "*", "23", "name", "class", "type", "group", "pos", "axis",
"springdamper", "limited", "actuatorforcelimited",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"stiffness", "range", "margin", "ref", "springref", "armature", "damping",
"frictionloss", "user"},
"stiffness", "range", "actuatorforcerange", "margin", "ref", "springref",
"armature", "damping", "frictionloss", "user"},
{"freejoint", "*", "2", "name", "group"},
{"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim",
"group", "priority", "size", "material", "friction", "mass", "density",
@@ -391,6 +391,7 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuatorpos", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorvel", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorfrc", "*", "5", "name", "actuator", "cutoff", "noise", "user"},
{"jointactuatorfrc", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"ballquat", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"ballangvel", "*", "5", "name", "joint", "cutoff", "noise", "user"},
{"jointlimitpos", "*", "5", "name", "joint", "cutoff", "noise", "user"},
@@ -1314,6 +1315,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) {
pjoint->type = (mjtJoint)n;
}
MapValue(elem, "limited", &pjoint->limited, TFAuto_map, 3);
MapValue(elem, "actuatorforcelimited", &pjoint->actfrclimited, TFAuto_map, 3);
ReadAttrInt(elem, "group", &pjoint->group);
ReadAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, text, false, false);
ReadAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, text, false, false);
@@ -1324,6 +1326,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjCJoint* pjoint) {
ReadAttr(elem, "springdamper", 2, pjoint->springdamper, text);
ReadAttr(elem, "stiffness", 1, &pjoint->stiffness, text);
ReadAttr(elem, "range", 2, pjoint->range, text);
ReadAttr(elem, "actuatorforcerange", 2, pjoint->actfrcrange, text);
ReadAttr(elem, "margin", 1, &pjoint->margin, text);
ReadAttr(elem, "ref", 1, &pjoint->ref, text);
ReadAttr(elem, "springref", 1, &pjoint->springref, text);
@@ -3004,6 +3007,10 @@ void mjXReader::Sensor(XMLElement* section) {
psen->type = mjSENS_ACTUATORFRC;
psen->objtype = mjOBJ_ACTUATOR;
ReadAttrTxt(elem, "actuator", psen->objname, true);
} else if (type=="jointactuatorfrc") {
psen->type = mjSENS_JOINTACTFRC;
psen->objtype = mjOBJ_JOINT;
ReadAttrTxt(elem, "joint", psen->objname, true);
}
// sensors related to ball joints
src/xml/xml_native_writer.cc
+11
View File
@@ -225,6 +225,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
}
bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0;
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==afrange_defined;
if (writingdefaults || !aflimited_inferred) {
WriteAttrKey(elem, "actutorforcelimited", TFAuto_map, 3,
pjoint->actfrclimited, def->joint.actfrclimited);
}
// defaults and regular
if (pjoint->type != def->joint.type) {
@@ -239,6 +245,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->joint.solimp_friction);
WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->joint.stiffness);
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
WriteAttr(elem, "actuatorforcerange", 2, pjoint->actfrcrange, def->joint.actfrcrange);
WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin);
WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature);
WriteAttr(elem, "damping", 1, &pjoint->damping, &def->joint.damping);
@@ -1645,6 +1652,10 @@ void mjXWriter::Sensor(XMLElement* root) {
elem = InsertEnd(section, "actuatorfrc");
WriteAttrTxt(elem, "actuator", psen->objname);
break;
case mjSENS_JOINTACTFRC:
elem = InsertEnd(section, "jointactuatorfrc");
WriteAttrTxt(elem, "joint", psen->objname);
break;
// sensors related to ball joints
case mjSENS_BALLQUAT:
test/engine/engine_core_smooth_test.cc
+6 -6
View File
@@ -252,7 +252,7 @@ TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) {
// Test Cartesian position control using site transmission with refsite
TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
constexpr char kRefsitePath[] = "engine/testdata/refsite.xml";
constexpr char kRefsitePath[] = "engine/testdata/actuation/refsite.xml";
const std::string xml_path = GetTestDataFilePath(kRefsitePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
ASSERT_THAT(model, NotNull());
@@ -265,7 +265,7 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
mju_copy3(data->ctrl+3, targetrot);
// step for 5 seconds
while (data->time < 5) {
while (data->time < 10) {
mj_step(model, data);
}
@@ -273,14 +273,14 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
int refsite_id = mj_name2id(model, mjOBJ_SITE, "reference");
int site_id = mj_name2id(model, mjOBJ_SITE, "end_effector");
// check that position matches target to within 1e-5 length units
double tol_pos = 1e-5;
// check that position matches target to within 1e-3 length units
double tol_pos = 1e-3;
mjtNum relpos[3];
mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id);
EXPECT_THAT(relpos, Pointwise(DoubleNear(tol_pos), targetpos));
// check that orientation matches target to within 1e-3 radians
double tol_rot = 1e-3;
// check that orientation matches target to within 0.06 radians
double tol_rot = 0.06;
mjtNum site_xquat[4], refsite_xquat[4], relrot[3];
mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id);
mju_mat2Quat(site_xquat, data->site_xmat+9*site_id);
test/engine/engine_derivative_test.cc
+1 -1
View File
@@ -97,7 +97,7 @@ static const char* const kTumblingThinObjectEllipsoidPath =
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kDamperActuatorsPath =
"engine/testdata/damper.xml";
"engine/testdata/actuation/damper.xml";
static const char* const kDampedPendulumPath =
"engine/testdata/derivative/damped_pendulum.xml";
static const char* const kLinearPath =
test/engine/engine_forward_test.cc
+50 -18
View File
@@ -38,6 +38,8 @@ static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kJointForceClamp =
"engine/testdata/actuation/joint_force_clamp.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
@@ -79,7 +81,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) {
data->ctrl[0] = 1.0;
// integrating up from 0, we will hit the clamp after 99 steps
for (int i=0; i<200; i++) {
for (int i=0; i < 200; i++) {
mj_step(model, data);
// always greater than lower bound
EXPECT_GT(data->act[0], -1);
@@ -90,7 +92,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) {
data->ctrl[0] = -1.0;
// integrating down from 1, we will hit the clamp after 199 steps
for (int i=0; i<300; i++) {
for (int i=0; i < 300; i++) {
mj_step(model, data);
// always smaller than upper bound
EXPECT_LT(data->act[0], model->actuator_actrange[1]);
@@ -139,13 +141,13 @@ TEST_F(ForwardTest, DamperDampens) {
// move the joint
data->ctrl[0] = 100.0;
data->ctrl[1] = 0.0;
for (int i=0; i<100; i++)
for (int i=0; i < 100; i++)
mj_step(model, data);
// stop the joint with damping
data->ctrl[0] = 0.0;
data->ctrl[1] = 100.0;
for (int i=0; i<1000; i++)
for (int i=0; i < 1000; i++)
mj_step(model, data);
EXPECT_LE(data->qvel[0], std::numeric_limits<double>::epsilon());
@@ -178,7 +180,7 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
mjData* data = mj_makeData(model);
// step 10 times with Euler, save copy of qpos as vector
for (int i=0; i<10; i++) {
for (int i=0; i < 10; i++) {
mj_step(model, data);
}
std::vector<mjtNum> qposEuler = AsVector(data->qpos, model->nq);
@@ -186,7 +188,7 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
// reset, step 10 times with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i=0; i<10; i++) {
for (int i=0; i < 10; i++) {
mj_step(model, data);
}
@@ -208,7 +210,7 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
mjData* data = mj_makeData(model);
// take 1000 steps with Euler
for (int i=0; i<1000; i++) {
for (int i=0; i < 1000; i++) {
mj_step(model, data);
}
// expect corresponding joint values to be significantly different
@@ -218,7 +220,7 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
// reset, take 1000 steps with implicit
mj_resetData(model, data);
model->opt.integrator = mjINT_IMPLICIT;
for (int i=0; i<10; i++) {
for (int i=0; i < 10; i++) {
mj_step(model, data);
}
@@ -241,7 +243,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) {
// take nstep steps with Euler, measure energy (potential + kinetic)
model->opt.integrator = mjINT_EULER;
for (int i=0; i<nstep; i++) {
for (int i=0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyEuler = data->energy[0] + data->energy[1];
@@ -249,7 +251,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) {
// take nstep steps with implicit, measure energy
model->opt.integrator = mjINT_IMPLICIT;
mj_resetData(model, data);
for (int i=0; i<nstep; i++) {
for (int i=0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyImplicit = data->energy[0] + data->energy[1];
@@ -257,7 +259,7 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) {
// take nstep steps with 4th order Runge-Kutta, measure energy
model->opt.integrator = mjINT_RK4;
mj_resetData(model, data);
for (int i=0; i<nstep; i++) {
for (int i=0; i < nstep; i++) {
mj_step(model, data);
}
mjtNum energyRK4 = data->energy[0] + data->energy[1];
@@ -326,7 +328,8 @@ TEST_F(ForwardTest, ControlClamping) {
// for the unclamped actuator, huge raises warning
data->ctrl[0] = 10*mjMAXVAL;
mj_forward(model, data);
EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0"));
EXPECT_THAT(warning,
HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0"));
// for the clamped actuator, huge does not raise warning
mj_resetData(model, data);
@@ -339,7 +342,8 @@ TEST_F(ForwardTest, ControlClamping) {
mj_resetData(model, data);
data->ctrl[1] = std::numeric_limits<double>::quiet_NaN();
mj_forward(model, data);
EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1"));
EXPECT_THAT(warning,
HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1"));
mj_deleteData(data);
mj_deleteModel(model);
@@ -412,11 +416,11 @@ TEST_F(ForwardTest, gravcomp) {
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
while(data->time < 1) { mj_step(model, data); }
while (data->time < 1) { mj_step(model, data); }
mjtNum dist = 0.5*mju_norm3(model->opt.gravity)*(data->time*data->time);
// expect that body 1 moves down allowing some slack from our estimated distance moved
// expect that body 1 moved down, allowing some slack from our estimate
EXPECT_NEAR(data->qpos[0], -dist, 0.011);
// expect that body 2 does not move
@@ -493,11 +497,11 @@ TEST_F(ForwardTest, MjcbActDynSecondOrderExpectsActnum) {
mj_deleteModel(model);
}
// -------------------------- adhesion actuators -------------------------------
// ------------------------------ actuators -----------------------------------
using AdhesionTest = MujocoTest;
using ActuatorTest = MujocoTest;
TEST_F(AdhesionTest, ExpectedAdhesionForce) {
TEST_F(ActuatorTest, ExpectedAdhesionForce) {
static constexpr char xml[] = R"(
<mujoco>
<option gravity="0 0 -1"/>
@@ -561,5 +565,33 @@ TEST_F(AdhesionTest, ExpectedAdhesionForce) {
mj_deleteModel(model);
}
// Actuator force clamping at joints
TEST_F(ActuatorTest, ActuatorForceClamping) {
const std::string xml_path = GetTestDataFilePath(kJointForceClamp);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
data->ctrl[0] = 10;
mj_forward(model, data);
// expect clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 1);
EXPECT_EQ(data->qfrc_actuator[0], 0.4);
// simulate for 2 seconds to gain velocity
while (data->time < 2) {
mj_step(model, data);
}
// activate damper, expect force to be clamped at lower bound
data->ctrl[1] = 1;
mj_forward(model, data);
EXPECT_EQ(data->qfrc_actuator[0], -0.4);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_vfs_test.cc
+1 -1
View File
@@ -26,7 +26,7 @@ using ::testing::NotNull;
using EngineVfsTest = MujocoTest;
TEST_F(EngineVfsTest, AddFileVFS) {
constexpr char path[] = "engine/testdata/";
constexpr char path[] = "engine/testdata/actuation/";
const std::string dir = GetTestDataFilePath(path);
std::string file1 = "activation.xml";
std::string file2 = "damper.xml";
test/engine/testdata/activation.xml → test/engine/testdata/actuation/activation.xml
Vendored
View File
test/engine/testdata/damper.xml → test/engine/testdata/actuation/damper.xml
Vendored
+4 -1
View File
@@ -1,5 +1,8 @@
<mujoco>
<compiler autolimits="true" />
<compiler autolimits="true"/>
<option integrator="implicitfast"/>
<worldbody>
<geom type="plane" size="1 1 .01"/>
<light pos="0 0 2"/>
test/engine/testdata/actuation/joint_force_clamp.xml
Vendored
+26
View File
@@ -0,0 +1,26 @@
<mujoco>
<compiler autolimits="true"/>
<option integrator="implicitfast"/>
<worldbody>
<geom type="plane" size="1 1 .01"/>
<light pos="0 0 2"/>
<body pos="0 0 .3">
<joint name="hinge" damping=".01" actuatorforcerange="-.4 .4"/>
<geom type="capsule" size=".01" fromto="0 0 0 .2 0 0"/>
<geom size=".03" pos=".2 0 0"/>
</body>
</worldbody>
<actuator>
<motor name="motor" joint="hinge" ctrlrange="-1 1"/>
<damper name="damper" joint="hinge" kv="10" ctrlrange="0 1"/>
</actuator>
<sensor>
<actuatorfrc name="motor" actuator="motor"/>
<actuatorfrc name="damper" actuator="damper"/>
<jointactuatorfrc name="hinge" joint="hinge"/>
</sensor>
</mujoco>
test/engine/testdata/refsite.xml → test/engine/testdata/actuation/refsite.xml
Vendored
+15 -5
View File
@@ -24,7 +24,12 @@
-->
<general gainprm=".5" biasprm="0 -.5 -.05" biastype="affine" ctrlrange="-1.571 1.571"/>
</default>
<joint damping="1e-4"/>
<!--
Clamping the total actuator torque at the joints means that the motion produced by the Cartesian
commands is achievable by individual joint actuators with the specified torque limits.
See https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping
-->
<joint stiffness="1e-1" actuatorforcerange="-1 1"/>
<site type="box" size=".012 .012 .012" rgba=".7 .7 .8 1"/>
</default>
@@ -33,16 +38,21 @@
<geom type="box" size=".25 .25 .01" pos="0 0 -.01"/>
<site name="reference" pos="0 0 .25"/>
<body name="arm" pos="-.25 .25 0">
<joint type="ball"/>
<joint axis="1 0 0"/>
<joint axis="0 1 0"/>
<geom type="box" size=".01" fromto="0 0 0 0 0 .25"/>
<body pos="0 0 .25">
<joint type="ball"/>
<joint axis="0 1 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 .25 0 0"/>
<body pos=".25 0 0">
<joint type="ball"/>
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 0 -.2 0"/>
<body pos="0 -.2 0">
<joint type="ball"/>
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<joint axis="0 1 0"/>
<geom type="box" size=".01" fromto="0 0 0 0 -.05 0"/>
<site name="end_effector" pos="0 -.05 0"/>
</body>
test/engine/testdata/sites.xml → test/engine/testdata/actuation/sites.xml
Vendored
View File
unity/Runtime/Bindings/MjBindings.cs
+26 -23
View File
@@ -325,29 +325,30 @@ public enum mjtSensor : int{
mjSENS_ACTUATORPOS = 12,
mjSENS_ACTUATORVEL = 13,
mjSENS_ACTUATORFRC = 14,
mjSENS_BALLQUAT = 15,
mjSENS_BALLANGVEL = 16,
mjSENS_JOINTLIMITPOS = 17,
mjSENS_JOINTLIMITVEL = 18,
mjSENS_JOINTLIMITFRC = 19,
mjSENS_TENDONLIMITPOS = 20,
mjSENS_TENDONLIMITVEL = 21,
mjSENS_TENDONLIMITFRC = 22,
mjSENS_FRAMEPOS = 23,
mjSENS_FRAMEQUAT = 24,
mjSENS_FRAMEXAXIS = 25,
mjSENS_FRAMEYAXIS = 26,
mjSENS_FRAMEZAXIS = 27,
mjSENS_FRAMELINVEL = 28,
mjSENS_FRAMEANGVEL = 29,
mjSENS_FRAMELINACC = 30,
mjSENS_FRAMEANGACC = 31,
mjSENS_SUBTREECOM = 32,
mjSENS_SUBTREELINVEL = 33,
mjSENS_SUBTREEANGMOM = 34,
mjSENS_CLOCK = 35,
mjSENS_PLUGIN = 36,
mjSENS_USER = 37,
mjSENS_JOINTACTFRC = 15,
mjSENS_BALLQUAT = 16,
mjSENS_BALLANGVEL = 17,
mjSENS_JOINTLIMITPOS = 18,
mjSENS_JOINTLIMITVEL = 19,
mjSENS_JOINTLIMITFRC = 20,
mjSENS_TENDONLIMITPOS = 21,
mjSENS_TENDONLIMITVEL = 22,
mjSENS_TENDONLIMITFRC = 23,
mjSENS_FRAMEPOS = 24,
mjSENS_FRAMEQUAT = 25,
mjSENS_FRAMEXAXIS = 26,
mjSENS_FRAMEYAXIS = 27,
mjSENS_FRAMEZAXIS = 28,
mjSENS_FRAMELINVEL = 29,
mjSENS_FRAMEANGVEL = 30,
mjSENS_FRAMELINACC = 31,
mjSENS_FRAMEANGACC = 32,
mjSENS_SUBTREECOM = 33,
mjSENS_SUBTREELINVEL = 34,
mjSENS_SUBTREEANGMOM = 35,
mjSENS_CLOCK = 36,
mjSENS_PLUGIN = 37,
mjSENS_USER = 38,
}
public enum mjtStage : int{
mjSTAGE_NONE = 0,
@@ -1987,12 +1988,14 @@ public unsafe struct mjModel_ {
public int* jnt_bodyid;
public int* jnt_group;
public byte* jnt_limited;
public byte* jnt_actfrclimited;
public double* jnt_solref;
public double* jnt_solimp;
public double* jnt_pos;
public double* jnt_axis;
public double* jnt_stiffness;
public double* jnt_range;
public double* jnt_actfrcrange;
public double* jnt_margin;
public double* jnt_user;
public int* dof_bodyid;