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:
Yuval Tassa
2026-07-17 07:48:46 -07:00
committed by Copybara-Service
parent 9331c1cd9b
commit 56a93979e0
9 changed files with 301 additions and 39 deletions
+44 -26
View File
@@ -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:
+6
View File
@@ -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>`
+8
View File
@@ -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
View File
@@ -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>
+66 -1
View File
@@ -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
-1
View File
@@ -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]) {
+3 -3
View File
@@ -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",
+166
View File
@@ -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) {
+4 -4
View File
@@ -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) {