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`
|
: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
|
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**
|
one of them must be specified. If this attribute is specified, the actuator acts on the given joint.
|
||||||
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
|
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
|
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
|
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:`-
|
the norm of :at:`gear`). Note that the length is defined on a circle: after total rotation of more than :math:`\pi`
|
||||||
\pi`, and vice-versa. Therefore :el:`position` servos for ball joints should generally use tighter limits which
|
it wraps to :math:`-\pi`, and vice-versa. :ref:`position<actuator-position>` and
|
||||||
prevent this wrapping. For **free** joints, gear defines a 3d translation axis in the world frame followed by a 3d
|
:ref:`intvelocity<actuator-intvelocity>` servos on such transmissions interpret their setpoint on the circle, driving
|
||||||
rotation axis in the child frame. The actuator generates force and torque relative to the specified axes. The
|
towards the nearest representative of the target, so targets can be wound continuously through any number of turns
|
||||||
actuator length for free joints is defined as zero (so it should not be used with position servos).
|
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:
|
.. _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
|
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
|
this case the actuator *does* have length and :el:`position` actuators can be used to directly control an end
|
||||||
effector, see `refsite.xml
|
effector, see `refsite.xml
|
||||||
<https://github.com/google-deepmind/mujoco/tree/main/test/engine/testdata/actuation/refsite.xml>`__ example
|
<https://github.com/google-deepmind/mujoco/tree/main/test/engine/testdata/actuation/refsite.xml>`__ example model. As
|
||||||
model. As above, the length is the dot product of the :at:`gear` vector and the frame difference. So ``gear="0 1 0 0
|
above, the length is the dot product of the :at:`gear` vector and the frame difference. So ``gear="0 1 0 0 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-
|
means "Y-offset of :at:`site` in the :at:`refsite` frame", while ``gear="0 0 0 0 0 1"`` means rotation "Z- rotation
|
||||||
rotation of :at:`site` in the :at:`refsite` frame". It is recommended to use a normalized :at:`gear` vector with
|
of :at:`site` in the :at:`refsite` frame". It is recommended to use a normalized :at:`gear` vector with nonzeros in
|
||||||
nonzeros in only the first 3 *or* the last 3 elements of :at:`gear`, so the actuator length will be in either length
|
only the first 3 *or* the last 3 elements of :at:`gear`, so the actuator length will be in either length units or
|
||||||
units or radians, respectively. As with ball joints (see :at:`joint` above), for rotations which exceed a total angle
|
radians, respectively. As with ball joints (see :ref:`general/joint<actuator-general-joint>` above), rotational
|
||||||
of :math:`\pi` will wrap around, so tighter limits are recommended.
|
lengths are defined on a circle and servo setpoints are interpreted on it; control limits are not required to prevent
|
||||||
|
wrapping.
|
||||||
|
|
||||||
.. _actuator-general-body:
|
.. _actuator-general-body:
|
||||||
|
|
||||||
@@ -5794,6 +5802,7 @@ gaintype fixed gainprm kp 0 0
|
|||||||
biastype affine biasprm 0 -kp -kv
|
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:
|
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
|
:ref:`Activation clamping <CActRange>` section of the Modeling chapter. The underlying
|
||||||
:el:`general` attributes are set as follows:
|
:el:`general` attributes are set as follows:
|
||||||
|
|
||||||
========== =========== ========= =========
|
========= =========== ========= =========
|
||||||
Attribute Setting Attribute Setting
|
Attribute Setting Attribute Setting
|
||||||
========== =========== ========= =========
|
========= =========== ========= =========
|
||||||
dyntype integrator dynprm 1 0 0
|
dyntype integrator dynprm 1 0 0
|
||||||
gaintype fixed gainprm kp 0 0
|
gaintype fixed gainprm kp 0 0
|
||||||
biastype affine biasprm 0 -kp -kv
|
biastype affine biasprm 0 -kp -kv
|
||||||
actlimited true
|
========= =========== ========= =========
|
||||||
========== =========== ========= =========
|
|
||||||
|
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:
|
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-forcelimited:
|
||||||
|
|
||||||
|
.. _actuator-intvelocity-actlimited:
|
||||||
|
|
||||||
.. _actuator-intvelocity-ctrlrange:
|
.. _actuator-intvelocity-ctrlrange:
|
||||||
|
|
||||||
.. _actuator-intvelocity-forcerange:
|
.. _actuator-intvelocity-forcerange:
|
||||||
@@ -6049,9 +6064,10 @@ This element has one custom attribute in addition to the common attributes:
|
|||||||
.. _actuator-intvelocity-armature:
|
.. _actuator-intvelocity-armature:
|
||||||
|
|
||||||
.. |actuator/intvelocity attrib list| replace::
|
.. |actuator/intvelocity attrib list| replace::
|
||||||
:at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`ctrlrange`,
|
:at:`name`, :at:`class`, :at:`group`, :at:`delay`, :at:`ctrllimited`, :at:`forcelimited`, :at:`actlimited`,
|
||||||
:at:`forcerange`, :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`,
|
:at:`ctrlrange`, :at:`forcerange`, :at:`actrange`, :at:`lengthrange`, :at:`gear`, :at:`cranklength`, :at:`joint`,
|
||||||
:at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`, :at:`damping`, :at:`armature`
|
:at:`jointinparent`, :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user`,
|
||||||
|
:at:`damping`, :at:`armature`
|
||||||
|
|
||||||
|actuator/intvelocity attrib list|
|
|actuator/intvelocity attrib list|
|
||||||
Same as in actuator/ :ref:`general <actuator-general>`.
|
Same as in actuator/ :ref:`general <actuator-general>`.
|
||||||
@@ -9990,6 +10006,8 @@ refsite, tendon, slidersite, cranksite.
|
|||||||
|
|
||||||
.. _default-intvelocity-forcelimited:
|
.. _default-intvelocity-forcelimited:
|
||||||
|
|
||||||
|
.. _default-intvelocity-actlimited:
|
||||||
|
|
||||||
.. _default-intvelocity-ctrlrange:
|
.. _default-intvelocity-ctrlrange:
|
||||||
|
|
||||||
.. _default-intvelocity-forcerange:
|
.. _default-intvelocity-forcerange:
|
||||||
|
|||||||
@@ -2660,6 +2660,9 @@
|
|||||||
.. grid-item::
|
.. grid-item::
|
||||||
:ref:`forcelimited<actuator-intvelocity-forcelimited>`
|
:ref:`forcelimited<actuator-intvelocity-forcelimited>`
|
||||||
|
|
||||||
|
.. grid-item::
|
||||||
|
:ref:`actlimited<actuator-intvelocity-actlimited>`
|
||||||
|
|
||||||
.. grid-item::
|
.. grid-item::
|
||||||
:ref:`ctrlrange<actuator-intvelocity-ctrlrange>`
|
:ref:`ctrlrange<actuator-intvelocity-ctrlrange>`
|
||||||
|
|
||||||
@@ -6032,6 +6035,9 @@
|
|||||||
.. grid-item::
|
.. grid-item::
|
||||||
:ref:`forcelimited<default-intvelocity-forcelimited>`
|
:ref:`forcelimited<default-intvelocity-forcelimited>`
|
||||||
|
|
||||||
|
.. grid-item::
|
||||||
|
:ref:`actlimited<default-intvelocity-actlimited>`
|
||||||
|
|
||||||
.. grid-item::
|
.. grid-item::
|
||||||
:ref:`ctrlrange<default-intvelocity-ctrlrange>`
|
:ref:`ctrlrange<default-intvelocity-ctrlrange>`
|
||||||
|
|
||||||
|
|||||||
@@ -52,6 +52,14 @@ General
|
|||||||
error instead of the generic "could not decode content" message.
|
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 resource writing via :ref:`mju_writeResource` and the ``write`` callback in :ref:`mjpResourceProvider`.
|
||||||
- Added support for :ref:`multiccd <coMultiCCD>` with arbitrarily large meshes.
|
- 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
|
- 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
|
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``,
|
``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).
|
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
|
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
|
position target would keep integrating beyond the joint limits, leading to loss of controllabillity. (On purely
|
||||||
of activation clamping, load the example model below:
|
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
|
.. collapse:: Example model with activation limits
|
||||||
|
|
||||||
@@ -853,8 +854,7 @@ of activation clamping, load the example model below:
|
|||||||
</worldbody>
|
</worldbody>
|
||||||
|
|
||||||
<actuator>
|
<actuator>
|
||||||
<general name="unclamped" joint="joint1" gainprm="1" biastype="affine"
|
<intvelocity name="unclamped" joint="joint1"/>
|
||||||
biasprm="0 -1" dyntype="integrator"/>
|
|
||||||
<intvelocity name="clamped" joint="joint2" actrange="-1.57 1.57"/>
|
<intvelocity name="clamped" joint="joint2" actrange="-1.57 1.57"/>
|
||||||
</actuator>
|
</actuator>
|
||||||
</mujoco>
|
</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)
|
// (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot)
|
||||||
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
void mj_fwdActuation(const mjModel* m, mjData* d) {
|
||||||
TM_START;
|
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
|
// DC motor without current state: use ctrl even if other activations exist
|
||||||
int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0);
|
int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0);
|
||||||
if (actnum == 0 || dcmotor_no_current) {
|
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 {
|
} else {
|
||||||
// use last activation variable associated with actuator i
|
// use last activation variable associated with actuator i
|
||||||
int act_adr = m->actuator_actadr[i] + actnum - 1;
|
int act_adr = m->actuator_actadr[i] + actnum - 1;
|
||||||
@@ -601,6 +645,12 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
|
|||||||
} else {
|
} else {
|
||||||
act = d->act[act_adr];
|
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;
|
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]);
|
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
|
// 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) {
|
double dampratio[1], double timeconst[1], double inheritrange) {
|
||||||
mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange);
|
mjs_setToPosition(actuator, kp, kv, dampratio, timeconst, inheritrange);
|
||||||
actuator->dyntype = mjDYN_INTEGRATOR;
|
actuator->dyntype = mjDYN_INTEGRATOR;
|
||||||
actuator->actlimited = mjLIMITED_TRUE;
|
|
||||||
|
|
||||||
if (inheritrange > 0) {
|
if (inheritrange > 0) {
|
||||||
if (actuator->actrange[0] || actuator->actrange[1]) {
|
if (actuator->actrange[0] || actuator->actrange[1]) {
|
||||||
|
|||||||
@@ -266,8 +266,8 @@ std::vector<const char*> MJCF[nMJCF] = {
|
|||||||
"delay", "kp", "kv", "dampratio", "timeconst"},
|
"delay", "kp", "kv", "dampratio", "timeconst"},
|
||||||
{"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear",
|
{"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear",
|
||||||
"damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
"damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
||||||
{"intvelocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange",
|
{"intvelocity", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange",
|
||||||
"inheritrange", "gear", "damping", "armature", "cranklength", "user", "group",
|
"actrange", "inheritrange", "gear", "damping", "armature", "cranklength", "user", "group",
|
||||||
"nsample", "interp", "delay", "kp", "kv", "dampratio"},
|
"nsample", "interp", "delay", "kp", "kv", "dampratio"},
|
||||||
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
|
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
|
||||||
"gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
|
"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",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
"kv"},
|
"kv"},
|
||||||
{"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
{"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay",
|
||||||
"ctrllimited", "forcelimited",
|
"ctrllimited", "forcelimited", "actlimited",
|
||||||
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
|
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
|
||||||
"gear", "damping", "armature", "cranklength", "user",
|
"gear", "damping", "armature", "cranklength", "user",
|
||||||
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
|
||||||
|
|||||||
@@ -616,6 +616,172 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
|
|||||||
mj_deleteModel(model);
|
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";
|
static const char* const kInertiaPath = "engine/testdata/inertia.xml";
|
||||||
|
|
||||||
TEST_F(CoreSmoothTest, FactorI) {
|
TEST_F(CoreSmoothTest, FactorI) {
|
||||||
|
|||||||
@@ -2859,7 +2859,7 @@ TEST_F(ActuatorParseTest, IntvelocityCheckDefaultsIfNotSpecified) {
|
|||||||
EXPECT_DOUBLE_EQ(model->actuator_biasprm[2], 0.0);
|
EXPECT_DOUBLE_EQ(model->actuator_biasprm[2], 0.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) {
|
TEST_F(ActuatorParseTest, IntvelocityNoActrangeIsValid) {
|
||||||
static constexpr char xml[] = R"(
|
static constexpr char xml[] = R"(
|
||||||
<mujoco>
|
<mujoco>
|
||||||
<worldbody>
|
<worldbody>
|
||||||
@@ -2875,9 +2875,9 @@ TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) {
|
|||||||
)";
|
)";
|
||||||
std::array<char, 1024> error;
|
std::array<char, 1024> error;
|
||||||
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
|
||||||
ASSERT_THAT(model.get(), IsNull());
|
ASSERT_THAT(model.get(), NotNull()) << error.data();
|
||||||
EXPECT_THAT(error.data(), HasSubstr("invalid actrange for actuator"));
|
// actlimited resolves to false when no actrange is provided
|
||||||
EXPECT_THAT(error.data(), HasSubstr("line 10"));
|
EXPECT_EQ(model->actuator_actlimited[0], 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(ActuatorParseTest, IntvelocityDefaultsPropagate) {
|
TEST_F(ActuatorParseTest, IntvelocityDefaultsPropagate) {
|
||||||
|
|||||||
Reference in New Issue
Block a user