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

Add actuatorgravcomp joint attribute, to treat gravity compensation forces as applied by actuators, rather than passive buoyancy.

Browse Source 
PiperOrigin-RevId: 620049593
Change-Id: I2c8a9dc152c087b408e4f904838034271a7dd910
This commit is contained in:
Yuval Tassa
2024-03-28 14:00:55 -07:00
committed by Copybara-Service
parent d258d5e152
commit 47ba72ea59
16 changed files with 257 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+41 -27
View File
@@ -2354,24 +2354,6 @@ rotations as unit quaternions.
joint inertia in the model reference configuration. Note that the format is the same as the solref parameter of the
constraint solver.
.. _body-joint-limited:
:at:`limited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies if the joint has limits. It interacts with the range attribute below. If this attribute
is "false", joint limits are disabled. If this attribute is "true", joint limits are enabled. If this
attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, joint limits will be enabled
if range is defined.
.. _body-joint-actuatorfrclimited:
:at:`actuatorfrclimited`: :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 actuatorfrcrange 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 actuatorfrcrange
is defined.
.. _body-joint-solreflimit:
.. _body-joint-solimplimit:
@@ -2400,16 +2382,44 @@ rotations as unit quaternions.
joints, the limit is imposed on the angle of rotation (relative to the reference configuration) regardless of the
axis of rotation. Only the second range parameter is used for ball joints; the first range parameter should be set to
0. See the :ref:`Limit <coLimit>` section in the Computation chapter for more information.
|br| Setting this attribute without specifying :at:`limited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`limited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _body-joint-limited:
:at:`limited`: :at-val:`[false, true, auto], "auto"`
This attribute specifies if the joint has limits. It interacts with the range attribute below. If this attribute
is "false", joint limits are disabled. If this attribute is "true", joint limits are enabled. If this
attribute is "auto", and :at:`autolimits` is set in :ref:`compiler <compiler>`, joint limits will be enabled
if range is defined.
.. _body-joint-actuatorfrcrange:
:at:`actuatorfrcrange`: :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:`actuatorfrclimited`
is an error, unless :at:`compiler-autolimits` is set.
is an error if :at:`compiler-autolimits` is "false".
.. _body-joint-actuatorfrclimited:
:at:`actuatorfrclimited`: :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 actuatorfrcrange 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
:at:`actuatorfrcrange` is defined.
.. _body-joint-actuatorgravcomp:
:at:`actuatorgravcomp`: :at-val:`[false, true], "false"`
If this flag is enabled, gravity compensation applied to this joint is added to actuator forces
(``mjData.qfrc_actuator``) rather than passive forces (``mjData.qfrc_passive``). Notionally, this means that gravity
compensation is the result of a control system rather than natural buoyancy. In practice, enabling this flag is
useful when joint-level actuator force clamping is used. In this case, the total actuation force applied on a joint,
including gravity compensation, is guaranteed to not exceeed the specified limits. See :ref:`CForceRange` and
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` for more details on this type of force limit.
.. _body-joint-margin:
@@ -5096,14 +5106,14 @@ specify them independently.
:at:`ctrlrange`: :at-val:`real(2), "0 0"`
Range for clamping the control input. The first value must be smaller than the second value.
|br| Setting this attribute without specifying :at:`ctrllimited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`ctrllimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _actuator-general-forcerange:
:at:`forcerange`: :at-val:`real(2), "0 0"`
Range for clamping the force output. The first value must be no greater than the second value.
|br| Setting this attribute without specifying :at:`forcelimited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`forcelimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _actuator-general-actrange:
@@ -5111,7 +5121,7 @@ specify them independently.
:at:`actrange`: :at-val:`real(2), "0 0"`
Range for clamping the activation state. The first value must be no greater than the second value.
See the :ref:`Activation clamping <CActRange>` section for more details.
|br| Setting this attribute without specifying :at:`actlimited` is an error, unless :at:`autolimits` is set in
|br| Setting this attribute without specifying :at:`actlimited` is an error if :at:`autolimits` is "false" in
:ref:`compiler <compiler>`.
.. _actuator-general-lengthrange:
@@ -6489,10 +6499,12 @@ arms determined by the transmission). This sensor can be attached to any actuato
: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.
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). If the joint's
:ref:`actuatorgravcomp<body-joint-actuatorgravcomp>` attribute is "true", this sensor will also measure contributions by
gravity compensation forces (which are added directly to the joint and would *not* register in the
:ref:`actuatorfrc<sensor-actuatorfrc>`) sensor. 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:
@@ -7386,6 +7398,8 @@ if omitted.
.. _default-joint-actuatorfrclimited:
.. _default-joint-actuatorgravcomp:
.. _default-joint-solreflimit:
.. _default-joint-solimplimit:
doc/XMLschema.rst
+5 -5
View File
@@ -286,9 +286,9 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimpfriction<body-joint-solimpfriction>` | :ref:`stiffness<body-joint-stiffness>` | :ref:`range<body-joint-range>` | :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`margin<body-joint-margin>` | :ref:`ref<body-joint-ref>` | :ref:`springref<body-joint-springref>` | :ref:`armature<body-joint-armature>` | |
| | | | :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>` | :ref:`margin<body-joint-margin>` | :ref:`ref<body-joint-ref>` | :ref:`springref<body-joint-springref>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`damping<body-joint-damping>` | :ref:`frictionloss<body-joint-frictionloss>` | :ref:`user<body-joint-user>` | | |
| | | | :ref:`armature<body-joint-armature>` | :ref:`damping<body-joint-damping>` | :ref:`frictionloss<body-joint-frictionloss>` | :ref:`user<body-joint-user>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| body |br| |_| |L| | | .. table:: |
@@ -1320,11 +1320,11 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solimplimit<default-joint-solimplimit>` | :ref:`solreffriction<default-joint-solreffriction>` | :ref:`solimpfriction<default-joint-solimpfriction>` | :ref:`stiffness<default-joint-stiffness>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`range<default-joint-range>` | :ref:`actuatorfrcrange<default-joint-actuatorfrcrange>` | :ref:`margin<default-joint-margin>` | :ref:`ref<default-joint-ref>` | |
| | | | :ref:`range<default-joint-range>` | :ref:`actuatorfrcrange<default-joint-actuatorfrcrange>` | :ref:`actuatorgravcomp<default-joint-actuatorgravcomp>` | :ref:`margin<default-joint-margin>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`springref<default-joint-springref>` | :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:`damping<default-joint-damping>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`user<default-joint-user>` | | | | |
| | | | :ref:`frictionloss<default-joint-frictionloss>` | :ref:`user<default-joint-user>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
doc/changelog.rst
+26 -19
View File
@@ -7,15 +7,35 @@ Upcoming version (not yet released)
General
^^^^^^^
1. Added support for gmsh format 2.2, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
1. Added the :ref:`actuatorgravcomp<body-joint-actuatorgravcomp>` joint attribute. When enabled, gravity compensation
forces on the joint are treated as applied by actuators. See attribute documentation for more details. The example
model
`refsite.xml <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/actuation/refsite.xml>`__,
which demostrates Cartesian actuation of an arm, has been updated to use this attribute.
2. Added support for gmsh format 2.2, as generated by e.g. `fTetwild <https://github.com/wildmeshing/fTetWild>`__.
MJX
^^^
3. Improved performance of SAT for convex collisions.
4. Fixed bug for sphere/capsule-convex deep penetration.
5. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``.
6. Throw an error for margin/gap for convex mesh collisions, since they are not supported.
Simulate
^^^^^^^^
7. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the
:ref:`invdiscrete<option-flag-invdiscrete>` or :ref:`sensornoise<option-flag-sensornoise>`
flags would actually toggle the other flag.
Python bindings
^^^^^^^^^^^^^^^
8. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID).
The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse
Kinematics, is available here: |ls_colab|
.. |ls_colab| image:: https://colab.research.google.com/assets/colab-badge.svg
:target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb
2. Improved performance of SAT for convex collisions.
3. Fixed bug for sphere/capsule-convex deep penetration.
4. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``.
5. Throw an error for margin/gap for convex mesh collisions, since they are not supported.
Version 3.1.3 (March 5th, 2024)
-------------------------------
@@ -37,7 +57,6 @@ General
MJX
^^^
4. Improved performance of getting and putting device data.
- Use ``tobytes()`` for numpy array serialization, which is orders of magnitude faster than converting to tuples.
@@ -56,21 +75,9 @@ Python bindings
^^^^^^^^^^^^^^^
11. Fixed incorrect data types in the bindings for the ``geom``, ``vert``, ``elem``, and ``flex`` array members
of the ``mjContact`` struct, and all array members of the ``mjrContext`` struct.
12. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID).
The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse
Kinematics, is available here: |ls_colab|
.. |ls_colab| image:: https://colab.research.google.com/assets/colab-badge.svg
:target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb
Simulate
^^^^^^^^
13. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the
:ref:`invdiscrete<option-flag-invdiscrete>` or :ref:`sensornoise<option-flag-sensornoise>`
flags would actually toggle the other flag.
Version 3.1.2 (February 05, 2024)
-----------------------------------
---------------------------------
General
^^^^^^^
doc/includes/references.h
+4 -3
View File
@@ -926,9 +926,9 @@ struct mjModel_ {
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
int nuserdata; // number of extra fields in mjData
int nsensordata; // number of fields in sensor data vector
int npluginstate; // number of fields in plugin state vector
int nuserdata; // number of mjtNums reserved for the user
int nsensordata; // number of mjtNums in sensor data vector
int npluginstate; // number of mjtNums in plugin state vector
size_t narena; // number of bytes in the mjData arena (inclusive of stack)
size_t nbuffer; // number of bytes in buffer
@@ -993,6 +993,7 @@ struct mjModel_ {
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)
mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (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)
include/mujoco/mjmodel.h
+4 -3
View File
@@ -641,9 +641,9 @@ struct mjModel_ {
int nemax; // number of potential equality-constraint rows
int njmax; // number of available rows in constraint Jacobian
int nconmax; // number of potential contacts in contact list
int nuserdata; // number of extra fields in mjData
int nsensordata; // number of fields in sensor data vector
int npluginstate; // number of fields in plugin state vector
int nuserdata; // number of mjtNums reserved for the user
int nsensordata; // number of mjtNums in sensor data vector
int npluginstate; // number of mjtNums in plugin state vector
size_t narena; // number of bytes in the mjData arena (inclusive of stack)
size_t nbuffer; // number of bytes in buffer
@@ -708,6 +708,7 @@ struct mjModel_ {
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)
mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (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)
include/mujoco/mjxmacro.h
+1
View File
@@ -215,6 +215,7 @@
XMJV( int, jnt_group, njnt, 1 ) \
X ( mjtByte, jnt_limited, njnt, 1 ) \
X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \
X ( mjtByte, jnt_actgravcomp, njnt, 1 ) \
X ( mjtNum, jnt_solref, njnt, mjNREF ) \
X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \
X ( mjtNum, jnt_pos, njnt, 3 ) \
introspect/structs.py
+10 -3
View File
@@ -1218,17 +1218,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([
StructFieldDecl(
name='nuserdata',
type=ValueType(name='int'),
doc='number of extra fields in mjData',
doc='number of mjtNums reserved for the user',
),
StructFieldDecl(
name='nsensordata',
type=ValueType(name='int'),
doc='number of fields in sensor data vector',
doc='number of mjtNums in sensor data vector',
),
StructFieldDecl(
name='npluginstate',
type=ValueType(name='int'),
doc='number of fields in plugin state vector',
doc='number of mjtNums in plugin state vector',
),
StructFieldDecl(
name='narena',
@@ -1556,6 +1556,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='does joint have actuator force limits (njnt x 1)',
),
StructFieldDecl(
name='jnt_actgravcomp',
type=PointerType(
inner_type=ValueType(name='mjtByte'),
),
doc='is gravcomp force applied via actuators (njnt x 1)',
),
StructFieldDecl(
name='jnt_solref',
type=PointerType(
src/engine/engine_forward.c
+41 -7
View File
@@ -244,7 +244,7 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d,
// clamp vector to range
static void mju_clamp(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
const int* index) {
for (int i=0; i < n; i++) {
int j = index ? index[i] : i;
@@ -277,7 +277,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mjtNum *ctrl = mj_stackAllocNum(d, nu);
mju_copy(ctrl, d->ctrl, nu);
if (!mjDISABLED(mjDSBL_CLAMPCTRL)) {
mju_clamp(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
}
// check controls, set all to 0 if any are bad
@@ -463,13 +463,47 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
// clamp actuator_force
mju_clamp(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL);
// qfrc_actuator = moment' * force
mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv);
// clamp qfrc_actuator
mju_clamp(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr);
// actuator-level gravity compensation
if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added as passive force
if (!m->jnt_actgravcomp[i]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
// clamp qfrc_actuator to joint-level actuator force limits
clampVec(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr);
mj_freeStack(d);
TM_END(mjTIMER_ACTUATION);
@@ -481,13 +515,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
void mj_fwdAcceleration(const mjModel* m, mjData* d) {
int nv = m->nv;
// qforce = sum of all non-constraint forces
// qfrc_smooth = sum of all non-constraint forces
mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv); // qfrc_bias is negative
mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv);
mj_xfrcAccumulate(m, d, d->qfrc_smooth);
// qacc_smooth = M \ qfr_smooth
// qacc_smooth = M \ qfrc_smooth
mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1);
}
src/engine/engine_passive.c
+32 -1
View File
@@ -255,8 +255,39 @@ void mj_passive(const mjModel* m, mjData* d) {
// add passive forces into qfrc_passive
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
if (has_gravcomp) mju_addTo(d->qfrc_passive, d->qfrc_gravcomp, nv);
if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
if (has_gravcomp) {
int njnt = m->njnt;
for (int i=0; i < njnt; i++) {
// skip if gravcomp added via actuators
if (m->jnt_actgravcomp[i]) {
continue;
}
// get number of dofs for this joint
int dofnum;
switch (m->jnt_type[i]) {
case mjJNT_HINGE:
case mjJNT_SLIDE:
dofnum = 1;
break;
case mjJNT_BALL:
dofnum = 3;
break;
case mjJNT_FREE:
dofnum = 6;
break;
}
// add gravcomp force
int dofadr = m->jnt_dofadr[i];
for (int j=0; j < dofnum; j++) {
d->qfrc_passive[dofadr+j] += d->qfrc_gravcomp[dofadr+j];
}
}
}
// user callback: add custom passive forces
if (mjcb_passive) {
src/user/user_api.h
+1
View File
@@ -230,6 +230,7 @@ typedef struct _mjsJoint { // joint specification
// other
int group; // group
mjtByte actgravcomp; // is gravcomp force applied via actuators
double urdfeffort; // effort (urdf)
mjDoubleVec userdata; // user data
mjString info; // message appended to compiler errors
src/user/user_model.cc
+1
View File
@@ -1768,6 +1768,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_group[jid] = pj->group;
m->jnt_limited[jid] = (mjtByte)pj->is_limited();
m->jnt_actfrclimited[jid] = (mjtByte)pj->is_actfrclimited();
m->jnt_actgravcomp[jid] = pj->actgravcomp;
m->jnt_qposadr[jid] = qposadr;
m->jnt_dofadr[jid] = dofadr;
m->jnt_bodyid[jid] = pj->body->id;
src/xml/xml_native_reader.cc
+9 -5
View File
@@ -138,10 +138,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mesh", "?", "1", "scale"},
{"material", "?", "8", "texture", "emission", "specular", "shininess",
"reflectance", "rgba", "texrepeat", "texuniform"},
{"joint", "?", "21", "type", "group", "pos", "axis", "springdamper",
{"joint", "?", "22", "type", "group", "pos", "axis", "springdamper",
"limited", "actuatorfrclimited", "solreflimit", "solimplimit",
"solreffriction", "solimpfriction", "stiffness", "range", "actuatorfrcrange",
"margin", "ref", "springref", "armature", "damping", "frictionloss", "user"},
"actuatorgravcomp", "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",
@@ -240,11 +241,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"<"},
{"inertial", "?", "9", "pos", "quat", "mass", "diaginertia",
"axisangle", "xyaxes", "zaxis", "euler", "fullinertia"},
{"joint", "*", "23", "name", "class", "type", "group", "pos", "axis",
{"joint", "*", "24", "name", "class", "type", "group", "pos", "axis",
"springdamper", "limited", "actuatorfrclimited",
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"stiffness", "range", "actuatorfrcrange", "margin", "ref", "springref",
"armature", "damping", "frictionloss", "user"},
"stiffness", "range", "actuatorfrcrange", "actuatorgravcomp", "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",
@@ -1573,6 +1574,9 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) {
ReadAttr(elem, "armature", 1, &pjoint->armature, text);
ReadAttr(elem, "damping", 1, &pjoint->damping, text);
ReadAttr(elem, "frictionloss", 1, &pjoint->frictionloss, text);
if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) {
pjoint->actgravcomp = (n==1);
}
// read userdata
if (ReadVector(elem, "user", userdata, text)) {
src/xml/xml_native_writer.cc
+1
View File
@@ -307,6 +307,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
WriteAttr(elem, "range", 2, pjoint->range, def->joint.range);
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->actfrclimited,
def->joint.actfrclimited);
WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->joint.actgravcomp);
WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->joint.actfrcrange);
WriteAttr(elem, "margin", 1, &pjoint->margin, &def->joint.margin);
WriteAttr(elem, "armature", 1, &pjoint->armature, &def->joint.armature);
test/engine/engine_forward_test.cc
+74
View File
@@ -765,6 +765,80 @@ TEST_F(ActuatorTest, ActuatorForceClamping) {
mj_deleteModel(model);
}
// Apply gravity compensation via actuators
TEST_F(ActuatorTest, ActuatorGravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp="1">
<joint name="joint" type="slide" axis="0 0 1"
actuatorfrcrange="-2 2" actuatorgravcomp="true"/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<motor name="actuator" joint="joint"/>
</actuator>
<sensor>
<actuatorfrc actuator="actuator"/>
<jointactuatorfrc joint="joint"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mj_forward(model, data);
// expect force clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 2);
// reduce gravity so gravcomp is not clamped
model->opt.gravity[2] = -1;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 1);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 1);
// add control, see that it adds up
data->ctrl[0] = 0.5;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 0.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0.5);
EXPECT_EQ(data->sensordata[1], 1.5);
// add larger control, expect clamping
data->ctrl[0] = 1.5;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 2);
// disable actgravcomp, expect gravcomp as a passive force
model->jnt_actgravcomp[0] = 0;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 1);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 1.5);
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------- filterexact actuators -------------------------------
using FilterExactTest = MujocoTest;
test/engine/testdata/actuation/refsite.xml
Vendored
+6 -6
View File
@@ -6,7 +6,7 @@
(e.g. 4 consecutive ball joints is not a realistic kinematic design)
-->
<option viscosity="10" integrator="implicit">
<flag gravity="disable" contact="disable"/>
<flag contact="disable"/>
</option>
<statistic meansize=".05"/>
@@ -29,7 +29,7 @@
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" actuatorfrcrange="-1 1"/>
<joint armature="1e-3" damping="1e-3" stiffness="1e-2" actuatorfrcrange="-1 1" actuatorgravcomp="true"/>
<site type="box" size=".012 .012 .012" rgba=".7 .7 .8 1"/>
</default>
@@ -37,19 +37,19 @@
<light pos="0 0 2"/>
<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">
<body name="arm" pos="-.25 .25 0" gravcomp="1">
<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">
<body pos="0 0 .25" gravcomp="1">
<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">
<body pos=".25 0 0" gravcomp="1">
<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">
<body pos="0 -.2 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<joint axis="0 1 0"/>
unity/Runtime/Bindings/MjBindings.cs
+1
View File
@@ -5218,6 +5218,7 @@ public unsafe struct mjModel_ {
public int* jnt_group;
public byte* jnt_limited;
public byte* jnt_actfrclimited;
public byte* jnt_actgravcomp;
public double* jnt_solref;
public double* jnt_solimp;
public double* jnt_pos;