From d26501c0f370e7e25e415a94ab2228141ab0d722 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 25 Aug 2022 10:21:26 -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: 470021901 Change-Id: I3faba0e5dcab2e29ee81096f5ff71ff66d99758c --- doc/APIreference.rst | 2 +- doc/XMLreference.rst | 14 +- doc/changelog.rst | 12 +- 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(+), 248 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 c9c397cf..e504cded 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -3163,7 +3163,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 d6261011..d62253c9 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:`torquescale` | | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:el:`joint` | \* | .. table:: | @@ -4023,6 +4025,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:`torquescale`: :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:`torquescale` can be + interpreted as the diameter of this glue patch. .. _equality-joint: diff --git a/doc/changelog.rst b/doc/changelog.rst index 109ee840..ab0efdee 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,8 +15,14 @@ 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:`torquescale` and :at:`anchor` attributes to :el:`weld` constraints. :at:`torquescale` 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. +- 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 + 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 +31,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 baf65736..b44e7817 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..59b2ba23 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 torquescale + 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 c2fff3d8..4c75dd78 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 82fcd965..d2202294 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -753,8 +753,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 1947c0fc..8dad9885 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", "torquescale"}, {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef", "active", "solref", "solimp"}, {"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef", @@ -1311,7 +1311,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, "torquescale", 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 59728467..4c6ed9d3 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, "torquescale", 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..862b40b5 --- /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..97691166 --- /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..ec51731b --- /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..be885ae8 --- /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..cb39f350 --- /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..0d2860c3 --- /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;