From abc0a39b7a9ff6f5e2f949ffe6e858b6e0bcbe83 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 22 Aug 2022 10:46:12 -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: 469228149 Change-Id: I836b0791f10fb624607a12ef3c687da991c21789 --- 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 | 109 ++++++---- 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, 250 insertions(+), 492 deletions(-) delete mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml delete mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml delete mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml delete mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml delete mode 100644 test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml delete mode 100644 test/engine/testdata/weld.xml diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 3c3ae5a2..05f323ce 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 | 11 | The maximal number of real-valued parameters used to define each equality constraint. | +| mjNEQDATA | 7 | 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 cd7ed78d..69616399 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -812,9 +812,7 @@ in the second column of the table have the following meaning: | | | +-------------------------+-------------------------+-------------------------+ | | | | | :at:`body2` | :at:`relpose` | :at:`active` | | | | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`solref` | :at:`solimp` | :at:`anchor` | | -| | | +-------------------------+-------------------------+-------------------------+ | -| | | | :at:`tfratio` | | | | +| | | | :at:`solref` | :at:`solimp` | | | | | | +-------------------------+-------------------------+-------------------------+ | +--------------------------+----+------------------------------------------------------------------------------------+ | |_2|:el:`joint` | \* | .. table:: | @@ -4022,16 +4020,6 @@ 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: @@ -4277,7 +4265,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 fcae0e1b..d8519200 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -15,13 +15,8 @@ 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 :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. +- 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. - 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. @@ -30,8 +25,6 @@ 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 8f02e7fb..1017b0b9 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -30,7 +30,7 @@ //---------------------------------- sizes --------------------------------------------------------- -#define mjNEQDATA 11 // number of eq_data fields +#define mjNEQDATA 7 // 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 4c84a955..35c8a6e4 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -398,16 +398,18 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_WELD: // fix relative position and orientation - // find global points + // find global points and their Jacobians for (int j=0; j<2; j++) { - mjtNum* anchor = data + 3*(1-j); - mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]); + // position offset for body1 only + if (j==0) { + mju_rotVecMat(pos[j], data, d->xmat + 9*id[j]); + } else { + mju_zero3(pos[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, @@ -417,14 +419,19 @@ 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); - // 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 + // 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 mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1) - mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*relpose + mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*data mju_copy3(cpos+3, quat2+1); // copy axis components - // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose + // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * data 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(efc_pos, 6); + *pos = mju_norm(d->efc_pos+i, 6); // mixes translation and rotation! } 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 ad7a3667..5b731904 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_com[6], cfrc[6], lfrc[6]; + mjtNum cfrc_body[6], tmp[6], tmp1[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(cfrc, d->xfrc_applied+6*i+3); - mju_copy3(cfrc+3, d->xfrc_applied+6*i); + mju_copy3(tmp1, d->xfrc_applied+6*i+3); + mju_copy3(tmp1+3, d->xfrc_applied+6*i); // map force from application point to com; both world-oriented - mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[i], d->xipos+3*i, 0); + mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[i], d->xipos+3*i, 0); // accumulate - mju_addTo(d->cfrc_ext+6*i, cfrc_com, 6); + mju_addTo(d->cfrc_ext+6*i, tmp, 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, lfrc); + mj_contactForce(m, d, i, tmp); - // cfrc = world-oriented torque:force vector (swap in the process) - mju_rotVecMatT(cfrc, lfrc+3, con->frame); - mju_rotVecMatT(cfrc+3, lfrc, con->frame); + // tmp1 = world-oriented torque:force vector (swap in the process) + mju_rotVecMatT(tmp1, tmp+3, con->frame); + mju_rotVecMatT(tmp1+3, tmp, con->frame); // body 1 int k; if ((k = m->geom_bodyid[con->geom1])) { // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); + mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); // apply (opposite for body 1) - mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6); + mju_subFrom(d->cfrc_ext+6*k, tmp, 6); } // body 2 if ((k = m->geom_bodyid[con->geom2])) { // tmp = subtree CoM-based torque_force vector - mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); + mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0); // apply - mju_addTo(d->cfrc_ext+6*k, cfrc_com, 6); + mju_addTo(d->cfrc_ext+6*k, tmp, 6); } } @@ -1883,41 +1883,73 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { int k; switch (m->eq_type[id]) { case mjEQ_CONNECT: - case mjEQ_WELD: - // 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 - } + // 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 point on body1: local -> global - mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_WELD), 0, k, 0); + // 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(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); + 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, cfrc_com, 6); + mju_addTo(d->cfrc_ext+6*k, tmp, 6); } // body 2 if ((k = m->eq_obj2id[id])) { - // transform point on body2: local -> global - mj_local2Global(d, pos, 0, eq_data + 3*(m->eq_type[id]==mjEQ_CONNECT), 0, k, 0); + // 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(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); + mju_transformSpatial(tmp, tmp1, 1, d->subtree_com+3*m->body_rootid[k], pos, 0); // apply - mju_subFrom(d->cfrc_ext+6*k, cfrc_com, 6); + mju_subFrom(d->cfrc_ext+6*k, tmp, 6); } - // increment rows - i += m->eq_type[id]==mjEQ_WELD ? 6 : 3; + // 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); + + // 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); + + // 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])) { + // weld force on body2 is always applied at body root + mju_copy3(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 6 rows of weld + i += 6; break; case mjEQ_JOINT: @@ -1935,23 +1967,22 @@ 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(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); + 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); // cfrc_body = cinert * cacc + cvel x (cinert * cvel) mju_mulInertVec(cfrc_body, d->cinert+10*i, d->cacc+6*i); - 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); + 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); // 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 882fc92b..4534ca48 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -185,25 +185,22 @@ 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+6] || - m->eq_data[mjNEQDATA*i+7] || - m->eq_data[mjNEQDATA*i+8] || - m->eq_data[mjNEQDATA*i+9]) { + 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]) { // normalize quaternion just in case - mju_normalize4(m->eq_data+mjNEQDATA*i+6); + mju_normalize4(m->eq_data+mjNEQDATA*i+3); continue; } - // anchor position is in body2 local frame - mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0); + // 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); - // 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 + // data[3-6] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame mju_negQuat(quat, d->xquat+4*id1); - mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2); + mju_mulQuat(m->eq_data+mjNEQDATA*i+3, quat, d->xquat+4*id2); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 11eea9a9..61bfd9b6 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1606,31 +1606,33 @@ 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 or weld + // connect for (int i=0; ineq; i++) { - if (m->eq_active[i] && (m->eq_type[i]==mjEQ_CONNECT || m->eq_type[i]==mjEQ_WELD)) { + if (m->eq_active[i] && m->eq_type[i]==mjEQ_CONNECT) { // compute endpoints in global coordinates - 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); + int j = m->eq_obj1id[i]; + int k = m->eq_obj2id[i]; + mju_rotVecMat(vec, m->eq_data+mjNEQDATA*i, d->xmat+9*j); mju_addTo3(vec, d->xpos+3*j); - mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i]==mjEQ_CONNECT), d->xmat+9*k); + mju_rotVecMat(end, m->eq_data+mjNEQDATA*i+3, 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]); - START - mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect); + f2f(thisgeom->rgba, m->vis.rgba.constraint, 4); + + // label flag 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 de2f9e7c..e8a068b4 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2884,7 +2884,6 @@ 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; @@ -2972,11 +2971,6 @@ 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 1013d923..d61c47c6 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 51746ecf..f5eda65c 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", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor", - "active", "solref", "solimp", "tfratio"}, + {"weld", "*", "8", "name", "class", "body1", "body2", "relpose", + "active", "solref", "solimp"}, {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef", "active", "solref", "solimp"}, {"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef", @@ -1310,11 +1310,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) { case mjEQ_WELD: ReadAttrTxt(elem, "body1", pequality->name1, true); ReadAttrTxt(elem, "body2", pequality->name2); - 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); - } + ReadAttr(elem, "relpose", mjNEQDATA, pequality->data, text); break; case mjEQ_JOINT: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 3fee6a93..e22280b6 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -456,9 +456,7 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) { case mjEQ_WELD: WriteAttrTxt(elem, "body1", peq->name1); WriteAttrTxt(elem, "body2", peq->name2); - WriteAttr(elem, "anchor", 3, peq->data); - WriteAttr(elem, "tfratio", 1, peq->data+10); - WriteAttr(elem, "relpose", 7, peq->data+3); + WriteAttr(elem, "relpose", 7, peq->data); break; case mjEQ_JOINT: diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index e8ea554b..97166af3 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -56,9 +56,10 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) { // --------------------------- connect constraint ------------------------------ -// 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); +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); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); @@ -74,46 +75,88 @@ void TestConnect(const char* const filepath) { } -TEST_F(CoreSmoothTest, RnePostConnectForceSlide) { - constexpr char kModelFilePath[] = - "engine/testdata/core_smooth/rne_post/connect/force_slide.xml"; - TestConnect(kModelFilePath); -} - - TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) { - constexpr char kModelFilePath[] = + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml"; - TestConnect(kModelFilePath); + 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); } TEST_F(CoreSmoothTest, RnePostConnectForceFree) { - constexpr char kModelFilePath[] = + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/connect/force_free.xml"; - TestConnect(kModelFilePath); + 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); } TEST_F(CoreSmoothTest, RnePostConnectTorque) { - constexpr char kModelFilePath[] = + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/connect/torque_free.xml"; - TestConnect(kModelFilePath); + 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); } TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) { - constexpr char kModelFilePath[] = + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml"; - TestConnect(kModelFilePath); + 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); } // --------------------------- weld constraint --------------------------------- -// 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); +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); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0); mjData* data = mj_makeData(model); @@ -134,65 +177,75 @@ void TestWeld(const char* const filepath) { } -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[] = +TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotatoed) { + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml"; - TestWeld(kModelFilePath); + 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); } TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) { - constexpr char kModelFilePath[] = + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml"; - TestWeld(kModelFilePath); + 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); } TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) { - constexpr char kModelFilePath[] = + static const char* const kModelFilePath = "engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml"; - 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); + 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); } 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 deleted file mode 100644 index 4ae08e38..00000000 --- a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - 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 deleted file mode 100644 index e034e5e1..00000000 --- a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - 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 deleted file mode 100644 index 9c8df21d..00000000 --- a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide_rotated.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - 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 deleted file mode 100644 index b75cf724..00000000 --- a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_multiple_constraints.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - - - 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 deleted file mode 100644 index 43d0e97c..00000000 --- a/test/engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - diff --git a/test/engine/testdata/weld.xml b/test/engine/testdata/weld.xml deleted file mode 100644 index 6f346be3..00000000 --- a/test/engine/testdata/weld.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/unity/Runtime/Bindings/MujocoBindings.cs b/unity/Runtime/Bindings/MujocoBindings.cs index 967744e5..c395aa33 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 = 11; +public const int mjNEQDATA = 7; public const int mjNDYN = 10; public const int mjNGAIN = 10; public const int mjNBIAS = 10;