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

Add filterexact and actearly options to MuJoCo.

Browse Source 
When filterexact is a new dyntype which is just like the existing `filter`, but the activation state is integrated with exact integration, instead of simple Euler integration.

When actearly is specified on a general actuator, `qfrc_actuator` is computed using the next timestep's `act` value, instead of the current value.

PiperOrigin-RevId: 554438423
Change-Id: If901e4988fa6b518d6f3097f149770665a189a4b
This commit is contained in:
Nimrod Gileadi
2023-08-07 04:49:42 -07:00
committed by Copybara-Service
parent f0f535ed04
commit 2c3297b3e7
17 changed files with 441 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+18 -10
View File
@@ -1744,6 +1744,8 @@ if omitted.
.. _default-general-biasprm:
.. _default-general-actearly:
:el-prefix:`default/` |-| **general** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -5086,20 +5088,21 @@ specify them independently.
.. _actuator-general-dyntype:
:at:`dyntype`: :at-val:`[none, integrator, filter, muscle, user], "none"`
:at:`dyntype`: :at-val:`[none, integrator, filter, filterexact, muscle, user], "none"`
Activation dynamics type for the actuator. The available dynamics types were already described in the :ref:`Actuation
model <geActuation>` section. Repeating that description in somewhat different notation (corresponding to the mjModel
and mjData fields involved) we have:
========== ==================================
Keyword Description
========== ==================================
none No internal state
integrator act_dot = ctrl
filter act_dot = (ctrl - act) / dynprm[0]
muscle act_dot = mju_muscleDynamics(...)
user act_dot = mjcb_act_dyn(...)
========== ==================================
=========== ======================================
Keyword Description
=========== ======================================
none No internal state
integrator act_dot = ctrl
filter act_dot = (ctrl - act) / dynprm[0]
filterexact Like filter but with exact integration
muscle act_dot = mju_muscleDynamics(...)
user act_dot = mjcb_act_dyn(...)
=========== ======================================
.. _actuator-general-gaintype:
@@ -5156,6 +5159,11 @@ specify them independently.
so the user can enter as many parameters as needed. These defaults are not compatible with muscle actuators; see
:ref:`muscle <actuator-muscle>` below.
.. _actuator-general-actearly:
:at:`actearly`: :at-val:`[false, true], "false"`
If true, force computation will use the next value of the activation variable rather than the current one.
Setting this flag reduces the delay between the control and accelerations by one time-step.
.. _actuator-motor:
doc/XMLschema.rst
+2 -2
View File
@@ -348,7 +348,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`gaintype<default-general-gaintype>` | :ref:`biastype<default-general-biastype>` | :ref:`dynprm<default-general-dynprm>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`gainprm<default-general-gainprm>` | :ref:`biasprm<default-general-biasprm>` | | |
| | | | :ref:`gainprm<default-general-gainprm>` | :ref:`biasprm<default-general-biasprm>` | :ref:`actearly<default-general-actearly>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| default |br| |_| |L| | | .. table:: |
@@ -982,7 +982,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`biastype<actuator-general-biastype>` | :ref:`dynprm<actuator-general-dynprm>` | :ref:`gainprm<actuator-general-gainprm>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`biasprm<actuator-general-biasprm>` | | | |
| | | | :ref:`biasprm<actuator-general-biasprm>` | :ref:`actearly<actuator-general-actearly>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
doc/changelog.rst
+11
View File
@@ -2,6 +2,17 @@
Changelog
=========
Upcoming version (not yet released)
-----------------------------------
General
^^^^^^^
- Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
the exact formula rather than with Euler integration.
- Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
Version 2.3.7 (July 20, 2023)
-----------------------------
doc/computation.rst
+20 -2
View File
@@ -314,8 +314,9 @@ independent of the other actuators. The activation types currently implemented a
.. math::
\begin{aligned}
\text{integrator}: & & \dot{w}_i &= u_i \\
\text{filter}: & & \dot{w}_i &= (u_i - w_i) / t \\
\text{integrator}: & & \dot{w}_i &= u_i \\
\text{filter}: & & \dot{w}_i &= (u_i - w_i) / t \\
\text{filterexact}: & & \dot{w}_i &= (u_i - w_i) / t \\
\end{aligned}
where :math:`t` is an actuator-specific time constant stored in ``mjModel.actuator_dynprm``. In addition the type can
@@ -323,6 +324,19 @@ be "user", in which case :math:`w_i` is computed by the user-defined callback :r
be "none" which corresponds to a regular actuator with no activation state. The dimensionality of :math:`w` equals
the number of actuators whose activation type is different from "none".
For ``filterexact`` activation dynamics, Euler integration of :math:`\dot{w}` is replaced with the analytic integral:
.. math::
\begin{aligned}
\text{filter}: & & w_{i+1} &= w_i + h (u_i - w_i) / t \\
\text{filterexact}: & & w_{i+1} &= w_i + (u_i - w_i) (1 - e^{-h / t}) \\
\end{aligned}
The two expressions converge to the same value in the :math:`h \rightarrow 0` limit.
Note that Euler-integrated filters diverge for :math:`t < h`, while exactly-integrated filters are stable for any
:math:`t > 0`.
.. _geActuatorForce:
Force generation
@@ -359,6 +373,10 @@ This quantity is stored in ``mjData.qfrc_actuator``. It is added to the applied
with any user-defined forces in joint or Cartesian coordinates (which are stored in ``mjData.qfrc_applied`` and
``mjData.xfrc_applied`` respectively).
Optionally, the :ref:`actearly<actuator-general-actearly>` attribute on an actuator computes ``mjData.qfrc_actuator``
based on the value of :math:`w_{i+1}` after integration, reducing the delay between changes to :math:`u` and
:math:`t`.
.. _gePassive:
Passive forces
doc/includes/references.h
+2
View File
@@ -485,6 +485,7 @@ typedef enum mjtDyn_ { // type of actuator dynamics
mjDYN_NONE = 0, // no internal dynamics; ctrl specifies force
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_USER // user-defined dynamics type
} mjtDyn;
@@ -1148,6 +1149,7 @@ struct mjModel_ {
mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN)
mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN)
mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS)
mjtByte* actuator_actearly; // step activation before force (nu x 1)
mjtNum* actuator_ctrlrange; // range of controls (nu x 2)
mjtNum* actuator_forcerange; // range of forces (nu x 2)
mjtNum* actuator_actrange; // range of activations (nu x 2)
include/mujoco/mjmodel.h
+2
View File
@@ -194,6 +194,7 @@ typedef enum mjtDyn_ { // type of actuator dynamics
mjDYN_NONE = 0, // no internal dynamics; ctrl specifies force
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_USER // user-defined dynamics type
} mjtDyn;
@@ -903,6 +904,7 @@ struct mjModel_ {
mjtNum* actuator_dynprm; // dynamics parameters (nu x mjNDYN)
mjtNum* actuator_gainprm; // gain parameters (nu x mjNGAIN)
mjtNum* actuator_biasprm; // bias parameters (nu x mjNBIAS)
mjtByte* actuator_actearly; // step activation before force (nu x 1)
mjtNum* actuator_ctrlrange; // range of controls (nu x 2)
mjtNum* actuator_forcerange; // range of forces (nu x 2)
mjtNum* actuator_actrange; // range of activations (nu x 2)
include/mujoco/mjxmacro.h
+1
View File
@@ -387,6 +387,7 @@
X ( mjtNum, actuator_dynprm, nu, mjNDYN ) \
X ( mjtNum, actuator_gainprm, nu, mjNGAIN ) \
X ( mjtNum, actuator_biasprm, nu, mjNBIAS ) \
X ( mjtByte, actuator_actearly, nu, 1 ) \
XMJV( mjtNum, actuator_ctrlrange, nu, 2 ) \
X ( mjtNum, actuator_forcerange, nu, 2 ) \
XMJV( mjtNum, actuator_actrange, nu, 2 ) \
introspect/enums.py
+3 -2
View File
@@ -210,8 +210,9 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjDYN_NONE', 0),
('mjDYN_INTEGRATOR', 1),
('mjDYN_FILTER', 2),
('mjDYN_MUSCLE', 3),
('mjDYN_USER', 4),
('mjDYN_FILTEREXACT', 3),
('mjDYN_MUSCLE', 4),
('mjDYN_USER', 5),
]),
)),
('mjtGain',
introspect/structs.py
+7
View File
@@ -2763,6 +2763,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='bias parameters (nu x mjNBIAS)',
),
StructFieldDecl(
name='actuator_actearly',
type=PointerType(
inner_type=ValueType(name='mjtByte'),
),
doc='step activation before force (nu x 1)',
),
StructFieldDecl(
name='actuator_ctrlrange',
type=PointerType(
src/engine/engine_forward.c
+82 -53
View File
@@ -152,7 +152,31 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
TM_END(mjTIMER_VELOCITY);
}
// returns the next act given the current act_dot, after clamping
static mjtNum nextActivation(const mjModel* m, const mjData* d,
int actuator_id, int act_adr, mjtNum act_dot) {
mjtNum act = d->act[act_adr];
if (m->actuator_dyntype[actuator_id] == 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
act = act + act_dot * m->opt.timestep;
}
// clamp to actrange
if (m->actuator_actlimited[actuator_id]) {
mjtNum* actrange = m->actuator_actrange + 2 * actuator_id;
act = mju_clip(act, actrange[0], actrange[1]);
}
return act;
}
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
void mj_fwdActuation(const mjModel* m, mjData* d) {
@@ -196,6 +220,51 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
}
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
if (m->actuator_plugin[i] >= 0) {
continue;
}
int j = m->actuator_actadr[i];
if (j < 0) {
continue;
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
// compute act_dot according to dynamics type
switch (m->actuator_dyntype[i]) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[j] = ctrl[i];
break;
case mjDYN_FILTER: // linear filter: prm = tau
case mjDYN_FILTEREXACT:
tau = mju_max(mjMINVAL, prm[0]);
d->act_dot[j] = (ctrl[i] - d->act[j]) / tau;
break;
case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact)
d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], prm);
break;
default: // user dynamics
if (mjcb_act_dyn) {
if (m->actuator_actnum[i] == 1) {
// scalar activation dynamics, get act_dot
d->act_dot[j] = mjcb_act_dyn(m, d, i);
} else {
// higher-order dynamics, mjcb_act_dyn writes into act_dot directly
mjcb_act_dyn(m, d, i);
}
} else {
d->act_dot[j] = 0;
}
}
}
// force = gain .* [ctrl/act] + bias
for (int i=0; i < nu; i++) {
// skip actuator plugins -- these are handled after builtin actuator types
@@ -237,7 +306,15 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
force[i] = gain * ctrl[i];
} else {
// use last activation variable associated with actuator i
force[i] = gain * d->act[m->actuator_actadr[i] + m->actuator_actnum[i] - 1];
int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
mjtNum act;
if (m->actuator_actearly[i]) {
act = nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
} else {
act = d->act[act_adr];
}
force[i] = gain * act;
}
// extract bias info
@@ -311,49 +388,6 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
}
}
// act_dot for stateful actuators
for (int i=0; i < nu; i++) {
if (m->actuator_plugin[i] >= 0) {
continue;
}
int j = m->actuator_actadr[i];
if (j < 0) {
continue;
}
// extract info
prm = m->actuator_dynprm + i*mjNDYN;
// compute act_dot according to dynamics type
switch (m->actuator_dyntype[i]) {
case mjDYN_INTEGRATOR: // simple integrator
d->act_dot[j] = ctrl[i];
break;
case mjDYN_FILTER: // linear filter: prm = tau
tau = mju_max(mjMINVAL, prm[0]);
d->act_dot[j] = (ctrl[i] - d->act[j]) / tau;
break;
case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact)
d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], prm);
break;
default: // user dynamics
if (mjcb_act_dyn) {
if (m->actuator_actnum[i] == 1) {
// scalar activation dynamics, get act_dot
d->act_dot[j] = mjcb_act_dyn(m, d, i);
} else {
// higher-order dynamics, mjcb_act_dyn writes into act_dot directly
mjcb_act_dyn(m, d, i);
}
} else {
d->act_dot[j] = 0;
}
}
}
mjFREESTACK;
TM_END(mjTIMER_ACTUATION);
}
@@ -514,16 +548,11 @@ static void mj_advance(const mjModel* m, mjData* d,
const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
// advance activations and clamp
if (m->na) {
mju_addToScl(d->act, act_dot, m->opt.timestep, m->na);
// clamp activations
for (int i=0; i < m->nu; i++) {
int j = m->actuator_actadr[i];
if (j > -1 && m->actuator_actlimited[i]) {
mjtNum* actrange = m->actuator_actrange + 2*i;
for (int k=0; k < m->actuator_actnum[i]; k++) {
d->act[j+k] = mju_clip(d->act[j+k], actrange[0], actrange[1]);
}
int actadr = m->actuator_actadr[i];
int actadr_end = actadr + m->actuator_actnum[i];
for (int j=actadr; j < actadr_end; j++) {
d->act[j] = nextActivation(m, d, i, j, act_dot[j]);
}
}
}
src/user/user_model.cc
+1
View File
@@ -1962,6 +1962,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->actuator_ctrllimited[i] = pac->ctrllimited;
m->actuator_forcelimited[i] = pac->forcelimited;
m->actuator_actlimited[i] = pac->actlimited;
m->actuator_actearly[i] = pac->actearly;
m->actuator_cranklength[i] = (mjtNum)pac->cranklength;
copyvec(m->actuator_gear + 6*i, pac->gear, 6);
copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN);
src/user/user_objects.h
+1
View File
@@ -1026,6 +1026,7 @@ class mjCActuator : public mjCBase {
double dynprm[mjNDYN]; // dynamics parameters
double gainprm[mjNGAIN]; // gain parameters
double biasprm[mjNGAIN]; // bias parameters
bool actearly = false; // apply activations to qfrc instantly
double ctrlrange[2]; // control range
double forcerange[2]; // force range
double actrange[2]; // activation range
src/xml/xml_native_reader.cc
+11 -5
View File
@@ -40,6 +40,7 @@
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_base.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
@@ -160,9 +161,9 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"solreflimit", "solimplimit", "solreffriction", "solimpfriction",
"frictionloss", "springlength", "width", "material",
"margin", "stiffness", "damping", "rgba", "user"},
{"general", "?", "17", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
{"general", "?", "18", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
"forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
{"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group"},
{"position", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
@@ -322,11 +323,12 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"actuator", "*", "0"},
{"<"},
{"general", "*", "28", "name", "class", "group",
{"general", "*", "29", "name", "class", "group",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"},
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
"actearly"},
{"motor", "*", "18", "name", "class", "group",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
@@ -596,11 +598,12 @@ const mjMap mark_map[mark_sz] = {
// dyn type
const int dyn_sz = 5;
const int dyn_sz = 6;
const mjMap dyn_map[dyn_sz] = {
{"none", mjDYN_NONE},
{"integrator", mjDYN_INTEGRATOR},
{"filter", mjDYN_FILTER},
{"filterexact", mjDYN_FILTEREXACT},
{"muscle", mjDYN_MUSCLE},
{"user", mjDYN_USER}
};
@@ -1686,6 +1689,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
if (MapValue(elem, "biastype", &n, bias_map, bias_sz)) {
pact->biastype = (mjtBias)n;
}
if (MapValue(elem, "actearly", &n, bool_map, 2)) {
pact->actearly = (n==1);
}
ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false);
ReadAttr(elem, "gainprm", mjNGAIN, pact->gainprm, text, false, false);
ReadAttr(elem, "biasprm", mjNBIAS, pact->biasprm, text, false, false);
src/xml/xml_native_writer.cc
+4 -2
View File
@@ -14,12 +14,10 @@
#include "xml/xml_native_writer.h"
#include <cfloat>
#include <cstddef>
#include <cstdio>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>
#include <mujoco/mjmodel.h>
@@ -28,8 +26,10 @@
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_base.h"
#include "xml/xml_util.h"
#include "tinyxml2.h"
@@ -628,6 +628,8 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->actuator.lengthrange);
WriteAttr(elem, "gear", 6, pact->gear, def->actuator.gear);
WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->actuator.cranklength);
WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly,
def->actuator.actearly);
// plugins: write config attributes
if (pact->is_plugin) {
test/engine/engine_forward_test.cc
+224 -1
View File
@@ -15,7 +15,11 @@
// Tests for engine/engine_forward.c.
#include "src/engine/engine_forward.h"
#include <cstddef>
#include <cmath>
#include <cstdlib>
#include <vector>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -46,6 +50,7 @@ using ::testing::DoubleNear;
using ::testing::Ne;
using ::testing::HasSubstr;
using ::testing::NotNull;
using ::testing::Gt;
// --------------------------- activation limits -------------------------------
@@ -592,6 +597,224 @@ TEST_F(ActuatorTest, ActuatorForceClamping) {
mj_deleteModel(model);
}
// ----------------------- filterexact actuators -------------------------------
using FilterExactTest = MujocoTest;
TEST_F(FilterExactTest, ApproximatesContinuousTime) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body name="box">
<joint name="slide" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" dyntype="filter" gainprm="1.1" />
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
const mjtNum kSimulationTime = 1.0;
// compute act with a small timestep to approximate continuous integration
model->opt.timestep = 0.001;
mj_resetData(model, data);
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model, data);
}
mjtNum continuous_act = data->act[0];
// compute again with a larger timestep, introducing integration error
model->opt.timestep = 0.01;
mj_resetData(model, data);
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model, data);
}
mjtNum discrete_act = data->act[0];
// compute a third time with exact integration
model->actuator_dyntype[0] = mjDYN_FILTEREXACT;
mj_resetData(model, data);
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model, data);
}
mjtNum exactfilter_act = data->act[0];
// expect exact integration to be closer to the small-timestep result
EXPECT_THAT(std::abs(continuous_act - discrete_act),
Gt(5*std::abs(continuous_act - exactfilter_act)))
<< "Using filterexact should make the error at least 5 times smaller";
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(FilterExactTest, TimestepIndependent) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body name="box">
<joint name="slide" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" dyntype="filterexact" dynprm="0.9" gainprm="1.1"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
const mjtNum kSimulationTime = 1.0;
// first, compute act based on a small timestep and exact integration
model->opt.timestep = 0.01;
mj_resetData(model, data);
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model, data);
}
mjtNum small_timestep_act = data->act[0];
// now change the timestep to a much larger timestep
model->opt.timestep = 0.1;
mj_resetData(model, data);
data->ctrl[0] = 1.0;
data->act[0] = 0.0;
for (int i = 0; i < std::round(kSimulationTime / model->opt.timestep); i++) {
mj_step(model, data);
}
mjtNum large_timestep_act = data->act[0];
EXPECT_THAT(small_timestep_act, DoubleNear(large_timestep_act, 1e-14))
<< "exact integration should be independent of timestep to machine "
"precision.";
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(FilterExactTest, ActEqualsCtrlWhenTauIsZero) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body name="box">
<joint name="slide" type="slide" axis="1 0 0" />
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<general joint="slide" dyntype="filterexact" dynprm="0" gainprm="1.1"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
data->ctrl[0] = 0.5;
data->act[0] = 0.0;
mj_step(model, data);
EXPECT_EQ(data->act[0], data->ctrl[0]);
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------- actearly actuator attribute -------------------------
using ActEarlyTest = MujocoTest;
TEST_F(ActEarlyTest, RemovesOneStepDelay) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/actuation/actearly.xml");
char error[1000];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_EQ(model->nu % 2, 0) << "number of actuators should be even";
ASSERT_EQ(model->nu, model->na) << "all actuators should be stateful";
ASSERT_EQ(model->nq, model->nu);
EXPECT_GT(model->nu, 0);
// actuators are ordered in pairs with actearly=true and actearly=false
for (int i = 0; i < model->na / 2; i++) {
EXPECT_TRUE(model->actuator_actearly[2*i]);
EXPECT_FALSE(model->actuator_actearly[2*i + 1]);
}
mjData* data = mj_makeData(model);
// set all controls to the same value and make one step
mju_fill(data->ctrl, 0.5, model->nu);
mj_step(model, data);
for (int i = 0; i < model->na / 2; i++) {
EXPECT_EQ(data->act[2 * i], data->act[2 * i + 1])
<< "act should be the same after first step for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * i);
EXPECT_EQ(data->act_dot[2 * i], data->act_dot[2 * i + 1])
<< "act_dot should be the same after first step for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * i);
}
for (int i = 0; i < 100; i++) {
std::vector<mjtNum> last_qfrc(data->qfrc_actuator,
data->qfrc_actuator + model->nu);
mj_step(model, data);
for (int j = 0; j < model->nu / 2; j++) {
// this is true for torque actuators
EXPECT_THAT(last_qfrc[2 * j],
DoubleNear(data->qfrc_actuator[2 * j + 1], 1e-3))
<< "there should be a 1 step delay between qfrc for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * j);
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(ActEarlyTest, DoesntChangeStateInMjForward) {
const std::string xml_path =
GetTestDataFilePath("engine/testdata/actuation/actearly.xml");
char error[1000];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// set all controls to the same value and make one step
mju_fill(data->ctrl, 0.5, model->nu);
mj_forward(model, data);
for (int i = 0; i < model->na; i++) {
EXPECT_EQ(data->act[i], 0)
<< "act should not change with mj_forward."
<< mj_id2name(model, mjOBJ_ACTUATOR, i);
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/actuation/actearly.xml
Vendored
+48
View File
@@ -0,0 +1,48 @@
<mujoco>
<compiler autolimits="true"/>
<option density="1000" timestep="0.05" integrator="implicitfast" />
<default>
<geom type="sphere" size="0.1" rgba="1 1 1 1"/>
<joint type="slide" axis="0 0 1"/>
<general ctrlrange="-1 1"/>
<default class="filter">
<general dyntype="filter" gear="100"/>
</default>
<default class="filterexact">
<general dyntype="filterexact" gear="100"/>
</default>
<default class="integrator">
<general dyntype="integrator" actrange="0 2" gear="100"/>
<default class="integrator_actearly">
<general actearly="true"/>
</default>
</default>
</default>
<worldbody>
<geom type="plane" size="10 10 .1" rgba="1 1 1 1"/>
<light pos="0 0 20"/>
<body pos="-0.3 0 0.1"><joint name="filter_actearly"/><geom/></body>
<body pos="-0.1 0 0.1"><joint name="filter"/><geom/></body>
<body pos="0.1 0 0.1"><joint name="filterexact_actearly"/><geom/></body>
<body pos="0.3 0 0.1"><joint name="filterexact"/><geom/></body>
<body pos=" 0.5 0 0.1"><joint name="integrator_actearly"/><geom/></body>
<body pos=" 0.7 0 0.1"><joint name="integrator"/><geom/></body>
</worldbody>
<actuator>
<general class="filter" name="filter_actearly" joint="filter_actearly" actearly="true"/>
<general class="filter" name="filter" joint="filter" actearly="false"/>
<general class="filterexact" name="filterexact_actearly" joint="filterexact_actearly" actearly="true"/>
<general class="filterexact" name="filterexact" joint="filterexact" actearly="false"/>
<general class="integrator_actearly" name="integrator_actearly" joint="integrator_actearly"/>
<general class="integrator" name="integrator" joint="integrator" actearly="false"/>
</actuator>
</mujoco>
unity/Runtime/Bindings/MjBindings.cs
+4 -2
View File
@@ -250,8 +250,9 @@ public enum mjtDyn : int{
mjDYN_NONE = 0,
mjDYN_INTEGRATOR = 1,
mjDYN_FILTER = 2,
mjDYN_MUSCLE = 3,
mjDYN_USER = 4,
mjDYN_FILTEREXACT = 3,
mjDYN_MUSCLE = 4,
mjDYN_USER = 5,
}
public enum mjtGain : int{
mjGAIN_FIXED = 0,
@@ -2184,6 +2185,7 @@ public unsafe struct mjModel_ {
public double* actuator_dynprm;
public double* actuator_gainprm;
public double* actuator_biasprm;
public byte* actuator_actearly;
public double* actuator_ctrlrange;
public double* actuator_forcerange;
public double* actuator_actrange;