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

Author: yuvaltassa

doc/XMLreference.rst

@@ -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:

doc/XMLschema.rst

@@ -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>`
 

doc/changelog.rst

@@ -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``,

doc/modeling.rst

@@ -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>

src/engine/engine_forward.c

@@ -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

src/user/user_api.cc

@@ -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]) {

src/xml/xml_native_reader.cc

@@ -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",