Interpret position and intvelocity setpoints on 3D rotational transmissions (ball joints, site+refsite) on the circle. The force uses the setpoint representative nearest the current length for smooth tracking beyond pi.
- Wrapping in force path is local; act is re-anchored at integration time. - Remove hardcoded `actrange` for intvelocity actuators. PiperOrigin-RevId: 949566477 Change-Id: I349fdf17eedfbb2174d698cc1a6a91d52810b4a3
This commit is contained in:
committed by
Copybara-Service
parent
9331c1cd9b
commit
56a93979e0
+44
-26
@@ -5531,16 +5531,23 @@ specify them independently.
|
||||
|
||||
:at:`joint`: :at-val:`string, optional`
|
||||
This and the next four attributes determine the type of actuator transmission. All of them are optional, and exactly
|
||||
one of them must be specified. If this attribute is specified, the actuator acts on the given joint. For **hinge**
|
||||
and **slide** joints, the actuator length equals the joint position/angle times the first element of :at:`gear`. For
|
||||
**ball** joints, the first three elements of gear define a 3d rotation axis in the child frame around which the
|
||||
one of them must be specified. If this attribute is specified, the actuator acts on the given joint.
|
||||
|
||||
For **hinge** and **slide** joints, the actuator length equals the joint position/angle times the first element of
|
||||
:at:`gear`.
|
||||
|
||||
For **ball** joints, the first three elements of gear define a 3d rotation axis in the child frame around which the
|
||||
actuator produces torque. The actuator length is defined as the dot-product between this gear axis and the angle-axis
|
||||
representation of the joint quaternion, and is in units of radian if :at:`gear` is normalized (generally scaled by
|
||||
by the norm of :at:`gear`). Note that after total rotation of more than :math:`\pi`, the length will wrap to :math:`-
|
||||
\pi`, and vice-versa. Therefore :el:`position` servos for ball joints should generally use tighter limits which
|
||||
prevent this wrapping. For **free** joints, gear defines a 3d translation axis in the world frame followed by a 3d
|
||||
rotation axis in the child frame. The actuator generates force and torque relative to the specified axes. The
|
||||
actuator length for free joints is defined as zero (so it should not be used with position servos).
|
||||
the norm of :at:`gear`). Note that the length is defined on a circle: after total rotation of more than :math:`\pi`
|
||||
it wraps to :math:`-\pi`, and vice-versa. :ref:`position<actuator-position>` and
|
||||
:ref:`intvelocity<actuator-intvelocity>` servos on such transmissions interpret their setpoint on the circle, driving
|
||||
towards the nearest representative of the target, so targets can be wound continuously through any number of turns
|
||||
and no control limits are required to prevent wrapping.
|
||||
|
||||
For **free** joints, gear defines a 3d translation axis in the world frame followed by a 3d rotation axis in the
|
||||
child frame. The actuator generates force and torque relative to the specified axes. The actuator length for free
|
||||
joints is defined as zero (so cannot be used with position servos).
|
||||
|
||||
.. _actuator-general-jointinparent:
|
||||
|
||||
@@ -5570,13 +5577,14 @@ specify them independently.
|
||||
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/google-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
|
||||
nonzeros in only the first 3 *or* the last 3 elements of :at:`gear`, so the actuator length will be in either length
|
||||
units or radians, respectively. As with ball joints (see :at:`joint` above), for rotations which exceed a total angle
|
||||
of :math:`\pi` will wrap around, so tighter limits are recommended.
|
||||
<https://github.com/google-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 nonzeros in
|
||||
only the first 3 *or* the last 3 elements of :at:`gear`, so the actuator length will be in either length units or
|
||||
radians, respectively. As with ball joints (see :ref:`general/joint<actuator-general-joint>` above), rotational
|
||||
lengths are defined on a circle and servo setpoints are interpreted on it; control limits are not required to prevent
|
||||
wrapping.
|
||||
|
||||
.. _actuator-general-body:
|
||||
|
||||
@@ -5794,6 +5802,7 @@ gaintype fixed gainprm kp 0 0
|
||||
biastype affine biasprm 0 -kp -kv
|
||||
========= =================== ========= =============
|
||||
|
||||
On purely rotational transmissions, setpoints are interpreted on the circle; see :ref:`gear<actuator-general-gear>`.
|
||||
|
||||
This element has one custom attribute in addition to the common attributes:
|
||||
|
||||
@@ -5989,14 +5998,18 @@ This element creates an integrated-velocity servo. For more information, see the
|
||||
:ref:`Activation clamping <CActRange>` section of the Modeling chapter. The underlying
|
||||
:el:`general` attributes are set as follows:
|
||||
|
||||
========== =========== ========= =========
|
||||
Attribute Setting Attribute Setting
|
||||
========== =========== ========= =========
|
||||
dyntype integrator dynprm 1 0 0
|
||||
gaintype fixed gainprm kp 0 0
|
||||
biastype affine biasprm 0 -kp -kv
|
||||
actlimited true
|
||||
========== =========== ========= =========
|
||||
========= =========== ========= =========
|
||||
Attribute Setting Attribute Setting
|
||||
========= =========== ========= =========
|
||||
dyntype integrator dynprm 1 0 0
|
||||
gaintype fixed gainprm kp 0 0
|
||||
biastype affine biasprm 0 -kp -kv
|
||||
========= =========== ========= =========
|
||||
|
||||
Activation clamping is controlled by :at:`actlimited` and :at:`actrange`, like any stateful actuator. On purely
|
||||
rotational transmissions, setpoints are interpreted on the circle, as for :ref:`position<actuator-position>`; the
|
||||
integrated setpoint is re-anchored to a bounded representative at each timestep, so clamping is not required for
|
||||
winding targets.
|
||||
|
||||
This element has one custom attribute in addition to the common attributes:
|
||||
|
||||
@@ -6016,6 +6029,8 @@ This element has one custom attribute in addition to the common attributes:
|
||||
|
||||
.. _actuator-intvelocity-forcelimited:
|
||||
|
||||
.. _actuator-intvelocity-actlimited:
|
||||
|
||||
.. _actuator-intvelocity-ctrlrange:
|
||||
|
||||
.. _actuator-intvelocity-forcerange:
|
||||
@@ -6049,9 +6064,10 @@ This element has one custom attribute in addition to the common attributes:
|
||||
.. _actuator-intvelocity-armature:
|
||||
|
||||
.. |actuator/intvelocity attrib list| replace::
|
||||
:at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`,
|
||||
:at:`forcerange`, :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`,
|
||||
:at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature`
|
||||
:at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`actlimited`,
|
||||
:at:`ctrlrange`, :at:`forcerange`, :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`,
|
||||
:at:`jointinparent`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`,
|
||||
:at:`damping`, :at:`armature`
|
||||
|
||||
|actuator/intvelocity attrib list|
|
||||
Same as in actuator/ :ref:`general <actuator-general>`.
|
||||
@@ -9990,6 +10006,8 @@ refsite, tendon, slidersite, cranksite.
|
||||
|
||||
.. _default-intvelocity-forcelimited:
|
||||
|
||||
.. _default-intvelocity-actlimited:
|
||||
|
||||
.. _default-intvelocity-ctrlrange:
|
||||
|
||||
.. _default-intvelocity-forcerange:
|
||||
|
||||
@@ -2660,6 +2660,9 @@
|
||||
.. grid-item::
|
||||
:ref:`forcelimited<actuator-intvelocity-forcelimited>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`actlimited<actuator-intvelocity-actlimited>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`ctrlrange<actuator-intvelocity-ctrlrange>`
|
||||
|
||||
@@ -6032,6 +6035,9 @@
|
||||
.. grid-item::
|
||||
:ref:`forcelimited<default-intvelocity-forcelimited>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`actlimited<default-intvelocity-actlimited>`
|
||||
|
||||
.. grid-item::
|
||||
:ref:`ctrlrange<default-intvelocity-ctrlrange>`
|
||||
|
||||
|
||||
@@ -52,6 +52,14 @@ General
|
||||
error instead of the generic "could not decode content" message.
|
||||
- Added support for resource writing via :ref:`mju_writeResource` and the ``write`` callback in :ref:`mjpResourceProvider`.
|
||||
- Added support for :ref:`multiccd <coMultiCCD>` with arbitrarily large meshes.
|
||||
- Setpoints of :ref:`position<actuator-position>` and :ref:`intvelocity<actuator-intvelocity>` servos acting on 3D
|
||||
rotational transmissions (ball joints, or site transmissions with a :ref:`refsite<actuator-general-refsite>` and
|
||||
purely rotational gear) are now interpreted on the circle: the force uses the setpoint representative nearest the
|
||||
current angle, so targets winding beyond half a turn are tracked continuously instead of slipping by full turns.
|
||||
Behavior is identical whenever the error does not exceed π. Relatedly, ``intvelocity`` actuators now expose
|
||||
:ref:`actlimited<actuator-intvelocity-actlimited>`, which was previously hardcoded to "true": as for
|
||||
:ref:`general<actuator-general>` actuators it defaults to "auto", so activation clamping is enabled by specifying
|
||||
``actrange``. Unclamped integrated setpoints are well-behaved on rotational transmissions, where they wrap.
|
||||
- Refactored actuator infrastructure in preparation for MIMO (multi-input multi-output) actuator support. Each actuator
|
||||
now has ``ctrlnum`` (number of controls) and ``outnum`` (number of force outputs). The total counts
|
||||
``nu = sum(ctrlnum)`` and ``nout = sum(outnum)`` dimension ``mjData.ctrl`` and ``mjData.actuator_force``,
|
||||
|
||||
+4
-4
@@ -828,8 +828,9 @@ is the most common implementation of actuators with velocity semantics, rather t
|
||||
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
|
||||
position target would keep integrating beyond the joint limits, leading to loss of controllabillity. To see the effect
|
||||
of activation clamping, load the example model below:
|
||||
position target would keep integrating beyond the joint limits, leading to loss of controllabillity. (On purely
|
||||
rotational transmissions, the setpoint wraps on the circle and stays bounded without clamping; see
|
||||
:ref:`gear<actuator-general-gear>`.) To see the effect of activation clamping, load the example model below:
|
||||
|
||||
.. collapse:: Example model with activation limits
|
||||
|
||||
@@ -853,8 +854,7 @@ of activation clamping, load the example model below:
|
||||
</worldbody>
|
||||
|
||||
<actuator>
|
||||
<general name="unclamped" joint="joint1" gainprm="1" biastype="affine"
|
||||
biasprm="0 -1" dyntype="integrator"/>
|
||||
<intvelocity name="unclamped" joint="joint1"/>
|
||||
<intvelocity name="clamped" joint="joint2" actrange="-1.57 1.57"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
|
||||
@@ -268,6 +268,43 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtBool* limited, i
|
||||
}
|
||||
|
||||
|
||||
// period of the rotational transmission for wrap-eligible servo actuators, 0 otherwise
|
||||
static mjtNum wrapPeriod(const mjModel* m, int i) {
|
||||
// servo shape: fixed gain, affine bias, matching kp, setpoint input
|
||||
mjtDyn dyntype = m->actuator_dyntype[i];
|
||||
if (m->actuator_gaintype[i] != mjGAIN_FIXED ||
|
||||
m->actuator_biastype[i] != mjBIAS_AFFINE ||
|
||||
m->actuator_gainprm[mjNGAIN*i] != -m->actuator_biasprm[mjNBIAS*i+1] ||
|
||||
(dyntype != mjDYN_NONE && dyntype != mjDYN_INTEGRATOR)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const mjtNum* gear = m->actuator_gear+6*m->actuator_outadr[i];
|
||||
mjtTrn trntype = m->actuator_trntype[i];
|
||||
|
||||
// site transmission with refsite and purely rotational gear
|
||||
if (trntype == mjTRN_SITE && m->actuator_trnid[2*i+1] >= 0 &&
|
||||
!gear[0] && !gear[1] && !gear[2]) {
|
||||
return 2*mjPI * mju_norm3(gear+3);
|
||||
}
|
||||
|
||||
// joint transmission on a ball joint
|
||||
if ((trntype == mjTRN_JOINT || trntype == mjTRN_JOINTINPARENT) &&
|
||||
m->jnt_type[m->actuator_trnid[2*i]] == mjJNT_BALL) {
|
||||
return 2*mjPI * mju_norm3(gear);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// representative of setpoint u nearest to length, given period
|
||||
static mjtNum wrapSetpoint(mjtNum u, mjtNum length, mjtNum period) {
|
||||
mjtNum err = u - length;
|
||||
return u - period * mju_round(err / period);
|
||||
}
|
||||
|
||||
|
||||
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
|
||||
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
TM_START;
|
||||
@@ -590,7 +627,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
// DC motor without current state: use ctrl even if other activations exist
|
||||
int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0);
|
||||
if (actnum == 0 || dcmotor_no_current) {
|
||||
force[oadr] = gain * ctrl[uadr];
|
||||
mjtNum input = ctrl[uadr];
|
||||
|
||||
// rotational setpoint: use representative nearest the length (local, no state change)
|
||||
mjtNum period = wrapPeriod(m, i);
|
||||
if (period > 0) {
|
||||
input = wrapSetpoint(input, d->actuator_length[oadr], period);
|
||||
}
|
||||
force[oadr] = gain * input;
|
||||
} else {
|
||||
// use last activation variable associated with actuator i
|
||||
int act_adr = m->actuator_actadr[i] + actnum - 1;
|
||||
@@ -601,6 +645,12 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||
} else {
|
||||
act = d->act[act_adr];
|
||||
}
|
||||
|
||||
// rotational setpoint: use representative nearest the length (local, no state change)
|
||||
mjtNum period = wrapPeriod(m, i);
|
||||
if (period > 0) {
|
||||
act = wrapSetpoint(act, d->actuator_length[oadr], period);
|
||||
}
|
||||
force[oadr] = gain * act;
|
||||
}
|
||||
|
||||
@@ -1087,6 +1137,21 @@ static void mj_advance(const mjModel* m, mjData* d,
|
||||
d->act[j] = mj_nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
|
||||
}
|
||||
}
|
||||
|
||||
// rotational setpoints stored in act: replace with an equivalent bounded representative,
|
||||
// like the actrange clamp above, this is a projection applied at integration time
|
||||
for (int i=0; i < nactuator; i++) {
|
||||
if (m->actuator_dyntype[i] != mjDYN_INTEGRATOR) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// per-axis servo: wrap act to the representative nearest the length
|
||||
mjtNum period = wrapPeriod(m, i);
|
||||
if (period > 0) {
|
||||
int adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1;
|
||||
d->act[adr] = wrapSetpoint(d->act[adr], d->actuator_length[m->actuator_outadr[i]], period);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// put islands to sleep according to velocity tolerance
|
||||
|
||||
@@ -1313,7 +1313,6 @@ const char* mjs_setToIntVelocity(mjsActuator* actuator, double kp, double kv[1],
|
||||
double dampratio[1], double timeconst[1], double inheritrange) {
|
||||
mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange);
|
||||
actuator->dyntype = mjDYN_INTEGRATOR;
|
||||
actuator->actlimited = mjLIMITED_TRUE;
|
||||
|
||||
if (inheritrange > 0) {
|
||||
if (actuator->actrange[0] || actuator->actrange[1]) {
|
||||
|
||||
@@ -266,8 +266,8 @@ std::vector<const char*> MJCF[nMJCF] = {
|
||||
"delay", "kp", "kv", "dampratio", "timeconst"},
|
||||
{"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear",
|
||||
"damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||
{"intvelocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange",
|
||||
"inheritrange", "gear", "damping", "armature", "cranklength", "user", "group",
|
||||
{"intvelocity", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange",
|
||||
"actrange", "inheritrange", "gear", "damping", "armature", "cranklength", "user", "group",
|
||||
"nsample", "interp", "delay", "kp", "kv", "dampratio"},
|
||||
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
|
||||
"gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||
@@ -491,7 +491,7 @@ std::vector<const char*> MJCF[nMJCF] = {
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
"kv"},
|
||||
{"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||
"ctrllimited", "forcelimited",
|
||||
"ctrllimited", "forcelimited", "actlimited",
|
||||
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
|
||||
"gear", "damping", "armature", "cranklength", "user",
|
||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||
|
||||
@@ -616,6 +616,172 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
// Test smooth tracking of a rotational target ramped through the pi boundary.
|
||||
// Rotational transmission lengths live in (-pi, pi]; a servo whose error is
|
||||
// computed in the chart rather than on the circle loses the target once it
|
||||
// crosses pi and enters a phase-slipping limit cycle.
|
||||
TEST_F(CoreSmoothTest, RefsiteTracksWindingTarget) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast">
|
||||
<flag contact="disable" gravity="disable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<site name="reference"/>
|
||||
<body name="box">
|
||||
<freejoint/>
|
||||
<geom type="box" size=".05 .07 .03"/>
|
||||
<site name="end_effector"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<position name="rz" site="end_effector" refsite="reference"
|
||||
gear="0 0 0 0 0 1" kp="1" dampratio="1"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||
mjData* data = mj_makeData(model.get());
|
||||
|
||||
int rz = mj_name2id(model.get(), mjOBJ_ACTUATOR, "rz");
|
||||
ASSERT_GE(rz, 0);
|
||||
|
||||
// ramp the rz target from 0 to 2*pi, slowly enough to track
|
||||
const mjtNum rate = 0.5; // rad/s
|
||||
while (data->time < 2 * mjPI / rate) {
|
||||
data->ctrl[rz] = rate * data->time;
|
||||
mj_step(model.get(), data);
|
||||
|
||||
// distance between target and actuator length, measured on the circle
|
||||
mjtNum error = data->ctrl[rz] - data->actuator_length[rz];
|
||||
error -= 2 * mjPI * mju_round(error / (2 * mjPI));
|
||||
ASSERT_LT(mju_abs(error), 0.5)
|
||||
<< "tracking lost at time " << data->time << ", target "
|
||||
<< data->ctrl[rz] << ", length " << data->actuator_length[rz];
|
||||
}
|
||||
|
||||
mj_deleteData(data);
|
||||
}
|
||||
|
||||
// Test single-axis winding on a ball joint with per-axis (wrapped) servos:
|
||||
// a target ramped through pi is tracked smoothly.
|
||||
TEST_F(CoreSmoothTest, BallTracksWindingTarget) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast">
|
||||
<flag contact="disable" gravity="disable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="ball" type="ball"/>
|
||||
<geom type="box" size=".05 .07 .03"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<position name="rx" joint="ball" gear="1 0 0" kp="1" dampratio="1"/>
|
||||
<position name="ry" joint="ball" gear="0 1 0" kp="1" dampratio="1"/>
|
||||
<position name="rz" joint="ball" gear="0 0 1" kp="1" dampratio="1"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||
mjData* data = mj_makeData(model.get());
|
||||
|
||||
int rz = mj_name2id(model.get(), mjOBJ_ACTUATOR, "rz");
|
||||
ASSERT_GE(rz, 0);
|
||||
|
||||
// ramp the rz target from 0 to 2*pi, assert tracking on the circle
|
||||
const mjtNum rate = 0.5; // rad/s
|
||||
while (data->time < 2 * mjPI / rate) {
|
||||
data->ctrl[rz] = rate * data->time;
|
||||
mj_step(model.get(), data);
|
||||
mjtNum error = data->ctrl[rz] - data->actuator_length[rz];
|
||||
error -= 2 * mjPI * mju_round(error / (2 * mjPI));
|
||||
ASSERT_LT(mju_abs(error), 0.5) << "tracking lost at time " << data->time;
|
||||
}
|
||||
|
||||
mj_deleteData(data);
|
||||
}
|
||||
|
||||
// Wrapped rotational intvelocity: actrange is optional, constant ctrl produces
|
||||
// steady rotation over many periods, activation stays bounded.
|
||||
TEST_F(CoreSmoothTest, IntVelocityWindsWithBoundedAct) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast">
|
||||
<flag contact="disable" gravity="disable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="ball" type="ball"/>
|
||||
<geom type="box" size=".05 .07 .03"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<intvelocity name="rz" joint="ball" gear="0 0 1" kp="1" dampratio="1"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||
mjData* data = mj_makeData(model.get());
|
||||
|
||||
// command constant angular rate for 4 full turns
|
||||
const mjtNum rate = 1.0; // rad/s
|
||||
data->ctrl[0] = rate;
|
||||
while (data->time < 8 * mjPI / rate) {
|
||||
mj_step(model.get(), data);
|
||||
ASSERT_LT(mju_abs(data->act[0]), mjPI + 0.1) << "act unbounded";
|
||||
}
|
||||
|
||||
// steady rotation at the commanded rate
|
||||
EXPECT_NEAR(data->actuator_velocity[0], rate, 0.01);
|
||||
|
||||
mj_deleteData(data);
|
||||
}
|
||||
|
||||
// mj_forward must not mutate state: wrapping of act happens at integration
|
||||
// time (mj_advance), never in the forward pass.
|
||||
TEST_F(CoreSmoothTest, ForwardDoesNotMutateAct) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<option integrator="implicitfast">
|
||||
<flag contact="disable" gravity="disable"/>
|
||||
</option>
|
||||
<worldbody>
|
||||
<body>
|
||||
<joint name="ball" type="ball"/>
|
||||
<geom type="box" size=".05 .07 .03"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<intvelocity name="rz" joint="ball" gear="0 0 1" kp="1" dampratio="1"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
)";
|
||||
char error[1024];
|
||||
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error;
|
||||
mjData* data = mj_makeData(model.get());
|
||||
|
||||
// forward leaves a far-from-length activation untouched, bit-for-bit
|
||||
data->act[0] = 100;
|
||||
mj_forward(model.get(), data);
|
||||
mj_forward(model.get(), data);
|
||||
EXPECT_EQ(data->act[0], 100);
|
||||
|
||||
// stepping wraps it to a bounded representative
|
||||
mj_step(model.get(), data);
|
||||
EXPECT_LT(mju_abs(data->act[0]), mjPI + 0.1);
|
||||
|
||||
mj_deleteData(data);
|
||||
}
|
||||
|
||||
static const char* const kInertiaPath = "engine/testdata/inertia.xml";
|
||||
|
||||
TEST_F(CoreSmoothTest, FactorI) {
|
||||
|
||||
@@ -2859,7 +2859,7 @@ TEST_F(ActuatorParseTest, IntvelocityCheckDefaultsIfNotSpecified) {
|
||||
EXPECT_DOUBLE_EQ(model->actuator_biasprm[2], 0.0);
|
||||
}
|
||||
|
||||
TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) {
|
||||
TEST_F(ActuatorParseTest, IntvelocityNoActrangeIsValid) {
|
||||
static constexpr char xml[] = R"(
|
||||
<mujoco>
|
||||
<worldbody>
|
||||
@@ -2875,9 +2875,9 @@ TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) {
|
||||
)";
|
||||
std::array<char, 1024> error;
|
||||
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||
ASSERT_THAT(model.get(), IsNull());
|
||||
EXPECT_THAT(error.data(), HasSubstr("invalid actrange for actuator"));
|
||||
EXPECT_THAT(error.data(), HasSubstr("line 10"));
|
||||
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||
// actlimited resolves to false when no actrange is provided
|
||||
EXPECT_EQ(model->actuator_actlimited[0], 0);
|
||||
}
|
||||
|
||||
TEST_F(ActuatorParseTest, IntvelocityDefaultsPropagate) {
|
||||
|
||||
Reference in New Issue
Block a user