From 46da1285af1fb9b3e0e9bc33e01568f06e8f39d0 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 1 Sep 2022 10:06:11 -0700 Subject: [PATCH] Cartesian end-effector control using site transmission. - Added `refsite` attribute to actuators with `site` transmission. - Such actuators now have a well defined length and can be used for e.g., Cartesian end-effector control. - Added example models and documentation describing the new feature. PiperOrigin-RevId: 471561218 Change-Id: I538f09af9600d5c53992c4a9fed325648aab4fa3 --- doc/XMLreference.rst | 43 +++++++++---- doc/changelog.rst | 12 +++- doc/computation.rst | 37 +++++++---- src/engine/engine_core_smooth.c | 89 ++++++++++++++++++++++---- src/user/user_objects.cc | 12 +++- src/user/user_objects.h | 1 + src/xml/xml_native_reader.cc | 36 ++++++----- src/xml/xml_native_writer.cc | 1 + test/engine/engine_core_smooth_test.cc | 46 +++++++++++++ test/engine/testdata/refsite.xml | 61 ++++++++++++++++++ 10 files changed, 283 insertions(+), 55 deletions(-) create mode 100644 test/engine/testdata/refsite.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index d62253c9..002bb425 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4302,25 +4302,42 @@ 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 gear. For + 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 position. 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). + 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). :at:`jointinparent`: :at-val:`string, optional` Identical to joint, except that for ball and free joints, the 3d rotation axis given by gear is defined in the parent frame (which is the world frame for free joints) rather than the child frame. :at:`site`: :at-val:`string, optional` - This transmission can apply force and torque at a site. The gear vector defines a 3d translation axis followed by a 3d - rotation axis. Both are defined in the site's frame. This can be used to model jets and propellers. The effect is - similar to actuating a free joint, and the actuator length is again defined as zero. One difference from the joint - and jointinparent transmissions above is that here the actuator operates on a site rather than a joint, but this - difference disappears when the site is defined at the frame origin of the free-floating body. The other difference is - that for site transmissions both the translation and rotation axes are defined in local coordinates. In contrast, - translation is global and rotation is local for joint, and both translation and rotation are global for - jointinparent. + This transmission can apply force and torque at a site. The gear vector defines a 3d translation axis followed by a + 3d rotation axis. Both are defined in the site's frame. This can be used to model jets and propellers. The effect is + similar to actuating a free joint, and the actuator length is defined as zero unless a :at:`refsite` is defined (see + below). One difference from the :at:`joint` and :at:`jointinparent` transmissions above is that here the actuator + operates on a site rather than a joint, but this difference disappears when the site is defined at the frame origin + of the free-floating body. The other difference is that for site transmissions both the translation and rotation axes + are defined in local coordinates. In contrast, translation is global and rotation is local for :at:`joint`, and both + translation and rotation are global for :at:`jointinparent`. + +.. youtube:: s-0JHanqV1A + :align: right + :height: 150px + +:at:`refsite`: :at-val:`string, optional` + 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. :at:`body`: :at-val:`string, optional` This transmission can apply linear forces at contact points in the direction of the contact normal. The set of contacts is all those belonging to the specified :at:`body`. This can be used to model natural active adhesion diff --git a/doc/changelog.rst b/doc/changelog.rst index 90e4d246..844e5a82 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -21,8 +21,16 @@ General - Increased ``mjNEQDATA``, the row length of equality constraint parameters in ``mjModel.eq_data``, from 7 to 11. - Added visualisation of anchor points for both :el:`connect` and :el:`weld` constraints (activated by the 'N' key in ``simulate``). -- Added `example model `_ showing different +- Added `weld.xml `_ showing different uses of new weld attributes. + + .. youtube:: s-0JHanqV1A + :align: right + :height: 150px + +- Cartesian 6D end-effector control is now possible by adding a reference site to actuators with :at:`site` + transmission. See description of new :at:`refsite` attribute in the :ref:`actuator` documentation and + `refsite.xml `_ example model. - Joint and tendon ``limited`` attribute and actuator ``ctrllimited``, ``forcelimited`` and ``actlimited`` attributes now default to ``auto`` rather than ``false``. Limits are automatically set to ``true`` if the corresponding range *is defined* and ``false`` otherwise. @@ -49,7 +57,7 @@ General - Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes. - ``simulate`` now supports taking a screenshot via a button in the File section or via ``Ctrl-P``. - Improvements to time synchronisation in `simulate`, in particular report actual real-time factor if different from - requested factor. + requested factor (if e.g., the timestep is so small that simulation cannot keep up with real-time). - Added a disable flag for sensors. - :ref:`mju_mulQuat` and :ref:`mju_mulQuatAxis` support in place computation. For example |br| ``mju_mulQuat(a, a, b);`` sets the quaternion ``a`` equal to the product of ``a`` and ``b``. diff --git a/doc/computation.rst b/doc/computation.rst index 82421373..b6acd9e7 100644 --- a/doc/computation.rst +++ b/doc/computation.rst @@ -255,14 +255,20 @@ These three components of an actuator - transmission, activation dynamics, and f actuator works. The user can set them independently for maximum flexibility, or use :ref:`Actuator shortcuts ` which instantiate common actuator types. -Transmission +.. _geTransmission: - Each actuator has a scalar length :math:`l_i(q)` defined by the type of transmission and its parameters. The gradient - :math:`\nabla l_i` is an :math:`n_V`-dimensional vector of moment arms. It determines the mapping from scalar - actuator force to joint force. The transmission properties are determined by the MuJoCo object to which the actuator - is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, :at:`jointinparent`, - :at:`slider-crank`, :at:`site`, and :at:`body`. The :at:`joint` and :at:`tendon` transmission types act as expected - mechanically and correspond to the actuator applying forces or torques to the target object. +Transmission +~~~~~~~~~~~~ + +Each actuator has a scalar length :math:`l_i(q)` defined by the type of transmission and its parameters. The gradient +:math:`\nabla l_i` is an :math:`n_V`-dimensional vector of moment arms. It determines the mapping from scalar +actuator force to joint force. The transmission properties are determined by the MuJoCo object to which the actuator +is attached; the possible attachment object types are :at:`joint`, :at:`tendon`, :at:`jointinparent`, +:at:`slider-crank`, :at:`site`, and :at:`body`. + + The :at:`joint` and :at:`tendon` transmission types act as expected and correspond to the actuator applying forces or + torques to the target object. Ball joints are special, see the :at:`joint` documentation in :ref:`actuator` + reference for more details. The :at:`jointinparent` transmission is unique to ball and free joint and asserts that rotation should be measured in the parent rather than child frame. @@ -273,11 +279,18 @@ Transmission also be modeled explicitly by creating MuJoCo bodies and coupling them with equality constraints to the rest of the system, but that would be less efficient. - :at:`site` and :at:`body` are degenerate transmission targets, as their length :math:`l_i(q)` is always 0. - They can therefore not be used to maintain a desired length value, as with a position actuator. Site - transmissions correspond to applying a Cartsian force/torque at the site, while :el:`body` transmissions correspond - to applying forces at contact points belonging to a body. For more information about adhesion, see the - :ref:`adhesion` shorcut documentation. + :at:`site` transmission (without a :at:`refsite`, see below) and :at:`body` transmission targets have a fixed zero + length :math:`l_i(q) = 0`. They can therefore not be used to maintain a desired length, but can be used to apply + forces. Site transmissions correspond to applying a Cartsian force/torque at the site, and are useful for modeling + jets and propellors. :el:`body` transmissions correspond to applying forces at contact points belonging to a body, in + order to model vacuum grippers and biomechanical adhesive appendages. For more information about adhesion, see the + :ref:`adhesion` actuator documentation. + + If a :at:`site` transmission target is defined with the optional :at:`refsite` attribute, forces and torques are + applied in the frame of the reference site rather than the the site's own frame. If a reference site is defined then + the length of the actuator is nonzero and corresponds to the pose difference of the two sites. This length can then + be controlled with a :el:`position` actuator, enabling Cartesian end-effector control. See the :at:`refsite` + documentation in :ref:`actuator` reference for more details. Activation dynamics Some actuators such as pneumatic and hydraulic cylinders as well as biological muscles have an internal state called diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ad7a3667..3acd5a51 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -614,6 +614,7 @@ void mj_transmission(const mjModel* m, mjData* d) { mjtNum wrench[6], gearAxis[3]; mjtNum *jac, *jacA, *jacS; mjtNum *length = d->actuator_length, *moment = d->actuator_moment, *gear; + mjtNum *jacref = NULL, *moment_tmp = NULL; // required for site actuators mjMARKSTACK; if (!nu) { @@ -763,7 +764,7 @@ void mj_transmission(const mjModel* m, mjData* d) { case mjTRN_TENDON: // tendon length[i] = d->ten_length[id]*gear[0]; - // moment: dense or sparse + // moment: sparse or dense if (mj_isSparse(m)) { int end = d->ten_J_rowadr[id] + d->ten_J_rownnz[id]; for (int j=d->ten_J_rowadr[id]; jsite_xmat+9*id, gear, 3, 3); // translation - mju_mulMatVec(wrench+3, d->site_xmat+9*id, gear+3, 3, 3); // rotation + // reference site undefined + if (m->actuator_trnid[2*i+1] == -1) { + // cannot compute meaningful length, set to 0 + length[i] = 0; + + // wrench: gear expressed in global frame + mju_rotVecMat(wrench, gear, d->site_xmat+9*id); // translation + mju_rotVecMat(wrench+3, gear+3, d->site_xmat+9*id); // rotation + + // moment: global Jacobian projected on wrench + mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation + mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation + mju_addTo(moment+i*nv, jac, nv); // add the two + } + + // reference site defined + else { + int refid = m->actuator_trnid[2*i+1]; + if (!jacref) jacref = mj_stackAlloc(d, 3*nv); + + // clear length + length[i] = 0; + + // translational transmission + if (!mju_isZero(gear, 3)) { + // vec: site position in reference site frame + mju_sub3(vec, d->site_xpos+3*id, d->site_xpos+3*refid); + mju_rotVecMatT(vec, vec, d->site_xmat+9*refid); + + // length: dot product with gear + length[i] += mju_dot3(vec, gear); + + // jacref: global Jacobian of reference site + mj_jacSite(m, d, jacref, NULL, refid); + + // subtract jacref from jac + mju_subFrom(jac, jacref, 3*nv); + + // wrench: translational gear expressed in global frame + mju_rotVecMat(wrench, gear, d->site_xmat+9*refid); + + // moment: global Jacobian projected on wrench + mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); + } + + // rotational transmission + if (!mju_isZero(gear+3, 3)) { + mjtNum refquat[4]; + + // get site and refsite quats from parent bodies (avoiding mju_mat2Quat) + mju_mulQuat(quat, m->site_quat+4*id, d->xquat+4*m->site_bodyid[id]); + mju_mulQuat(refquat, m->site_quat+4*refid, d->xquat+4*m->site_bodyid[refid]); + + // convert difference to expmap (axis-angle) + mju_subQuat(vec, quat, refquat); + + // add length: dot product with gear + length[i] += mju_dot3(vec, gear+3); + + // jacref: global rotational Jacobian of reference site + mj_jacSite(m, d, NULL, jacref, refid); + + // subtract jacref from jacS + mju_subFrom(jacS, jacref, 3*nv); + + // wrench: rotational gear expressed in global frame + mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid); + + // moment_tmp: global Jacobian projected on wrench, add to moment + if (!moment_tmp) moment_tmp = mj_stackAlloc(d, nv); + mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv); + mju_addTo(moment+i*nv, moment_tmp, nv); + } + } - // moment: global Jacobian projected on wrench - mju_mulMatTVec(moment+i*nv, jac, wrench, 3, nv); // translation - mju_mulMatTVec(jac, jacS, wrench+3, 3, nv); // rotation - mju_addTo(moment+i*nv, jac, nv); // add the two break; case mjTRN_BODY: // body (adhesive contacts) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 4c75dd78..6ca810d9 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3358,6 +3358,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { cranklength = 0; target.clear(); slidersite.clear(); + refsite.clear(); userdata.clear(); // clear private variables @@ -3499,7 +3500,16 @@ void mjCActuator::Compile(void) { break; case mjTRN_SITE: - // get site + // get refsite, copy into trnid[1] + if (!refsite.empty()) { + ptarget = model->FindObject(mjOBJ_SITE, refsite); + if (!ptarget) { + throw mjCError(this, "reference site '%s' not found for actuator %d", refsite.c_str(), id); + } + trnid[1] = ptarget->id; + } + + // proceed with regular site target ptarget = model->FindObject(mjOBJ_SITE, target); break; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d2202294..6407ab64 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -863,6 +863,7 @@ class mjCActuator : public mjCBase { std::vector userdata; // user data std::string target; // transmission target name std::string slidersite; // site defining cylinder, for slider-crank only + std::string refsite; // reference site, for site transmission only private: mjCActuator(mjCModel* = 0, mjCDef* = 0);// constructor diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 8dad9885..e3f35797 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -265,40 +265,40 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"actuator", "*", "0"}, {"<"}, - {"general", "*", "26", "name", "class", "group", + {"general", "*", "27", "name", "class", "group", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "body", - "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, - {"motor", "*", "17", "name", "class", "group", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", + "body", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm"}, + {"motor", "*", "18", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site"}, - {"position", "*", "18", "name", "class", "group", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"}, + {"position", "*", "19", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp"}, - {"velocity", "*", "18", "name", "class", "group", + {"velocity", "*", "19", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"intvelocity", "*", "19", "name", "class", "group", + {"intvelocity", "*", "20", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp"}, - {"damper", "*", "17", "name", "class", "group", + {"damper", "*", "18", "name", "class", "group", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"cylinder", "*", "21", "name", "class", "group", + {"cylinder", "*", "22", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", - "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "timeconst", "area", "diameter", "bias"}, {"muscle", "*", "25", "name", "class", "group", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", @@ -1439,6 +1439,12 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) { throw mjXError(elem, "cranklength and slidersite can only be used in slidercrank transmission"); } + // site-specific parameters (refsite) + int r3 = ReadAttrTxt(elem, "refsite", pact->refsite); + if (r3 && pact->trntype!=mjTRN_SITE && pact->trntype!=mjTRN_UNDEFINED) { + throw mjXError(elem, "refsite can only be used with site transmission"); + } + // get predefined type type = elem->Value(); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 4c6ed9d3..a3b8af5a 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -566,6 +566,7 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) { case mjTRN_SITE: WriteAttrTxt(elem, "site", pact->target); + WriteAttrTxt(elem, "refsite", pact->refsite); break; case mjTRN_BODY: diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index e8ea554b..573f4dd0 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -21,12 +21,16 @@ #include #include #include +#include "src/engine/engine_util_blas.h" +#include "src/engine/engine_util_spatial.h" #include "test/fixture.h" namespace mujoco { namespace { using ::testing::ElementsAre; +using ::testing::Pointwise; +using ::testing::DoubleNear; using CoreSmoothTest = MujocoTest; static std::vector GetVector(const mjtNum* array, int length) { @@ -195,6 +199,48 @@ TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) { TestConnect(kModelFilePath); } +// --------------------------- site actuators ---------------------------------- + +// Test Cartesian position control using site transmission with refsite +TEST_F(CoreSmoothTest, RefsiteBringsToPose) { + constexpr char kRefsitePath[] = "engine/testdata/refsite.xml"; + const std::string xml_path = GetTestDataFilePath(kRefsitePath); + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); + mjData* data = mj_makeData(model); + + // set pose target in ctrl (3 positions, 3 rotations) + mjtNum targetpos[] = {.01, .02, .03}; + mjtNum targetrot[] = {.1, .2, .3}; + mju_copy3(data->ctrl, targetpos); + mju_copy3(data->ctrl+3, targetrot); + + // step for 5 seconds + while (data->time < 5) { + mj_step(model, data); + } + + // get site IDs + int refsite_id = mj_name2id(model, mjOBJ_SITE, "reference"); + int site_id = mj_name2id(model, mjOBJ_SITE, "end_effector"); + + // check that position matches target to within 1e-5 length units + double tol_pos = 1e-5; + mjtNum relpos[3]; + mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id); + EXPECT_THAT(relpos, Pointwise(DoubleNear(tol_pos), targetpos)); + + // check that orientation matches target to within 1e-3 radians + double tol_rot = 1e-3; + mjtNum site_xquat[4], refsite_xquat[4], relrot[3]; + mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id); + mju_mat2Quat(site_xquat, data->site_xmat+9*site_id); + mju_subQuat(relrot, site_xquat, refsite_xquat); + EXPECT_THAT(relrot, Pointwise(DoubleNear(tol_rot), targetrot)); + + mj_deleteData(data); + mj_deleteModel(model); +} + // ------------------------ ellipsoid fluid model ------------------------------ diff --git a/test/engine/testdata/refsite.xml b/test/engine/testdata/refsite.xml new file mode 100644 index 00000000..acf2ea7b --- /dev/null +++ b/test/engine/testdata/refsite.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +