From 8ca5887c20d47f7534c9c22b3e363b64e4e8e36b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 22 Aug 2022 08:18:43 -0700 Subject: [PATCH] Add two new attributes to weld constraints: - `anchor` determines the point of wrench application, in the frame of body2. - `tfratio` scales applied torques relative to applied forces. - Add visualisation of both anchor points to both weld and connect constraints. - Add a test model showing how the new weld parameters behave. PiperOrigin-RevId: 469190483 Change-Id: I20f6da85b09cd2c4c0b5d29eb8003eabb524e55e --- doc/APIreference.rst | 2 +- doc/XMLreference.rst | 16 +- doc/changelog.rst | 11 +- include/mujoco/mjmodel.h | 2 +- src/engine/engine_core_constraint.c | 45 +++-- src/engine/engine_core_smooth.c | 107 ++++------ src/engine/engine_setconst.c | 23 ++- src/engine/engine_vis_visualize.c | 28 ++- src/user/user_objects.cc | 6 + src/user/user_objects.h | 4 +- src/xml/xml_native_reader.cc | 10 +- src/xml/xml_native_writer.cc | 4 +- test/engine/engine_core_smooth_test.cc | 191 +++++++----------- .../rne_post/weld/tfratio0_force_free.xml | 43 ++++ .../rne_post/weld/tfratio0_force_slide.xml | 43 ++++ .../weld/tfratio0_force_slide_rotated.xml | 43 ++++ .../weld/tfratio0_multiple_constraints.xml | 59 ++++++ .../rne_post/weld/tfratio0_torque_free.xml | 43 ++++ test/engine/testdata/weld.xml | 58 ++++++ unity/Runtime/Bindings/MujocoBindings.cs | 2 +- 20 files changed, 491 insertions(+), 249 deletions(-) create mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml create mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml create mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml create mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml create mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml create mode 100644 test/engine/testdata/weld.xml diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 05f323ce..3c3ae5a2 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -3162,7 +3162,7 @@ Numeric constants +------------------+--------+----------------------------------------------------------------------------------------+ | mjMAXVFSNAME | 100 | The maximal number of characters in the name of each file in the virtual file system. | +------------------+--------+----------------------------------------------------------------------------------------+ -| mjNEQDATA | 7 | The maximal number of real-valued parameters used to define each equality constraint. | +| mjNEQDATA | 11 | The maximal number of real-valued parameters used to define each equality constraint. | | | | Determines the size of mjModel.eq_data. This and the next five constants correspond to | | | | array sizes which we have not fully settled. There may be reasons to increase them in | | | | the future, so as to accommodate extra parameters needed for more elaborate | diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 69616399..cd7ed78d 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -812,7 +812,9 @@ in the second column of the table have the following meaning: | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`body2` | :at:`relpose` | :at:`active` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`solref` | :at:`solimp` | | | +| | | | :at:`solref` | :at:`solimp` | :at:`anchor` | | +| | | +-------------------------+-------------------------+-------------------------+ | +| | | | :at:`tfratio` | | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:el:`joint` | \* | .. table:: | @@ -4020,6 +4022,16 @@ of the other body, without any joint elements in the child body. body1. If the quaternion part (i.e., last 4 components of the vector) are all zeros, as in the default setting, this attribute is ignored and the relative pose is the one corresponding to the model reference pose in qpos0. The unusual default is because all equality constraint types share the same default for their numeric parameters. +:at:`anchor`: :at-val:`real(3), "0 0 0"` + Coordinates of the weld point relative to body2. If :at:`relpose` is not specified, the meaning of + this parameter is the same as for connect constraints, except that is relative to body2. If :at:`relpose` is + specified, body1 will use the pose to compute its anchor point. +:at:`tfratio`: :at-val:`real, "1"` + Relative torque-to-force ratio. This ratio is used by the weld to scale how much it "cares" about rotational + displacements vs. translational displacements. Setting this value to 0 makes the :el:`weld` behave like a + :el:`connect` constraint. Note that this value has units of length and can therefore be interpreted as follows. + Imagining that the weld is implemented by a patch of glue sticking the two bodies together, :at:`tfratio` can be + interpreted as the diameter of this glue patch. .. _equality-joint: @@ -4265,7 +4277,7 @@ specify them independently. If true, the internal state (activation) associated with this actuator is automatically clamped to :at:`actrange` at runtime. If false, activation clamping is disabled. If auto, activation clamping will automatically be set to true if :at:`actrange` is defined without explicitly setting this attribute to "true". See the :ref:`Activation clamping ` - section for more details. + section for more details. :at:`ctrlrange`: :at-val:`real(2), "0 0"` Range for clamping the control input. The compiler expects the first value to be smaller than the second value. :at:`forcerange`: :at-val:`real(2), "0 0"` diff --git a/doc/changelog.rst b/doc/changelog.rst index d8519200..fcae0e1b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,8 +15,13 @@ General - Added :ref:`adhesion actuators` mimicking vacuum grippers and adhesive biomechanical appendages. - Added related `example model `_ and video: - Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree. -- Added moment of inertia computation for concave meshes. This is currently activated by setting the compiler flag - :at:`exactmeshinertia` to ``true`` (defaults to ``false``). This default may change in the future. +- Added :at:`tfratio` and :at:`anchor` attributes to :el:`weld` constraints. :at:`tfratio` sets the torque-to-force + ratio exerted by the constraint, :at:`anchor` sets the point at which the weld wrench is applied. See + :ref:`weld ` for more details. +- 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 + uses of new weld attributes. - 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. @@ -25,6 +30,8 @@ General This is a minor breaking change. In models where a range was defined but :at:`limited` was unspecified, the target element will now be limited. Explicitly set limited to ``false`` to revert to the previous behavior. +- Added moment of inertia computation for all well-formed meshes. This option is activated by setting the compiler + flag :at:`exactmeshinertia` to ``true`` (defaults to ``false``). This default may change in the future. - Added parameter :at:`shellinertia` to :at:`geom`, for locating the inferred inertia on the boundary (shell). Currently only meshes are supported. - For meshes from which volumetric inertia is inferred, raise error if the orientation of mesh faces is not consistent. diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 1017b0b9..8f02e7fb 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -30,7 +30,7 @@ //---------------------------------- sizes --------------------------------------------------------- -#define mjNEQDATA 7 // number of eq_data fields +#define mjNEQDATA 11 // number of eq_data fields #define mjNDYN 10 // number of actuator dynamics parameters #define mjNGAIN 10 // number of actuator gain parameters #define mjNBIAS 10 // number of actuator bias parameters diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 35c8a6e4..4c84a955 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -398,18 +398,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_WELD: // fix relative position and orientation - // find global points and their Jacobians + // find global points for (int j=0; j<2; j++) { - // position offset for body1 only - if (j==0) { - mju_rotVecMat(pos[j], data, d->xmat + 9*id[j]); - } else { - mju_zero3(pos[j]); - } - + mjtNum* anchor = data + 3*(1-j); + mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]); mju_addTo3(pos[j], d->xpos + 3*id[j]); } + // compute position error + mju_sub3(cpos, pos[0], pos[1]); + // compute error Jacobian (opposite of contact: 0 - 1) NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0], jac[1], jac[0], jacdif, @@ -419,19 +417,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy(jac[0], jacdif, 3*NV); mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV); - // get desired position offset in global frame - mju_rotVecMat(cpos, data, d->xmat+9*id[0]); - - // compute position error: p0 - p1 - data - mju_sub3(cpos, pos[0], pos[1]); - - // compute orientation error: neg(q1) * q0 * data (axis components only) - mju_mulQuat(quat, d->xquat+4*id[0], data+3); // quat = q0*data + // compute orientation error: neg(q1) * q0 * relpose (axis components only) + mjtNum* relpose = data+6; + mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1) - mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*data + mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*relpose mju_copy3(cpos+3, quat2+1); // copy axis components - // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * data + // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose for (int j=0; jeq_type[id]==mjEQ_WELD) { + mjtNum rotlinratio = m->eq_data[mjNEQDATA*id+10]; + mjtNum efc_pos[6]; + + // copy translational residual + mju_copy3(efc_pos, d->efc_pos+i); + + // multiply orientations by tfratio + mju_scl3(efc_pos+3, d->efc_pos+i+3, rotlinratio); *dim = 6; - *pos = mju_norm(d->efc_pos+i, 6); // mixes translation and rotation! + *pos = mju_norm(efc_pos, 6); } else if (m->eq_type[id]==mjEQ_CONNECT) { *dim = 3; *pos = mju_norm(d->efc_pos+i, 3); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 5b731904..ad7a3667 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1814,7 +1814,7 @@ void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) { // RNE with complete data: compute cacc, cfrc_ext, cfrc_int void mj_rnePostConstraint(const mjModel* m, mjData* d) { int nbody=m->nbody; - mjtNum cfrc_body[6], tmp[6], tmp1[6]; + mjtNum cfrc_com[6], cfrc[6], lfrc[6]; mjContact* con; // clear cacc, set world acceleration to -gravity @@ -1828,14 +1828,14 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { for (int i=1; ixfrc_applied+6*i, 6)) { // rearrange as torque:force - mju_copy3(tmp1, d->xfrc_applied+6*i+3); - mju_copy3(tmp1+3, d->xfrc_applied+6*i); + mju_copy3(cfrc, d->xfrc_applied+6*i+3); + mju_copy3(cfrc+3, d->xfrc_applied+6*i); // map force from application point to com; both world-oriented - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[i], d->xipos+3*i, 0); + mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[i], d->xipos+3*i, 0); // accumulate - mju_addTo(d->cfrc_ext+6*i, tmp, 6); + mju_addTo(d->cfrc_ext+6*i, cfrc_com, 6); } // cfrc_ext += contacts @@ -1845,29 +1845,29 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { con = d->contact+i; // tmp = contact-local force:torque vector - mj_contactForce(m, d, i, tmp); + mj_contactForce(m, d, i, lfrc); - // tmp1 = world-oriented torque:force vector (swap in the process) - mju_rotVecMatT(tmp1, tmp+3, con->frame); - mju_rotVecMatT(tmp1+3, tmp, con->frame); + // cfrc = world-oriented torque:force vector (swap in the process) + mju_rotVecMatT(cfrc, lfrc+3, con->frame); + mju_rotVecMatT(cfrc+3, lfrc, con->frame); // body 1 int k; if ((k = m->geom_bodyid[con->geom1])) { // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); + mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); // apply (opposite for body 1) - mju_subFrom(d->cfrc_ext+6*k, tmp, 6); + mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6); } // body 2 if ((k = m->geom_bodyid[con->geom2])) { // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); + mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); // apply - mju_addTo(d->cfrc_ext+6*k, tmp, 6); + mju_addTo(d->cfrc_ext+6*k, cfrc_com, 6); } } @@ -1883,73 +1883,41 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { int k; switch (m->eq_type[id]) { case mjEQ_CONNECT: - // tmp1 = world-oriented torque:force vector - mju_zero3(tmp1); // no torque from connect - mju_copy3(tmp1 + 3, d->efc_force + i); - - // body 1 - if ((k = m->eq_obj1id[id])) { - // transform connect point on body1: local -> global - mju_rotVecMat(pos, eq_data, d->xmat+9*k); - mju_addTo3(pos, d->xpos+3*k); - - // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); - - // apply (opposite for body 1) - mju_addTo(d->cfrc_ext+6*k, tmp, 6); - } - - // body 2 - if ((k = m->eq_obj2id[id])) { - // transform connect point on body2: local -> global - mju_rotVecMat(pos, eq_data + 3, d->xmat+9*k); - mju_addTo3(pos, d->xpos+3*k); - - // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); - - // apply - mju_subFrom(d->cfrc_ext+6*k, tmp, 6); - } - - // increment 3 rows of connect - i += 3; - - break; - case mjEQ_WELD: - // tmp1 = world-oriented torque:force vector (efc is f:t, so swap) - mju_copy3(tmp1, d->efc_force + i + 3); - mju_copy3(tmp1 + 3, d->efc_force + i); + // cfrc = world-oriented torque:force vector + mju_copy3(cfrc + 3, d->efc_force + i); + if (m->eq_type[id]==mjEQ_WELD) { + mju_copy3(cfrc, d->efc_force + i + 3); + } else { + mju_zero3(cfrc); // no torque from connect + } // body 1 if ((k = m->eq_obj1id[id])) { - // transform weld point on body1: local -> global - mju_rotVecMat(pos, eq_data, d->xmat+9*k); - mju_addTo3(pos, d->xpos+3*k); + // transform point on body1: local -> global + mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_WELD), 0, k, 0); // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); + mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); // apply (opposite for body 1) - mju_addTo(d->cfrc_ext+6*k, tmp, 6); + mju_addTo(d->cfrc_ext+6*k, cfrc_com, 6); } // body 2 if ((k = m->eq_obj2id[id])) { - // weld force on body2 is always applied at body root - mju_copy3(pos, d->xpos+3*k); + // transform point on body2: local -> global + mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_CONNECT), 0, k, 0); // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); + mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); // apply - mju_subFrom(d->cfrc_ext+6*k, tmp, 6); + mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6); } - // increment 6 rows of weld - i += 6; + // increment rows + i += m->eq_type[id]==mjEQ_WELD ? 6 : 3; break; case mjEQ_JOINT: @@ -1967,22 +1935,23 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { } // forward pass over bodies: compute cacc, cfrc_int + mjtNum cacc[6], cfrc_body[6], cfrc_corr[6]; mju_zero(d->cfrc_int, 6); for (int i=1; inbody; i++) { // get body's first dof address int bda = m->body_dofadr[i]; // cacc = cacc_parent + cdofdot * qvel + cdof * qacc - mju_mulDofVec(tmp, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]); - mju_add(d->cacc+6*i, d->cacc+6*m->body_parentid[i], tmp, 6); - mju_mulDofVec(tmp, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]); - mju_addTo(d->cacc+6*i, tmp, 6); + mju_mulDofVec(cacc, d->cdof_dot+6*bda, d->qvel+bda, m->body_dofnum[i]); + mju_add(d->cacc+6*i, d->cacc+6*m->body_parentid[i], cacc, 6); + mju_mulDofVec(cacc, d->cdof+6*bda, d->qacc+bda, m->body_dofnum[i]); + mju_addTo(d->cacc+6*i, cacc, 6); // cfrc_body = cinert * cacc + cvel x (cinert * cvel) mju_mulInertVec(cfrc_body, d->cinert+10*i, d->cacc+6*i); - mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i); - mju_crossForce(tmp1, d->cvel+6*i, tmp); - mju_addTo(cfrc_body, tmp1, 6); + mju_mulInertVec(cfrc_corr, d->cinert+10*i, d->cvel+6*i); + mju_crossForce(cfrc, d->cvel+6*i, cfrc_corr); + mju_addTo(cfrc_body, cfrc, 6); // set cfrc_int = cfrc_body - cfrc_ext mju_sub(d->cfrc_int+6*i, cfrc_body, d->cfrc_ext+6*i, 6); diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 4534ca48..882fc92b 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -185,22 +185,25 @@ static void set0(mjModel* m, mjData* d) { // weld constraint else if (m->eq_type[i]==mjEQ_WELD) { // skip if user has set any quaternion data - if (m->eq_data[mjNEQDATA*i+3] || - m->eq_data[mjNEQDATA*i+4] || - m->eq_data[mjNEQDATA*i+5] || - m->eq_data[mjNEQDATA*i+6]) { + if (m->eq_data[mjNEQDATA*i+6] || + m->eq_data[mjNEQDATA*i+7] || + m->eq_data[mjNEQDATA*i+8] || + m->eq_data[mjNEQDATA*i+9]) { // normalize quaternion just in case - mju_normalize4(m->eq_data+mjNEQDATA*i+3); + mju_normalize4(m->eq_data+mjNEQDATA*i+6); continue; } - // data[0-2] = xpos2-xpos1 in body1 local frame - mju_sub3(pos, d->xpos+3*id2, d->xpos+3*id1); - mju_rotVecMatT(m->eq_data+mjNEQDATA*i, pos, d->xmat+9*id1); + // anchor position is in body2 local frame + mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0); - // data[3-6] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame + // data[3-5] = anchor position in body1 local frame + mju_subFrom3(pos, d->xpos+3*id1); + mju_rotVecMatT(m->eq_data+mjNEQDATA*i+3, pos, d->xmat+9*id1); + + // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame mju_negQuat(quat, d->xquat+4*id1); - mju_mulQuat(m->eq_data+mjNEQDATA*i+3, quat, d->xquat+4*id2); + mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 61bfd9b6..11eea9a9 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1606,33 +1606,31 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, objtype = mjOBJ_EQUALITY; category = mjCAT_DECOR; if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) { - // connect + // connect or weld for (int i=0; ineq; i++) { - if (m->eq_active[i] && m->eq_type[i]==mjEQ_CONNECT) { + if (m->eq_active[i] && (m->eq_type[i]==mjEQ_CONNECT || m->eq_type[i]==mjEQ_WELD)) { // compute endpoints in global coordinates - int j = m->eq_obj1id[i]; - int k = m->eq_obj2id[i]; - mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i, d->xmat+9*j); + int j = m->eq_obj1id[i], k = m->eq_obj2id[i]; + mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_WELD), d->xmat+9*j); mju_addTo3(vec, d->xpos+3*j); - mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3, d->xmat+9*k); + mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_CONNECT), d->xmat+9*k); mju_addTo3(end, d->xpos+3*k); - // connect endpoints - START - // construct geom sz[0] = scl * m->vis.scale.constraint; - mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0], - vec[0], vec[1], vec[2], - end[0], end[1], end[2]); - f2f(thisgeom->rgba, m->vis.rgba.constraint, 4); - - // label flag + START + mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect); if (vopt->label==mjLABEL_CONSTRAINT) { makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); } + FINISH + START + mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint); + if (vopt->label==mjLABEL_CONSTRAINT) { + makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label); + } FINISH } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e8a068b4..de2f9e7c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2884,6 +2884,7 @@ mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) { mjuu_zerovec(data, mjNEQDATA); data[1] = 1; + data[10] = 1; // torque:force ratio // clear internal variables obj1id = obj2id = -1; @@ -2971,6 +2972,11 @@ void mjCEquality::Compile(void) { if (type==mjEQ_CONNECT) { ((mjCBody*)px1)->MakeLocal(anchor, qdummy, data, qunit); mjuu_copyvec(data, anchor, 3); + } else if (type==mjEQ_WELD) { + if (px2) { + ((mjCBody*)px2)->MakeLocal(anchor, qdummy, data, qunit); + mjuu_copyvec(data, anchor, 3); + } } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d61c47c6..1013d923 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -752,8 +752,8 @@ class mjCEquality : public mjCBase { double data[mjNEQDATA]; // type-dependent data private: - mjCEquality(mjCModel* = 0, mjCDef* = 0);// constructor - void Compile(void); // compiler + mjCEquality(mjCModel* = 0, mjCDef* = 0); // constructor + void Compile(void); // compiler int obj1id; // id of object 1 int obj2id; // id of object 2 diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index f5eda65c..51746ecf 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -234,8 +234,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = { {"<"}, {"connect", "*", "8", "name", "class", "body1", "body2", "anchor", "active", "solref", "solimp"}, - {"weld", "*", "8", "name", "class", "body1", "body2", "relpose", - "active", "solref", "solimp"}, + {"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor", + "active", "solref", "solimp", "tfratio"}, {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef", "active", "solref", "solimp"}, {"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef", @@ -1310,7 +1310,11 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { case mjEQ_WELD: ReadAttrTxt(elem, "body1", pequality->name1, true); ReadAttrTxt(elem, "body2", pequality->name2); - ReadAttr(elem, "relpose", mjNEQDATA, pequality->data, text); + ReadAttr(elem, "relpose", 7, pequality->data+3, text); + ReadAttr(elem, "tfratio", 1, pequality->data+10, text); + if (!ReadAttr(elem, "anchor", 3, pequality->data, text)) { + mjuu_zerovec(pequality->data, 3); + } break; case mjEQ_JOINT: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index e22280b6..3fee6a93 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -456,7 +456,9 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) { case mjEQ_WELD: WriteAttrTxt(elem, "body1", peq->name1); WriteAttrTxt(elem, "body2", peq->name2); - WriteAttr(elem, "relpose", 7, peq->data); + WriteAttr(elem, "anchor", 3, peq->data); + WriteAttr(elem, "tfratio", 1, peq->data+10); + WriteAttr(elem, "relpose", 7, peq->data+3); break; case mjEQ_JOINT: diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 97166af3..e8ea554b 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -56,10 +56,9 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { // --------------------------- connect constraint ------------------------------ -TEST_F(CoreSmoothTest, RnePostConnectForceSlide) { - static const char* const kModelFilePath = - "engine/testdata/core_smooth/rne_post/connect/force_slide.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); +// test that bodies hanging on connects lead to expected force sensor readings +void TestConnect(const char* const filepath) { + const std::string xml_path = GetTestDataFilePath(filepath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); @@ -75,88 +74,46 @@ TEST_F(CoreSmoothTest, RnePostConnectForceSlide) { } +TEST_F(CoreSmoothTest, RnePostConnectForceSlide) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/connect/force_slide.xml"; + TestConnect(kModelFilePath); +} + + TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) { - static const char* const kModelFilePath = + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int i=0; i < 3; i++) { - EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6); - } - mj_deleteData(data); - mj_deleteModel(model); + TestConnect(kModelFilePath); } TEST_F(CoreSmoothTest, RnePostConnectForceFree) { - static const char* const kModelFilePath = + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/force_free.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int i=0; i < 3; i++) { - EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6); - } - mj_deleteData(data); - mj_deleteModel(model); + TestConnect(kModelFilePath); } TEST_F(CoreSmoothTest, RnePostConnectTorque) { - static const char* const kModelFilePath = + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/torque_free.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int i=0; i < 3; i++) { - EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6); - } - mj_deleteData(data); - mj_deleteModel(model); + TestConnect(kModelFilePath); } TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) { - static const char* const kModelFilePath = + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int i=0; i < 3; i++) { - EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6); - } - mj_deleteData(data); - mj_deleteModel(model); + TestConnect(kModelFilePath); } // --------------------------- weld constraint --------------------------------- -TEST_F(CoreSmoothTest, RnePostWeldForceFree) { - static const char* const kModelFilePath = - "engine/testdata/core_smooth/rne_post/weld/force_free.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); +// test that bodies attached with welds lead to expected force sensor readings +void TestWeld(const char* const filepath) { + const std::string xml_path = GetTestDataFilePath(filepath); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); @@ -177,75 +134,65 @@ TEST_F(CoreSmoothTest, RnePostWeldForceFree) { } -TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotatoed) { - static const char* const kModelFilePath = +TEST_F(CoreSmoothTest, RnePostWeldForceFree) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/weld/force_free.xml"; + TestWeld(kModelFilePath); +} + + +TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) { + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) { - for (int i=0; i < 3; i++) { - EXPECT_NEAR( - data->sensordata[model->sensor_adr[sensor_index] + i], - model->sensor_user[model->nuser_sensor*sensor_index + i], - 1e-6); - } - } - mj_deleteData(data); - mj_deleteModel(model); + TestWeld(kModelFilePath); } TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) { - static const char* const kModelFilePath = + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) { - for (int i=0; i < 3; i++) { - EXPECT_NEAR( - data->sensordata[model->sensor_adr[sensor_index] + i], - model->sensor_user[model->nuser_sensor*sensor_index + i], - 1e-6); - } - } - mj_deleteData(data); - mj_deleteModel(model); + TestWeld(kModelFilePath); } TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) { - static const char* const kModelFilePath = + constexpr char kModelFilePath[] = "engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml"; - const std::string xml_path = GetTestDataFilePath(kModelFilePath); - mjModel* model = - mj_loadXML(xml_path.c_str(), nullptr, 0, 0); - mjData* data = mj_makeData(model); - // settle physics: - for (int i=0; i < 1000; i++) { - mj_step(model, data); - } - for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) { - for (int i=0; i < 3; i++) { - EXPECT_NEAR( - data->sensordata[model->sensor_adr[sensor_index] + i], - model->sensor_user[model->nuser_sensor*sensor_index + i], - 1e-6); - } - } - mj_deleteData(data); - mj_deleteModel(model); + TestWeld(kModelFilePath); +} + + +TEST_F(CoreSmoothTest, WeldRatioForceFree) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml"; + TestConnect(kModelFilePath); +} + + +TEST_F(CoreSmoothTest, WeldRatioForceSlide) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml"; + TestConnect(kModelFilePath); +} + + +TEST_F(CoreSmoothTest, WeldRatioTorqueFree) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml"; + TestConnect(kModelFilePath); +} + + +TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml"; + TestConnect(kModelFilePath); +} + +TEST_F(CoreSmoothTest, WeldRatioMultipleConstraints) { + constexpr char kModelFilePath[] = + "engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml"; + TestConnect(kModelFilePath); } diff --git a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml new file mode 100644 index 00000000..4ae08e38 --- /dev/null +++ b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml @@ -0,0 +1,43 @@ + + + + + diff --git a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml new file mode 100644 index 00000000..e034e5e1 --- /dev/null +++ b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml @@ -0,0 +1,43 @@ + + + + + diff --git a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml new file mode 100644 index 00000000..9c8df21d --- /dev/null +++ b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml @@ -0,0 +1,43 @@ + + + + + diff --git a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml new file mode 100644 index 00000000..b75cf724 --- /dev/null +++ b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml @@ -0,0 +1,59 @@ + + + + + diff --git a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml new file mode 100644 index 00000000..43d0e97c --- /dev/null +++ b/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml @@ -0,0 +1,43 @@ + + + + + diff --git a/test/engine/testdata/weld.xml b/test/engine/testdata/weld.xml new file mode 100644 index 00000000..6f346be3 --- /dev/null +++ b/test/engine/testdata/weld.xml @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index c395aa33..967744e5 100644 --- a/unity/Runtime/Bindings/MujocoBindings.cs +++ b/unity/Runtime/Bindings/MujocoBindings.cs @@ -40,7 +40,7 @@ public const double mjMAXIMP = 0.9999; public const int mjMAXCONPAIR = 50; public const int mjMAXVFS = 2000; public const int mjMAXVFSNAME = 1000; -public const int mjNEQDATA = 7; +public const int mjNEQDATA = 11; public const int mjNDYN = 10; public const int mjNGAIN = 10; public const int mjNBIAS = 10;