diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 3ce59649..838c1d1a 100644 --- a/doc/XMLreference.rst +++ b/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` and + :ref:`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 - `__ 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. + `__ 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` 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`. 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 ` 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`; 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 `. @@ -9990,6 +10006,8 @@ refsite, tendon, slidersite, cranksite. .. _default-intvelocity-forcelimited: +.. _default-intvelocity-actlimited: + .. _default-intvelocity-ctrlrange: .. _default-intvelocity-forcerange: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index e82a994b..643af31e 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2660,6 +2660,9 @@ .. grid-item:: :ref:`forcelimited` + .. grid-item:: + :ref:`actlimited` + .. grid-item:: :ref:`ctrlrange` @@ -6032,6 +6035,9 @@ .. grid-item:: :ref:`forcelimited` + .. grid-item:: + :ref:`actlimited` + .. grid-item:: :ref:`ctrlrange` diff --git a/doc/changelog.rst b/doc/changelog.rst index 029e8bb0..94b1e7e8 100644 --- a/doc/changelog.rst +++ b/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 ` with arbitrarily large meshes. +- Setpoints of :ref:`position` and :ref:`intvelocity` servos acting on 3D + rotational transmissions (ball joints, or site transmissions with a :ref:`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`, which was previously hardcoded to "true": as for + :ref:`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``, diff --git a/doc/modeling.rst b/doc/modeling.rst index 5492c407..d3dbb432 100644 --- a/doc/modeling.rst +++ b/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`.) 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: - + diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index dbbc3dd2..a9f53bb7 100644 --- a/src/engine/engine_forward.c +++ b/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 diff --git a/src/user/user_api.cc b/src/user/user_api.cc index cb1e5a92..1eb207d5 100644 --- a/src/user/user_api.cc +++ b/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]) { diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 31161c2e..0eba322e 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -266,8 +266,8 @@ std::vector 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 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", diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 80542a8a..8ed484ac 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -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"( + + + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + )"; + 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"( + + + + + + + + + + + + + )"; + 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) { diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 1de30d3a..c1b0cfa1 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -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"( @@ -2875,9 +2875,9 @@ TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) { )"; std::array 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) {